From 488fcffcf34a383766bf3c9d2711e9074e4255a9 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 20 Jul 2023 22:34:32 -0600 Subject: [PATCH] Expose optical frame in CameraSensor so it can be set in DepthCameraSensor Signed-off-by: Levi Armstrong --- include/gz/sensors/CameraSensor.hh | 4 ++++ src/CameraSensor.cc | 6 ++++++ src/DepthCameraSensor.cc | 26 +++++++++++++++----------- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index aae0ad45..b4939f7a 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/include/gz/sensors/CameraSensor.hh @@ -146,6 +146,10 @@ namespace gz /// \todo(iche033) Make this function virtual on Harmonic public: bool HasInfoConnections() const; + /// \brief Get the camera optical frame + /// \return The camera optical frame + public: const std::string& OpticalFrameId() const; + /// \brief Advertise camera info topic. /// \return True if successful. protected: bool AdvertiseInfo(); diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 173f2aaf..140d162d 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -921,6 +921,12 @@ bool CameraSensor::HasInfoConnections() const return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections(); } +////////////////////////////////////////////////// +const std::string& CameraSensor::OpticalFrameId() const +{ + return this->dataPtr->opticalFrameId; +} + ////////////////////////////////////////////////// math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix( double _imageWidth, double _imageHeight, diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index e295d0e5..f965e18e 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -299,15 +299,6 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) gzdbg << "Points for [" << this->Name() << "] advertised on [" << this->Topic() << "/points]" << std::endl; - // 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->CreateCamera(); @@ -422,6 +413,18 @@ bool DepthCameraSensor::CreateCamera() 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()); @@ -549,9 +552,10 @@ bool DepthCameraSensor::Update( rendering::PF_FLOAT32_R)); msg.set_pixel_format_type(msgsFormat); *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); + + auto* frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameId()); + frame->add_value(this->OpticalFrameId()); std::lock_guard lock(this->dataPtr->mutex); msg.set_data(this->dataPtr->depthBuffer,