fix optical frame id in depth and rgbd camera

Signed-off-by: Ian Chen <[email protected]>

include/gz/sensors/CameraSensor.hh

@@ -153,6 +153,10 @@ namespace ignition
       /// \todo(iche033) Make this function virtual on Harmonic
       public: bool HasInfoConnections() const;
 
+      /// \brief Get the camera optical frame
+      /// \return The camera optical frame
+      public: const std::string &OpticalFrameId() const;
+
       /// \brief Advertise camera info topic.
       /// \return True if successful.
       protected: bool AdvertiseInfo();

src/CameraSensor.cc

@@ -895,6 +895,12 @@ bool CameraSensor::HasInfoConnections() const
   return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections();
 }
 
+//////////////////////////////////////////////////
+const std::string &CameraSensor::OpticalFrameId() const
+{
+  return this->dataPtr->opticalFrameId;
+}
+
 //////////////////////////////////////////////////
 math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix(
     double _imageWidth, double _imageHeight,

src/DepthCameraSensor.cc

@@ -304,15 +304,6 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
   igndbg << "Points for [" << this->Name() << "] advertised on ["
          << this->Topic() << "/points]" << std::endl;
 
-  // Initialize the point message.
-  // \todo(anyone) The true value in the following function call forces
-  // the xyz and rgb fields to be aligned to memory boundaries. This is need
-  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
-  // alignment should be configured.
-  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true,
-      {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
-       {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
-
   if (this->Scene())
   {
     this->CreateCamera();
@@ -427,6 +418,18 @@ bool DepthCameraSensor::CreateCamera()
         std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
         std::placeholders::_4, std::placeholders::_5));
 
+  // Initialize the point message.
+  // \todo(anyone) The true value in the following function call forces
+  // the xyz and rgb fields to be aligned to memory boundaries. This is need
+  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
+  // alignment should be configured.
+  msgs::InitPointCloudPacked(
+      this->dataPtr->pointMsg,
+      this->OpticalFrameId(),
+      true,
+      {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
+       {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
+
   // Set the values of the point message based on the camera information.
   this->dataPtr->pointMsg.set_width(this->ImageWidth());
   this->dataPtr->pointMsg.set_height(this->ImageHeight());
@@ -554,9 +557,10 @@ bool DepthCameraSensor::Update(
                rendering::PF_FLOAT32_R));
   msg.set_pixel_format_type(msgsFormat);
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
+
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->FrameId());
+  frame->add_value(this->OpticalFrameId());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   msg.set_data(this->dataPtr->depthBuffer,

src/RgbdCameraSensor.cc

@@ -247,15 +247,6 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
   if (!this->AdvertiseInfo(this->Topic() + "/camera_info"))
     return false;
 
-  // Initialize the point message.
-  // \todo(anyone) The true value in the following function call forces
-  // the xyz and rgb fields to be aligned to memory boundaries. This is need
-  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
-  // alignment should be configured.
-  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true,
-      {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
-       {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
-
   if (this->Scene())
   {
     this->CreateCameras();
@@ -389,6 +380,16 @@ bool RgbdCameraSensor::CreateCameras()
         std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
         std::placeholders::_4, std::placeholders::_5));
 
+  // Initialize the point message.
+  // \todo(anyone) The true value in the following function call forces
+  // the xyz and rgb fields to be aligned to memory boundaries. This is need
+  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
+  // alignment should be configured.
+  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->OpticalFrameId(),
+      true,
+      {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
+       {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
+
   // Set the values of the point message based on the camera information.
   this->dataPtr->pointMsg.set_width(this->ImageWidth());
   this->dataPtr->pointMsg.set_height(this->ImageHeight());