Use //sensor/frame_id sdf element (#444)

Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

src/CameraSensor.cc

@@ -689,6 +689,9 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf)
   // To make this configurable the user has the option to set an optical frame.
   // If the user has set <optical_frame_id> in the cameraSdf use it,
   // otherwise fall back to the sensor frame.
+  // \todo(iche033 sdf::Camera::OpticalFrameId is deprecated.
+  // Remove this if statement when gz-sensors is updated to use sdformat16
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   if (_cameraSdf->OpticalFrameId().empty())
   {
    this->dataPtr->opticalFrameId = this->FrameId();
@@ -697,6 +700,8 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf)
   {
    this->dataPtr->opticalFrameId = _cameraSdf->OpticalFrameId();
   }
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
+
   auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data();
   infoFrame->set_key("frame_id");
   infoFrame->add_value(this->dataPtr->opticalFrameId);

src/DopplerVelocityLog.cc

@@ -1946,6 +1946,9 @@ namespace gz
         if (this->dataPtr->publishingEstimates)
         {
           auto * headerMessage = bottomModeMessage.mutable_header();
+          auto frame = headerMessage->add_data();
+          frame->set_key("frame_id");
+          frame->add_value(this->FrameId());
           this->AddSequence(headerMessage, "doppler_velocity_log");
           this->dataPtr->pub.Publish(bottomModeMessage);
         }
@@ -1965,6 +1968,9 @@ namespace gz
         if (this->dataPtr->publishingEstimates)
         {
           auto * headerMessage = waterMassModeMessage.mutable_header();
+          auto frame = headerMessage->add_data();
+          frame->set_key("frame_id");
+          frame->add_value(this->FrameId());
           this->AddSequence(headerMessage, "doppler_velocity_log");
           this->dataPtr->pub.Publish(waterMassModeMessage);
         }

src/GpuLidarSensor.cc

@@ -134,7 +134,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf)
   // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
   // alignment should be configured. This same problem is in the
   // RgbdCameraSensor.
-  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->Name(), true,
+  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true,
       {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
       {"intensity", msgs::PointCloudPacked::Field::FLOAT32},
       {"ring", msgs::PointCloudPacked::Field::UINT16}});

src/RgbdCameraSensor.cc

@@ -302,6 +302,9 @@ bool RgbdCameraSensor::CreateCameras()
   // To make this configurable the user has the option to set an optical frame.
   // If the user has set <optical_frame_id> in the cameraSdf use it,
   // otherwise fall back to the sensor frame.
+  // \todo(iche033 sdf::Camera::OpticalFrameId is deprecated.
+  // Remove this if statement when gz-sensors is updated to use sdformat16
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   if (cameraSdf->OpticalFrameId().empty())
   {
    this->dataPtr->opticalFrameId = this->FrameId();
@@ -310,6 +313,7 @@ bool RgbdCameraSensor::CreateCameras()
   {
    this->dataPtr->opticalFrameId = cameraSdf->OpticalFrameId();
   }
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
 
   // Depth camera clip params are new and only override the camera clip
   // params if specified.

src/Sensor.cc

@@ -138,7 +138,7 @@ class gz::sensors::SensorPrivate
   public: std::map<std::string, uint64_t> sequences;
 
   /// \brief frame id
-  public: std::string frame_id;
+  public: std::string frameId;
 
   /// \brief If sensor is active or not.
   public: bool active = true;
@@ -161,9 +161,6 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
 {
   this->sdfSensor = _sdf;
 
-  // All SDF code gets auto converted to latest version. This code is
-  // written assuming sdformat 1.7 is the latest
-
   // \todo(nkoenig) what to do with <always_on>? SDFormat docs seem
   // to say if true
   //  then the update_rate will be obeyed. Gazebo seems to use it as if
@@ -174,24 +171,29 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
   // sensor data. Whether or not that data should be visualized seems to
   // be outside the scope of this library
 
-  // \todo(nkoenig) how to use frame?
   this->name = _sdf.Name();
   if (!_sdf.Topic().empty())
   {
     if (!this->SetTopic(_sdf.Topic()))
       return false;
   }
 
-  sdf::ElementPtr element = _sdf.Element();
-  if (element)
+  this->frameId = _sdf.FrameId();
+  if (this->frameId.empty())
   {
-    if (element->HasElement("gz_frame_id"))
-    {
-      this->frame_id = element->Get<std::string>("gz_frame_id");
-    }
-    else
+    sdf::ElementPtr element = _sdf.Element();
+    if (element)
     {
-      this->frame_id = this->name;
+      if (element->HasElement("gz_frame_id"))
+      {
+        gzwarn << "//sensor/gz_frame_id is deprecated. "
+               << "Please use //sensor/frame_id instead. " << std::endl;
+        this->frameId = element->Get<std::string>("gz_frame_id");
+      }
+      else
+      {
+        this->frameId = this->name;
+      }
     }
   }
 
@@ -288,13 +290,13 @@ std::string Sensor::Name() const
 //////////////////////////////////////////////////
 std::string Sensor::FrameId() const
 {
-  return this->dataPtr->frame_id;
+  return this->dataPtr->frameId;
 }
 
 //////////////////////////////////////////////////
 void Sensor::SetFrameId(const std::string &_frameId)
 {
-  this->dataPtr->frame_id = _frameId;
+  this->dataPtr->frameId = _frameId;
 }
 
 //////////////////////////////////////////////////

src/Sensor_TEST.cc

@@ -390,15 +390,17 @@ TEST(Sensor_TEST, FrameIdFromSdf)
   {
     TestSensor testSensor;
     loadSensorWithSdfParam(testSensor,
-                           "<gz_frame_id>custom_frame_id</gz_frame_id>");
+                           "<frame_id>custom_frame_id</frame_id>");
     EXPECT_EQ("custom_frame_id", testSensor.FrameId());
   }
+  // todo(iche033)
+  // Remove when gz-sensors is updated to use sdformat16
   {
     TestSensor testSensor;
     loadSensorWithSdfParam(testSensor, R"(
-      <ignition_frame_id>custom_frame_id</ignition_frame_id>
+      <frame_id>custom_frame_id</frame_id>
       <gz_frame_id>other_custom_frame_id</gz_frame_id>)");
-    EXPECT_EQ("other_custom_frame_id", testSensor.FrameId());
+    EXPECT_EQ("custom_frame_id", testSensor.FrameId());
   }
 }
 //////////////////////////////////////////////////

test/integration/dvl.cc

@@ -45,6 +45,7 @@ using DopplerVelocityLog = sensors::DopplerVelocityLog;
 struct DVLConfig
 {
   std::string name = "dvl";
+  std::string frameId = "dvl_frame";
   std::string topic = "/gz/sensors/test/dvl";
   double updateRate = 30;  // Hz
 
@@ -75,6 +76,7 @@ sdf::ElementPtr MakeDVLSdf(const DVLConfig &_config)
     << " <model name='model'>"
     << "  <link name='link'>"
     << "   <sensor name='" << _config.name << "' type='custom' gz:type='dvl'>"
+    << "    <frame_id>" << _config.frameId << "</frame_id>"
     << "    <always_on>" << _config.alwaysOn << "</always_on>"
     << "    <update_rate>" << _config.updateRate << "</update_rate>"
     << "    <topic>" << _config.topic << "</topic>"
@@ -345,6 +347,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileStatic)
   }
   EXPECT_EQ(0, message.status());
 
+  // check frame id
+  EXPECT_TRUE(message.has_header());
+  EXPECT_LT(1, message.header().data().size());
+  EXPECT_EQ("frame_id", message.header().data(0).key());
+  ASSERT_EQ(1, message.header().data(0).value().size());
+  EXPECT_EQ("dvl_frame", message.header().data(0).value(0));
+
   this->manager.Remove(sensor->Id());
 }
 
@@ -436,6 +445,13 @@ TEST_P(DopplerVelocityLogTest, WaterMassTrackingWhileStatic)
   }
   EXPECT_EQ(0, message.status());
 
+  // check frame id
+  EXPECT_TRUE(message.has_header());
+  EXPECT_LT(1, message.header().data().size());
+  EXPECT_EQ("frame_id", message.header().data(0).key());
+  ASSERT_EQ(1, message.header().data(0).value().size());
+  EXPECT_EQ("dvl_frame", message.header().data(0).value(0));
+
   this->manager.Remove(sensor->Id());
 }
 
@@ -519,6 +535,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileInMotion)
   }
   EXPECT_EQ(0, message.status());
 
+  // check frame id
+  EXPECT_TRUE(message.has_header());
+  EXPECT_LT(1, message.header().data().size());
+  EXPECT_EQ("frame_id", message.header().data(0).key());
+  ASSERT_EQ(1, message.header().data(0).value().size());
+  EXPECT_EQ("dvl_frame", message.header().data(0).value(0));
+
   this->manager.Remove(sensor->Id());
 }
 

test/integration/gpu_lidar_sensor.cc

@@ -65,7 +65,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name,
     const double vertResolution, const double vertMinAngle,
     const double vertMaxAngle, const double rangeResolution,
     const double rangeMin, const double rangeMax, const bool alwaysOn,
-    const bool visualize)
+    const bool visualize, const std::string &frameId)
 {
   std::ostringstream stream;
   stream
@@ -74,6 +74,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name,
     << " <model name='m1'>"
     << "  <link name='link1'>"
     << "    <sensor name='" << name << "' type='gpu_lidar'>"
+    << "      <frame_id>" << frameId << "</frame_id>"
     << "      <pose>" << pose << "</pose>"
     << "      <topic>" << topic << "</topic>"
     << "      <updateRate>"<< updateRate <<"</updateRate>"
@@ -194,14 +195,15 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
 
   // Create sensor description in SDF
   gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0.1),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
     horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
     vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Setup gz-rendering with an empty scene
   auto *engine = gz::rendering::engine(_renderEngine);
@@ -313,14 +315,15 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
 
   // Create sensor SDF
   gz::math::Pose3d testPose(gz::math::Vector3d(0.0, 0.0, 0.1),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
     horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
     vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create and populate scene
   gz::rendering::RenderEngine *engine =
@@ -399,7 +402,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
       LASER_TOL);
   EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), gz::math::INF_D);
 
-  EXPECT_EQ(laserMsgs.back().frame(), name);
+  EXPECT_EQ(laserMsgs.back().frame(), frameId);
   EXPECT_NEAR(laserMsgs.back().angle_min(), horzMinAngle, 1e-4);
   EXPECT_NEAR(laserMsgs.back().angle_max(), horzMaxAngle, 1e-4);
   EXPECT_NEAR(laserMsgs.back().count(), horzSamples, 1e-4);
@@ -425,6 +428,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
   EXPECT_FALSE(pointMsgs.back().is_dense());
   EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size());
 
+  EXPECT_TRUE(pointMsgs.back().has_header());
+  EXPECT_LT(1, pointMsgs.back().header().data().size());
+  EXPECT_EQ("frame_id", pointMsgs.back().header().data(0).key());
+  ASSERT_EQ(1, pointMsgs.back().header().data(0).value().size());
+  EXPECT_EQ(frameId, pointMsgs.back().header().data(0).value(0));
+
   // Clean up rendering ptrs
   visualBox1.reset();
 
@@ -462,22 +471,23 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
 
   // Create sensor SDF
   gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate,
       topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create a second sensor SDF rotated
   gz::math::Pose3d testPose2(gz::math::Vector3d(0, 0, 0.1),
       gz::math::Quaterniond(GZ_PI/2.0, 0, 0));
   sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate,
       topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create and populate scene
   gz::rendering::RenderEngine *engine =
@@ -618,14 +628,15 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
 
   // Create sensor SDF
   gz::math::Pose3d testPose(gz::math::Vector3d(0.25, 0.0, 0.5),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
     horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
     vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+    rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create and populate scene
   gz::rendering::RenderEngine *engine =
@@ -745,22 +756,23 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
 
   // Create sensor SDF
   gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate,
       topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create a second sensor SDF at an xy offset of 1
   gz::math::Pose3d testPose2(gz::math::Vector3d(1, 1, 0.1),
       gz::math::Quaterniond::Identity);
   sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate,
       topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
   // Create and populate scene
   gz::rendering::RenderEngine *engine =
@@ -870,6 +882,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
   const double rangeMax = 10.0;
   const bool alwaysOn = 1;
   const bool visualize = 1;
+  const std::string frameId = "TestGpuLidar_frame";
   auto testPose = gz::math::Pose3d();
 
   // Scene
@@ -892,7 +905,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
     auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
       horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
     auto lidar = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
     ASSERT_NE(nullptr, lidar);
@@ -906,7 +919,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
     auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
       horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
     auto lidar = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
     ASSERT_NE(nullptr, lidar);
@@ -921,7 +934,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
     auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
       horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
       vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
-      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
+      rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
 
     auto sensor = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
     EXPECT_EQ(nullptr, sensor);