Fix frame_id in rgbd_camera (#458)

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

src/RgbdCameraSensor.cc

@@ -251,15 +251,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();
@@ -390,6 +381,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());