diff --git a/CMakeLists.txt b/CMakeLists.txt index b7562e40..6abaa3d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,7 +86,7 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- # Find SDFormat -ign_find_package(sdformat12 REQUIRED VERSION 12.5.0) +ign_find_package(sdformat12 REQUIRED VERSION 12.6.0) set(SDF_VER ${sdformat12_VERSION_MAJOR}) set(IGN_SENSORS_PLUGIN_PATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 4fc0ae4b..6ea7688b 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -129,6 +129,9 @@ class ignition::sensors::CameraSensorPrivate /// \brief Camera information message. public: msgs::CameraInfo infoMsg; + /// \brief The frame this camera uses in its camera_info topic. + public: std::string opticalFrameId{""}; + /// \brief Topic for info message. public: std::string infoTopic{""}; @@ -462,7 +465,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) *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()); + frame->add_value(this->dataPtr->opticalFrameId); msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); } @@ -676,11 +679,20 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) // Note: while Gazebo interprets the camera frame to be looking towards +X, // other tools, such as ROS, may interpret this frame as looking towards +Z. - // TODO(anyone) Expose the `frame_id` as an SDF parameter so downstream users - // can populate it with arbitrary frames. + // To make this configurable the user has the option to set an optical frame. + // If the user has set in the cameraSdf use it, + // otherwise fall back to the sensor frame. + if (_cameraSdf->OpticalFrameId().empty()) + { + this->dataPtr->opticalFrameId = this->FrameId(); + } + else + { + this->dataPtr->opticalFrameId = _cameraSdf->OpticalFrameId(); + } auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data(); infoFrame->set_key("frame_id"); - infoFrame->add_value(this->FrameId()); + infoFrame->add_value(this->dataPtr->opticalFrameId); this->dataPtr->infoMsg.set_width(width); this->dataPtr->infoMsg.set_height(height); diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index aeab3d55..783e17df 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -119,6 +119,9 @@ class ignition::sensors::RgbdCameraSensorPrivate /// point cloud. public: unsigned int channels = 4; + /// \brief Frame ID the camera_info message header is expressed. + public: std::string opticalFrameId{""}; + /// \brief Pointer to an image to be published public: ignition::rendering::Image image; @@ -258,7 +261,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) // 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->Name(), true, + msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); @@ -299,6 +302,20 @@ bool RgbdCameraSensor::CreateCameras() this->dataPtr->depthCamera->SetNearClipPlane(cameraSdf->NearClip()); this->dataPtr->depthCamera->SetFarClipPlane(cameraSdf->FarClip()); + // Note: while Gazebo interprets the camera frame to be looking towards +X, + // other tools, such as ROS, may interpret this frame as looking towards +Z. + // To make this configurable the user has the option to set an optical frame. + // If the user has set in the cameraSdf use it, + // otherwise fall back to the sensor frame. + if (cameraSdf->OpticalFrameId().empty()) + { + this->dataPtr->opticalFrameId = this->FrameId(); + } + else + { + this->dataPtr->opticalFrameId = cameraSdf->OpticalFrameId(); + } + // Depth camera clip params are new and only override the camera clip // params if specified. if (cameraSdf->HasDepthCamera()) @@ -477,7 +494,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *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()); + frame->add_value(this->dataPtr->opticalFrameId); std::lock_guard lock(this->dataPtr->mutex); @@ -589,7 +606,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *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()); + frame->add_value(this->dataPtr->opticalFrameId); msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height));