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

…ensor

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: 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();
 }
 
+//////////////////////////////////////////////////
+std::string CameraSensor::OpticalFrameId() const
+{
+  return this->dataPtr->opticalFrameId;
+}
+
 //////////////////////////////////////////////////
 math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix(
     double _imageWidth, double _imageHeight,

src/DepthCameraSensor.cc

@@ -90,6 +90,8 @@ class gz::sensors::DepthCameraSensorPrivate
   /// \brief xyz data buffer.
   public: float *xyzBuffer = nullptr;
 
+  public: gz::msgs::Header_Map* frame_id = nullptr;
+
   /// \brief Near clip distance.
   public: float near = 0.0;
 
@@ -131,6 +133,9 @@ class gz::sensors::DepthCameraSensorPrivate
   /// \brief SDF Sensor DOM object.
   public: sdf::Sensor sdfSensor;
 
+  /// \brief The frame this camera uses in its camera_info topic.
+  public: std::string opticalFrameId{""};
+
   /// \brief The point cloud message.
   public: msgs::PointCloudPacked pointMsg;
 
@@ -549,9 +554,12 @@ 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());
+  if (!this->dataPtr->frame_id)
+  {
+    this->dataPtr->frame_id = msg.mutable_header()->add_data();
+    this->dataPtr->frame_id->set_key("frame_id");
+    this->dataPtr->frame_id->add_value(this->OpticalFrameId());
+  }
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   msg.set_data(this->dataPtr->depthBuffer,
@@ -608,6 +616,10 @@ bool DepthCameraSensor::Update(
         this->dataPtr->xyzBuffer,
         this->dataPtr->image.Data<unsigned char>());
 
+    auto frame = this->dataPtr->pointMsg.mutable_header()->add_data();
+    frame->set_key("frame_id");
+    frame->add_value(this->OpticalFrameId());
+
     this->AddSequence(this->dataPtr->pointMsg.mutable_header(), "pointMsg");
     this->dataPtr->pointPub.Publish(this->dataPtr->pointMsg);
   }