Expose optical frame in CameraSensor so it can be set in DepthCameraS…

…ensor

Signed-off-by: Levi Armstrong <[email protected]>

include/gz/sensors/CameraSensor.hh

@@ -146,6 +146,10 @@ namespace gz
       /// \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

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

@@ -299,15 +299,6 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
   gzdbg << "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();
@@ -422,6 +413,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());
@@ -549,9 +552,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();
+
+  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,