Add camera optical_frame_id element

Author: MarqRazz

include/sdf/Camera.hh

@@ -348,6 +348,20 @@ namespace sdf
     /// \param[in] _frame The name of the pose relative-to frame.
     public: void SetPoseRelativeTo(const std::string &_frame);
 
+    /// \brief Get the name of the coordinate frame relative to which this
+    /// object's camera_info message header is expressed.
+    /// Note: while Gazebo interprets the camera frame to be looking towards +X,
+    /// other tools, such as ROS interprets this frame as looking towards +Z.
+    /// The Camera sensor assumes that the color and depth images are captured at
+    /// the same frame_id.
+    /// \return The name of the frame this camera uses in its camera_info topic.
+    public: const std::string OpticalFrameId() const;
+
+    /// \brief Set the name of the coordinate frame relative to which this
+    /// object's camera_info is expressed.
+    /// \param[in] _frame The frame this camera uses in its camera_info topic.
+    public: void SetOpticalFrameId(const std::string &_frame);
+
     /// \brief Get the lens type. This is the type of the lens mapping.
     /// Supported values are gnomonical, stereographic, equidistant,
     /// equisolid_angle, orthographic, custom. For gnomonical (perspective)

sdf/1.7/camera.sdf

@@ -161,5 +161,9 @@
     <description><![CDATA[Visibility mask of a camera. When (camera's visibility_mask & visual's visibility_flags) evaluates to non-zero, the visual will be visible to the camera.]]></description>
   </element>
 
+  <element name="optical_frame_id" type="string" default="" required="0">
+    <description>An optional frame id name to be used in the camera_info message header.</description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
 </element> <!-- End Camera -->

sdf/1.8/camera.sdf

@@ -161,5 +161,9 @@
     <description><![CDATA[Visibility mask of a camera. When (camera's visibility_mask & visual's visibility_flags) evaluates to non-zero, the visual will be visible to the camera.]]></description>
   </element>
 
+  <element name="optical_frame_id" type="string" default="" required="0">
+    <description>An optional frame id name to be used in the camera_info message header.</description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
 </element> <!-- End Camera -->

sdf/1.9/camera.sdf

@@ -196,5 +196,9 @@
     <description><![CDATA[Visibility mask of a camera. When (camera's visibility_mask & visual's visibility_flags) evaluates to non-zero, the visual will be visible to the camera.]]></description>
   </element>
 
+  <element name="optical_frame_id" type="string" default="" required="0">
+    <description>An optional frame id name to be used in the camera_info message header.</description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
 </element> <!-- End Camera -->

src/Camera.cc

@@ -142,6 +142,9 @@ class sdf::Camera::Implementation
   /// \brief Frame of the pose.
   public: std::string poseRelativeTo = "";
 
+  /// \brief Frame ID the camera_info message header is expressed.
+  public: std::string opticalFrameId{""};
+
   /// \brief Lens type.
   public: std::string lensType{"stereographic"};
 
@@ -348,6 +351,13 @@ Errors Camera::Load(ElementPtr _sdf)
   // Load the pose. Ignore the return value since the pose is optional.
   loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo);
 
+  // Load the optional optical_frame_id value.
+  if (_sdf->HasElement("optical_frame_id"))
+  {
+    this->dataPtr->opticalFrameId = _sdf->Get<std::string>("optical_frame_id", 
+        this->dataPtr->opticalFrameId).first;
+  }
+
   // Load the lens values.
   if (_sdf->HasElement("lens"))
   {
@@ -692,7 +702,8 @@ bool Camera::operator==(const Camera &_cam) const
     this->SaveFrames() == _cam.SaveFrames() &&
     this->SaveFramesPath() == _cam.SaveFramesPath() &&
     this->ImageNoise() == _cam.ImageNoise() &&
-    this->VisibilityMask() == _cam.VisibilityMask();
+    this->VisibilityMask() == _cam.VisibilityMask() &&
+    this->OpticalFrameId() == _cam.OpticalFrameId();
 }
 
 //////////////////////////////////////////////////
@@ -809,6 +820,18 @@ void Camera::SetPoseRelativeTo(const std::string &_frame)
   this->dataPtr->poseRelativeTo = _frame;
 }
 
+/////////////////////////////////////////////////
+const std::string Camera::OpticalFrameId() const
+{
+  return this->dataPtr->opticalFrameId;
+}
+
+/////////////////////////////////////////////////
+void Camera::SetOpticalFrameId(const std::string &_frame)
+{
+  this->dataPtr->opticalFrameId = _frame;
+}
+
 /////////////////////////////////////////////////
 std::string Camera::LensType() const
 {
@@ -1149,5 +1172,7 @@ sdf::ElementPtr Camera::ToElement() const
         this->SegmentationType());
   }
 
+  elem->GetElement("optical_frame_id")->Set<std::string>(this->OpticalFrameId());
+
   return elem;
 }

src/Camera_TEST.cc

@@ -130,6 +130,10 @@ TEST(DOMCamera, Construction)
   cam.SetPoseRelativeTo("/frame");
   EXPECT_EQ("/frame", cam.PoseRelativeTo());
 
+  EXPECT_TRUE(cam.OpticalFrameId().empty());
+  cam.SetOpticalFrameId("/optical_frame");
+  EXPECT_EQ("/optical_frame", cam.OpticalFrameId());
+
   EXPECT_EQ("stereographic", cam.LensType());
   cam.SetLensType("custom");
   EXPECT_EQ("custom", cam.LensType());
@@ -240,6 +244,7 @@ TEST(DOMCamera, ToElement)
   cam.SetPoseRelativeTo("/frame");
   cam.SetSaveFrames(true);
   cam.SetSaveFramesPath("/tmp");
+  cam.SetOpticalFrameId("/optical_frame");
 
   sdf::ElementPtr camElem = cam.ToElement();
   EXPECT_NE(nullptr, camElem);
@@ -259,6 +264,7 @@ TEST(DOMCamera, ToElement)
   EXPECT_EQ("/frame", cam2.PoseRelativeTo());
   EXPECT_TRUE(cam2.SaveFrames());
   EXPECT_EQ("/tmp", cam2.SaveFramesPath());
+  EXPECT_EQ("/optical_frame", cam2.OpticalFrameId());
 
   // make changes to DOM and verify ToElement produces updated values
   cam2.SetNearClip(0.33);