From d7469881ea4fcbf7676c2b356f8dee344609bd55 Mon Sep 17 00:00:00 2001 From: Kai Date: Fri, 29 Nov 2024 15:35:44 +0100 Subject: [PATCH 1/2] Fix RgbdSensor_nws_ros2 Optimized camera data handling: Data is now retrieved only during the first iteration and stored in a class variable, avoiding repeated requests in every cycle. Distortion management: Support for the none distortion type has been added, eliminating errors and reducing the number of log messages generated by yarprobotinterface. --- .../RgbdSensor_nws_ros2.cpp | 68 +++++++++---------- .../rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.h | 13 ++-- 2 files changed, 40 insertions(+), 41 deletions(-) diff --git a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp index 0860b96..9daefad 100644 --- a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp +++ b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp @@ -223,7 +223,6 @@ bool RgbdSensor_nws_ros2::detach() bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, const std::string& frame_id, - const yarp::os::Stamp& stamp, const SensorType& sensorType) { double phyF = 0.0; @@ -259,22 +258,21 @@ bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, } distModel = camData.find("distortionModel").asString(); - if (distModel != "plumb_bob") - { - yCError(RGBDSENSOR_NWS_ROS2) << "Distortion model not supported"; - return false; - } parVector.emplace_back(phyF,"physFocalLength"); parVector.emplace_back(fx,"focalLengthX"); parVector.emplace_back(fy,"focalLengthY"); parVector.emplace_back(cx,"principalPointX"); parVector.emplace_back(cy,"principalPointY"); - parVector.emplace_back(k1,"k1"); - parVector.emplace_back(k2,"k2"); - parVector.emplace_back(t1,"t1"); - parVector.emplace_back(t2,"t2"); - parVector.emplace_back(k3,"k3"); + + if (distModel != none) + { + parVector.emplace_back(k1,"k1"); + parVector.emplace_back(k2,"k2"); + parVector.emplace_back(t1,"t1"); + parVector.emplace_back(t2,"t2"); + parVector.emplace_back(k3,"k3"); + } for(auto& par : parVector) { if(!camData.check(par.parname)) { @@ -285,18 +283,19 @@ bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, } cameraInfo.header.frame_id = frame_id; - cameraInfo.header.stamp.sec = static_cast(stamp.getTime()); // FIXME - cameraInfo.header.stamp.nanosec = static_cast(1000000000UL * (stamp.getTime() - int(stamp.getTime()))); // FIXME cameraInfo.width = sensorType == COLOR_SENSOR ? sensor_p->getRgbWidth() : sensor_p->getDepthWidth(); cameraInfo.height = sensorType == COLOR_SENSOR ? sensor_p->getRgbHeight() : sensor_p->getDepthHeight(); cameraInfo.distortion_model = distModel; - cameraInfo.d.resize(5); - cameraInfo.d[0] = k1; - cameraInfo.d[1] = k2; - cameraInfo.d[2] = t1; - cameraInfo.d[3] = t2; - cameraInfo.d[4] = k3; + if (distModel != none) + { + cameraInfo.d.resize(5); + cameraInfo.d[0] = k1; + cameraInfo.d[1] = k2; + cameraInfo.d[2] = t1; + cameraInfo.d[3] = t2; + cameraInfo.d[4] = k3; + } cameraInfo.k[0] = fx; cameraInfo.k[1] = 0; cameraInfo.k[2] = cx; cameraInfo.k[3] = 0; cameraInfo.k[4] = fy; cameraInfo.k[5] = cy; @@ -356,7 +355,14 @@ bool RgbdSensor_nws_ros2::writeData() oldDepthStamp = depthStamp; } - // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves. + // Call setCamInfo only the first time + if (m_firstTime) { + setCamInfo(m_camInfoData.colorCamInfo, "color_frame", COLOR_SENSOR); + setCamInfo(m_camInfoData.depthCamInfo, "depth_frame", DEPTH_SENSOR); + m_firstTime = false; + } + + // Use m_camInfoData for subsequent calls if (rgb_data_ok) { sensor_msgs::msg::Image rColorImage; rColorImage.data.resize(colorImage.getRawImageSize()); @@ -372,15 +378,10 @@ bool RgbdSensor_nws_ros2::writeData() rosPublisher_color->publish(rColorImage); - sensor_msgs::msg::CameraInfo camInfoC; - if (setCamInfo(camInfoC, m_color_frame_id, colorStamp, COLOR_SENSOR)) { - if(m_forceInfoSync) { - camInfoC.header.stamp = rColorImage.header.stamp; - } - rosPublisher_colorCaminfo->publish(camInfoC); - } else { - yCWarning(RGBDSENSOR_NWS_ROS2, "Missing color camera parameters... camera info messages will be not sent"); + if (m_forceInfoSync) { + m_camInfoData.colorCamInfo.header.stamp = rColorImage.header.stamp; } + rosPublisher_colorCaminfo->publish(m_camInfoData.colorCamInfo); } if (depth_data_ok) @@ -399,15 +400,10 @@ bool RgbdSensor_nws_ros2::writeData() rosPublisher_depth->publish(rDepthImage); - sensor_msgs::msg::CameraInfo camInfoD; - if (setCamInfo(camInfoD, m_depth_frame_id, depthStamp, DEPTH_SENSOR)) { - if(m_forceInfoSync) { - camInfoD.header.stamp = rDepthImage.header.stamp; - } - rosPublisher_depthCaminfo->publish(camInfoD); - } else { - yCWarning(RGBDSENSOR_NWS_ROS2, "Missing depth camera parameters... camera info messages will be not sent"); + if (m_forceInfoSync) { + m_camInfoData.depthCamInfo.header.stamp = rDepthImage.header.stamp; } + rosPublisher_depthCaminfo->publish(m_camInfoData.depthCamInfo); } return true; diff --git a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.h b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.h index 282b0aa..09a742c 100644 --- a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.h +++ b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.h @@ -30,6 +30,11 @@ * Documentation to be added * */ +struct CameraInfoData { + sensor_msgs::msg::CameraInfo colorCamInfo; + sensor_msgs::msg::CameraInfo depthCamInfo; +}; + class RgbdSensor_nws_ros2 : public yarp::dev::DeviceDriver, public yarp::dev::WrapperSingle, @@ -86,16 +91,14 @@ class RgbdSensor_nws_ros2 : yarp::dev::IRGBDSensor* sensor_p {nullptr}; yarp::dev::IFrameGrabberControls* fgCtrl {nullptr}; bool m_forceInfoSync {true}; + bool m_firstTime {true}; + + CameraInfoData m_camInfoData; bool writeData(); bool setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, const std::string& frame_id, - const yarp::os::Stamp& stamp, const SensorType& sensorType); - -// static std::string yarp2RosPixelCode(int code); - - bool fromConfig(yarp::os::Searchable &config); bool initialize_ROS2(yarp::os::Searchable& config); From 3d431cfbe7c091237b1347ea9c3d5283081a9ea7 Mon Sep 17 00:00:00 2001 From: Kai Date: Mon, 2 Dec 2024 10:43:33 +0100 Subject: [PATCH 2/2] None as string in condition --- src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp index 9daefad..0956330 100644 --- a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp +++ b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp @@ -265,7 +265,7 @@ bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, parVector.emplace_back(cx,"principalPointX"); parVector.emplace_back(cy,"principalPointY"); - if (distModel != none) + if (distModel != "none") { parVector.emplace_back(k1,"k1"); parVector.emplace_back(k2,"k2"); @@ -287,7 +287,7 @@ bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo, cameraInfo.height = sensorType == COLOR_SENSOR ? sensor_p->getRgbHeight() : sensor_p->getDepthHeight(); cameraInfo.distortion_model = distModel; - if (distModel != none) + if (distModel != "none") { cameraInfo.d.resize(5); cameraInfo.d[0] = k1;