Conform to ros format for header field frame_id of sensor msgs (#195)

Signed-off-by: ahcorde <ahcorde@gmail.com>
gazebosim · Mar 1, 2022 · 1eb8a63 · 1eb8a63
1 parent 5d24d67
commit 1eb8a63
Show file tree

Hide file tree

Showing 15 changed files with 58 additions and 12 deletions.
diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh
@@ -138,6 +138,14 @@ namespace ignition
       /// \return Name of sensor.
       public: std::string Name() const;
 
+      /// \brief FrameId.
+      /// \return FrameId of sensor.
+      public: std::string FrameId() const;
+
+      /// \brief Set Frame ID of the sensor
+      /// \param[in] _frameId Frame ID of the sensor
+      public: void SetFrameId(const std::string &_frameId);
+
       /// \brief Get topic where sensor data is published.
       /// \return Topic sensor publishes data to
       public: std::string Topic() const;

diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc
@@ -159,7 +159,7 @@ bool AirPressureSensor::Update(const ignition::common::Time &_now)
   msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // This block of code comes from RotorS:
   // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp

diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc
@@ -147,7 +147,7 @@ bool AltimeterSensor::Update(const ignition::common::Time &_now)
   msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // Apply altimeter vertical position noise
   if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) !=

diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc
@@ -400,7 +400,7 @@ bool CameraSensor::Update(const ignition::common::Time &_now)
     msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
     auto frame = msg.mutable_header()->add_data();
     frame->set_key("frame_id");
-    frame->add_value(this->Name());
+    frame->add_value(this->FrameId());
     msg.set_data(data, this->dataPtr->camera->ImageMemorySize());
   }
 
@@ -606,7 +606,7 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf)
   // can populate it with arbitrary frames.
   auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data();
   infoFrame->set_key("frame_id");
-  infoFrame->add_value(this->Name());
+  infoFrame->add_value(this->FrameId());
 
   this->dataPtr->infoMsg.set_width(width);
   this->dataPtr->infoMsg.set_height(height);

diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc
@@ -543,7 +543,7 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now)
   msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   msg.set_data(this->dataPtr->depthBuffer,

diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc
@@ -216,7 +216,7 @@ bool ImuSensor::Update(const ignition::common::Time &_now)
   msg.set_entity_name(this->Name());
   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_orientation(), this->dataPtr->orientation);
   msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel);

diff --git a/src/Lidar.cc b/src/Lidar.cc
@@ -231,8 +231,11 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now)
   this->dataPtr->laserMsg.mutable_header()->clear_data();
   auto frame = this->dataPtr->laserMsg.mutable_header()->add_data();
   frame->set_key("frame_id");
+  // keeping here the sensor name instead of frame_id because the visualizeLidar
+  // plugin relies on this value to get the position of the lidar.
+  // the ros_ign plugin is using the laserscan.proto 'frame' field
   frame->add_value(this->Name());
-  this->dataPtr->laserMsg.set_frame(this->Name());
+  this->dataPtr->laserMsg.set_frame(this->FrameId());
 
   // Store the latest laser scans into laserMsg
   msgs::Set(this->dataPtr->laserMsg.mutable_world_pose(),

diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc
@@ -166,7 +166,7 @@ bool LogicalCameraSensor::Update(const ignition::common::Time &_now)
   this->dataPtr->msg.mutable_header()->clear_data();
   auto frame = this->dataPtr->msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // publish
   this->AddSequence(this->dataPtr->msg.mutable_header());

diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc
@@ -168,7 +168,7 @@ bool MagnetometerSensor::Update(const ignition::common::Time &_now)
   msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // Apply magnetometer noise after converting to body frame
   if (this->dataPtr->noises.find(MAGNETOMETER_X_NOISE_TESLA) !=

diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc
@@ -468,7 +468,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now)
     msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
     auto frame = msg.mutable_header()->add_data();
     frame->set_key("frame_id");
-    frame->add_value(this->Name());
+    frame->add_value(this->FrameId());
 
     std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
@@ -583,7 +583,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now)
       msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec);
       auto frame = msg.mutable_header()->add_data();
       frame->set_key("frame_id");
-      frame->add_value(this->Name());
+      frame->add_value(this->FrameId());
       msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8,
         width, height));
 

diff --git a/src/Sensor.cc b/src/Sensor.cc
@@ -91,6 +91,9 @@ class ignition::sensors::SensorPrivate
   /// A map is used so that a single sensor can have multiple sensor
   /// streams each with a sequence counter.
   public: std::map<std::string, uint64_t> sequences;
+
+  /// \brief frame id
+  public: std::string frame_id;
 };
 
 SensorId SensorPrivate::idCounter = 0;
@@ -121,6 +124,19 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
       return false;
   }
 
+  sdf::ElementPtr element = _sdf.Element();
+  if (element)
+  {
+    if (element->HasElement("ignition_frame_id"))
+    {
+      this->frame_id = element->Get<std::string>("ignition_frame_id");
+    }
+    else
+    {
+      this->frame_id = this->name;
+    }
+  }
+
   // Try resolving the pose first, and only use the raw pose if that fails
   auto semPose = _sdf.SemanticPose();
   sdf::Errors errors = semPose.Resolve(this->pose);
@@ -192,6 +208,18 @@ std::string Sensor::Name() const
   return this->dataPtr->name;
 }
 
+//////////////////////////////////////////////////
+std::string Sensor::FrameId() const
+{
+  return this->dataPtr->frame_id;
+}
+
+//////////////////////////////////////////////////
+void Sensor::SetFrameId(const std::string &_frameId)
+{
+  this->dataPtr->frame_id = _frameId;
+}
+
 //////////////////////////////////////////////////
 std::string Sensor::Topic() const
 {

diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc
@@ -88,6 +88,10 @@ TEST(Sensor_TEST, Sensor)
   EXPECT_EQ(1u, sensor.Id());
 
   EXPECT_EQ(nullptr, sensor.SDF());
+
+  EXPECT_EQ(sensor.Name(), sensor.FrameId());
+  sensor.SetFrameId("frame_id_12");
+  EXPECT_EQ(std::string("frame_id_12"), sensor.FrameId());
 }
 
 //////////////////////////////////////////////////

diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc
@@ -414,7 +414,7 @@ bool ThermalCameraSensor::Update(const ignition::common::Time &_now)
   stamp->set_nsec(_now.nsec);
   auto frame = this->dataPtr->thermalMsg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer,

diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc
@@ -95,6 +95,8 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine)
   EXPECT_EQ(256u, sensor->ImageWidth());
   EXPECT_EQ(257u, sensor->ImageHeight());
 
+  EXPECT_EQ(std::string("base_camera"), sensor->FrameId());
+
   std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF";
   WaitForMessageTestHelper<ignition::msgs::Image> helper(topic);
 

diff --git a/test/integration/camera_sensor_builtin.sdf b/test/integration/camera_sensor_builtin.sdf
@@ -4,6 +4,7 @@
     <link name="link1">
       <sensor name="camera1" type="camera">
         <update_rate>10</update_rate>
+        <ignition_frame_id>base_camera</ignition_frame_id>
         <topic>/test/integration/CameraPlugin_imagesWithBuiltinSDF</topic>
         <camera>
           <horizontal_fov>1.05</horizontal_fov>