Conform to ros format for header field frame_id of sensor msgs

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;

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

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) !=

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);

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,

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);

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(),

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

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) !=

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));
 

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
 {

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

@@ -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,

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);
 

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>