diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index b766d4089..223bcab76 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -324,6 +324,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/src/Camera.cc b/src/Camera.cc index 52c2705cf..9d3e52ed2 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -133,6 +133,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"}; @@ -329,6 +332,11 @@ 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); + if (_sdf->HasElement("optical_frame_id")) + { + this->dataPtr->opticalFrameId = _sdf->Get("optical_frame_id", "").first; + } + // Load the lens values. if (_sdf->HasElement("lens")) { @@ -635,7 +643,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(); } ////////////////////////////////////////////////// @@ -752,6 +761,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 {