From 3659ffd1102be4163434f6352d034adc8e898908 Mon Sep 17 00:00:00 2001 From: jingyang-huang <1178065793@qq.com> Date: Thu, 9 Apr 2026 16:53:50 +0800 Subject: [PATCH] change into raw image sync, better for online inference --- config/control_command.yaml | 4 +- include/cloud_reprojection_ros_node.hpp | 18 +++---- src/cloud_reprojection_ros.cpp | 62 ++++++++++++------------- 3 files changed, 43 insertions(+), 41 deletions(-) diff --git a/config/control_command.yaml b/config/control_command.yaml index 07531e4..03d498c 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -72,8 +72,8 @@ register_keys: overlay_output_topic: "/odin1/combined_image/compressed" overlay_jpeg_quality: 85 - # cloud_reprojection: 4th input = camera JPEG (sensor_msgs/CompressedImage), e.g. /odin1/image/compressed - sync_camera_topic: "/odin1/image/compressed" + # cloud_reprojection: 4th input = raw camera (sensor_msgs/Image bgr8), e.g. /odin1/image + sync_camera_topic: "/odin1/image" sync_topic_prefix: "/odin1/sync" combined_compressed_topic: "/odin1/combined_image/compressed" combined_jpeg_quality: 85 diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index 5eabfbd..dbb0863 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -16,6 +16,7 @@ limitations under the License. #ifdef ROS2 #include #include + #include #include #include #include @@ -50,6 +51,7 @@ public: private: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Odometry = nav_msgs::msg::Odometry; + using Image = sensor_msgs::msg::Image; using CompressedImage = sensor_msgs::msg::CompressedImage; std::string cloud_slam_topic_; @@ -66,9 +68,9 @@ private: message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; message_filters::Subscriber wiwc_sub_; - message_filters::Subscriber image_compressed_sub_; + message_filters::Subscriber image_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; typedef message_filters::Synchronizer Sync; std::shared_ptr sync_; @@ -83,7 +85,7 @@ private: rclcpp::Publisher::SharedPtr sync_cloud_slam_pub_; rclcpp::Publisher::SharedPtr sync_odom_pub_; rclcpp::Publisher::SharedPtr sync_wiwc_pub_; - rclcpp::Publisher::SharedPtr sync_image_pub_; + rclcpp::Publisher::SharedPtr sync_image_pub_; rclcpp::Publisher::SharedPtr overlay_compressed_pub_; // optional if send_overlay_ rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ @@ -94,7 +96,7 @@ private: void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg, const Odometry::ConstSharedPtr& odom_msg, const Odometry::ConstSharedPtr& wiwc_msg, - const CompressedImage::ConstSharedPtr& image_compressed_msg); + const Image::ConstSharedPtr& image_msg); }; #else class CloudReprojectionRosNode @@ -126,10 +128,10 @@ private: message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; message_filters::Subscriber wiwc_sub_; - message_filters::Subscriber image_compressed_sub_; + message_filters::Subscriber image_sub_; typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::CompressedImage> MySyncPolicy; + sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::Image> MySyncPolicy; typedef message_filters::Synchronizer Sync; std::shared_ptr sync_; @@ -137,7 +139,7 @@ private: ros::Publisher sync_cloud_slam_pub_; ros::Publisher sync_odom_pub_; ros::Publisher sync_wiwc_pub_; - ros::Publisher sync_image_pub_; // CompressedImage + ros::Publisher sync_image_pub_; // Image ros::Publisher overlay_compressed_pub_; ros::Publisher combined_pub_; @@ -148,6 +150,6 @@ private: void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const nav_msgs::OdometryConstPtr& odom_msg, const nav_msgs::OdometryConstPtr& wiwc_msg, - const sensor_msgs::CompressedImageConstPtr& image_compressed_msg); + const sensor_msgs::ImageConstPtr& image_msg); }; #endif diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index a8ffc02..83f0f18 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -170,9 +170,9 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op cloud_sub_.subscribe(this, cloud_slam_topic_); odom_sub_.subscribe(this, odometry_topic_); wiwc_sub_.subscribe(this, wiwc_topic_); - image_compressed_sub_.subscribe(this, camera_image_topic_); + image_sub_.subscribe(this, camera_image_topic_); - sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_); + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_); sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); @@ -181,7 +181,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op sync_cloud_slam_pub_ = this->create_publisher(sync_cloud_slam_topic_, 10); sync_odom_pub_ = this->create_publisher(sync_odom_topic_, 10); sync_wiwc_pub_ = this->create_publisher(sync_wiwc_topic_, 10); - sync_image_pub_ = this->create_publisher(sync_image_topic_, 10); + sync_image_pub_ = this->create_publisher(sync_image_topic_, 10); if (send_overlay_) { overlay_compressed_pub_ = @@ -200,7 +200,7 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("cloud_slam_topic", "/odin1/cloud_slam"); this->declare_parameter("odometry_topic", "/odin1/odometry"); this->declare_parameter("wiwc_topic", "/odin1/wiwc"); - this->declare_parameter("register_keys.sync_camera_topic", "/odin1/image/compressed"); + this->declare_parameter("register_keys.sync_camera_topic", "/odin1/image"); this->declare_parameter("register_keys.sync_topic_prefix", "/odin1/sync"); this->declare_parameter("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed"); this->declare_parameter("register_keys.combined_jpeg_quality", 85); @@ -226,7 +226,7 @@ void CloudReprojectionRosNode::loadParameters() sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam"; sync_odom_topic_ = sync_topic_prefix_ + "/odometry"; sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc"; - sync_image_topic_ = sync_topic_prefix_ + "/image/compressed"; + sync_image_topic_ = sync_topic_prefix_ + "/image"; sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed"; // Load camera parameters from calib.yaml file directly @@ -310,9 +310,9 @@ void CloudReprojectionRosNode::syncCallback( const PointCloud2::ConstSharedPtr& cloud_msg, const Odometry::ConstSharedPtr& odom_msg, const Odometry::ConstSharedPtr& wiwc_msg, - const CompressedImage::ConstSharedPtr& image_compressed_msg) + const Image::ConstSharedPtr& image_msg) { - const auto sync_stamp = image_compressed_msg->header.stamp; + const auto sync_stamp = image_msg->header.stamp; PointCloud2 sync_cloud_slam_msg = *cloud_msg; sync_cloud_slam_msg.header.stamp = sync_stamp; @@ -320,7 +320,7 @@ void CloudReprojectionRosNode::syncCallback( sync_odom_msg.header.stamp = sync_stamp; Odometry sync_wiwc_msg = *wiwc_msg; sync_wiwc_msg.header.stamp = sync_stamp; - CompressedImage sync_image_msg = *image_compressed_msg; + Image sync_image_msg = *image_msg; sync_image_msg.header.stamp = sync_stamp; @@ -371,7 +371,7 @@ void CloudReprojectionRosNode::syncCallback( PointCloud2 cloud_cam_msg; pcl::toROSMsg(cloud_cam, cloud_cam_msg); - cloud_cam_msg.header = image_compressed_msg->header; + cloud_cam_msg.header = image_msg->header; cloud_cam_msg.header.stamp = sync_stamp; if (cloud_cam_msg.header.frame_id.empty()) { cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id; @@ -389,11 +389,11 @@ void CloudReprojectionRosNode::syncCallback( cv::Mat cam_bgr; if (send_overlay_ || need_combined) { - cam_bgr = decodeCompressedToBgr( - image_compressed_msg->data.data(), image_compressed_msg->data.size()); - if (cam_bgr.empty()) { - RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)", - image_compressed_msg->format.c_str()); + try { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8"); + cam_bgr = cv_ptr->image; + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what()); return; } } @@ -415,7 +415,7 @@ void CloudReprojectionRosNode::syncCallback( return; } CompressedImage omsg; - omsg.header = image_compressed_msg->header; + omsg.header = image_msg->header; omsg.format = "jpeg"; omsg.data.assign(obuf.begin(), obuf.end()); overlay_compressed_pub_->publish(omsg); @@ -437,7 +437,7 @@ void CloudReprojectionRosNode::syncCallback( } CompressedImage out; - out.header = image_compressed_msg->header; + out.header = image_msg->header; out.format = "jpeg"; out.data.assign(buf.begin(), buf.end()); combined_pub_->publish(out); @@ -534,16 +534,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1); odom_sub_.subscribe(nh_, odometry_topic_, 1); wiwc_sub_.subscribe(nh_, wiwc_topic_, 1); - image_compressed_sub_.subscribe(nh_, camera_image_topic_, 1); + image_sub_.subscribe(nh_, camera_image_topic_, 1); - sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_); + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_); sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4)); sync_cloud_pub_ = nh_.advertise(sync_cloud_topic_, 1); sync_cloud_slam_pub_ = nh_.advertise(sync_cloud_slam_topic_, 1); sync_odom_pub_ = nh_.advertise(sync_odom_topic_, 1); sync_wiwc_pub_ = nh_.advertise(sync_wiwc_topic_, 1); - sync_image_pub_ = nh_.advertise(sync_image_topic_, 1); + sync_image_pub_ = nh_.advertise(sync_image_topic_, 1); if (send_overlay_) { overlay_compressed_pub_ = @@ -561,7 +561,7 @@ void CloudReprojectionRosNode::loadParameters() pnh_.param("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam")); pnh_.param("odometry_topic", odometry_topic_, std::string("/odin1/odometry")); pnh_.param("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc")); - pnh_.param("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed")); + pnh_.param("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image")); pnh_.param("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync")); pnh_.param("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed")); int jq = 85; @@ -581,7 +581,7 @@ void CloudReprojectionRosNode::loadParameters() sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam"; sync_odom_topic_ = sync_topic_prefix_ + "/odometry"; sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc"; - sync_image_topic_ = sync_topic_prefix_ + "/image/compressed"; + sync_image_topic_ = sync_topic_prefix_ + "/image"; sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed"; // Load camera parameters @@ -640,12 +640,12 @@ void CloudReprojectionRosNode::syncCallback( const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const nav_msgs::OdometryConstPtr& odom_msg, const nav_msgs::OdometryConstPtr& wiwc_msg, - const sensor_msgs::CompressedImageConstPtr& image_compressed_msg) + const sensor_msgs::ImageConstPtr& image_msg) { sync_cloud_slam_pub_.publish(cloud_msg); sync_odom_pub_.publish(odom_msg); sync_wiwc_pub_.publish(wiwc_msg); - sync_image_pub_.publish(image_compressed_msg); + sync_image_pub_.publish(image_msg); pcl::PointCloud cloud_odom; pcl::fromROSMsg(*cloud_msg, cloud_odom); @@ -692,7 +692,7 @@ void CloudReprojectionRosNode::syncCallback( sensor_msgs::PointCloud2 cloud_cam_msg; pcl::toROSMsg(cloud_cam, cloud_cam_msg); - cloud_cam_msg.header = image_compressed_msg->header; + cloud_cam_msg.header = image_msg->header; if (cloud_cam_msg.header.frame_id.empty()) { cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id; } @@ -709,11 +709,11 @@ void CloudReprojectionRosNode::syncCallback( cv::Mat cam_bgr; if (send_overlay_ || need_combined) { - cam_bgr = decodeCompressedToBgr( - image_compressed_msg->data.data(), image_compressed_msg->data.size()); - if (cam_bgr.empty()) { - ROS_ERROR("cv::imdecode failed (sync compressed image, format=%s)", - image_compressed_msg->format.c_str()); + try { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8"); + cam_bgr = cv_ptr->image; + } catch (const cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge (sync image): %s", e.what()); return; } } @@ -729,7 +729,7 @@ void CloudReprojectionRosNode::syncCallback( return; } sensor_msgs::CompressedImage omsg; - omsg.header = image_compressed_msg->header; + omsg.header = image_msg->header; omsg.format = "jpeg"; omsg.data.assign(obuf.begin(), obuf.end()); overlay_compressed_pub_.publish(omsg); @@ -751,7 +751,7 @@ void CloudReprojectionRosNode::syncCallback( } sensor_msgs::CompressedImage out; - out.header = image_compressed_msg->header; + out.header = image_msg->header; out.format = "jpeg"; out.data.assign(buf.begin(), buf.end()); combined_pub_.publish(out);