diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh
index 44c5874e6..9650b1f06 100644
--- a/include/sdf/Camera.hh
+++ b/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)
diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf
index ef637048d..9974aa544 100644
--- a/sdf/1.7/camera.sdf
+++ b/sdf/1.7/camera.sdf
@@ -161,5 +161,9 @@
+
+ An optional frame id name to be used in the camera_info message header.
+
+
diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf
index ef637048d..9974aa544 100644
--- a/sdf/1.8/camera.sdf
+++ b/sdf/1.8/camera.sdf
@@ -161,5 +161,9 @@
+
+ An optional frame id name to be used in the camera_info message header.
+
+
diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf
index 092aba97c..cfd09f890 100644
--- a/sdf/1.9/camera.sdf
+++ b/sdf/1.9/camera.sdf
@@ -196,5 +196,9 @@
+
+ An optional frame id name to be used in the camera_info message header.
+
+
diff --git a/src/Camera.cc b/src/Camera.cc
index 6700b9144..770ead070 100644
--- a/src/Camera.cc
+++ b/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("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,8 @@ sdf::ElementPtr Camera::ToElement() const
this->SegmentationType());
}
+ elem->GetElement("optical_frame_id")->Set(
+ this->OpticalFrameId());
+
return elem;
}
diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc
index b76651a23..2fb7cd01e 100644
--- a/src/Camera_TEST.cc
+++ b/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);