Fix <ignition_frame_id> not working for GpuLidarSensor (#218)

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>
gazebosim · Apr 19, 2022 · bbdb620 · bbdb620
1 parent cb24d95
commit bbdb620
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc
@@ -250,7 +250,7 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now)
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   msgs::Set(msg.mutable_force(), measuredForce);
   msgs::Set(msg.mutable_torque(), measuredTorque);

diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc
@@ -256,6 +256,19 @@ bool GpuLidarSensor::Update(const std::chrono::steady_clock::duration &_now)
     // Set the time stamp
     *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() =
       msgs::Convert(_now);
+    // Set frame_id
+    for (auto i = 0;
+         i < this->dataPtr->pointMsg.mutable_header()->data_size();
+         ++i)
+    {
+      if (this->dataPtr->pointMsg.mutable_header()->data(i).key() == "frame_id"
+          && this->dataPtr->pointMsg.mutable_header()->data(i).value_size() > 0)
+      {
+        this->dataPtr->pointMsg.mutable_header()->mutable_data(i)->set_value(
+              0,
+              this->FrameId());
+      }
+    }
 
     this->dataPtr->FillPointCloudMsg(this->laserBuffer);
 

diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc
@@ -487,7 +487,7 @@ bool SegmentationCameraSensor::Update(
   *stamp = msgs::Convert(_now);
   auto frame = this->dataPtr->coloredMapMsg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   this->dataPtr->labelsMapMsg.CopyFrom(this->dataPtr->coloredMapMsg);