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

include/ignition/sensors/Sensor.hh

@@ -142,6 +142,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;

src/AirPressureSensor.cc

@@ -159,7 +159,7 @@ bool AirPressureSensor::Update(
   *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());
 
   // This block of code comes from RotorS:
   // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp

src/AltimeterSensor.cc

@@ -146,7 +146,7 @@ bool AltimeterSensor::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());
 
   // Apply altimeter vertical position noise
   if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) !=

src/CameraSensor.cc

@@ -400,7 +400,7 @@ bool CameraSensor::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());
     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);

src/DepthCameraSensor.cc

@@ -545,7 +545,7 @@ bool DepthCameraSensor::Update(
   *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());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   msg.set_data(this->dataPtr->depthBuffer,

src/ImuSensor.cc

@@ -233,7 +233,7 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_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());
 
   if (this->dataPtr->orientationEnabled)
   {

src/Lidar.cc

@@ -232,8 +232,11 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_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(),

src/LogicalCameraSensor.cc

@@ -163,7 +163,7 @@ bool LogicalCameraSensor::Update(
   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());

src/MagnetometerSensor.cc

@@ -168,7 +168,7 @@ bool MagnetometerSensor::Update(
   *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());
 
   // Apply magnetometer noise after converting to body frame
   if (this->dataPtr->noises.find(MAGNETOMETER_X_NOISE_TESLA) !=

src/RgbdCameraSensor.cc

@@ -477,7 +477,7 @@ bool RgbdCameraSensor::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());
 
     std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
@@ -589,7 +589,7 @@ bool RgbdCameraSensor::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());
       msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8,
         width, height));
 

src/Sensor.cc

@@ -106,6 +106,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;
@@ -136,6 +139,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);
@@ -226,6 +242,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
 {

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());
 }
 
 //////////////////////////////////////////////////

src/ThermalCameraSensor.cc

@@ -457,7 +457,7 @@ bool ThermalCameraSensor::Update(
   *stamp = msgs::Convert(_now);
   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);
 

test/integration/camera.cc

@@ -101,6 +101,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);
 

test/sdf/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>