diff --git a/config/control_command.yaml b/config/control_command.yaml index 9104df6..e78cd69 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -77,6 +77,8 @@ register_keys: sync_topic_prefix: "/odin1/sync" combined_compressed_topic: "/odin1/combined_image/compressed" combined_jpeg_quality: 85 + # 0: 不发布 combined 拼接 JPEG，不创建该 publisher（省 CPU/带宽）；1: 发布 depth|camera 拼接图 + send_combined_compressed: 0 # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software. # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/ diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index 5311566..0d8671f 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -63,6 +63,7 @@ private: std::string reprojected_image_topic_; std::string combined_compressed_topic_; int combined_jpeg_quality_; + bool publish_combined_compressed_; message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; @@ -74,17 +75,19 @@ private: std::shared_ptr sync_; std::string sync_cloud_topic_; + std::string sync_cloud_slam_topic_; std::string sync_odom_topic_; std::string sync_wiwc_topic_; std::string sync_image_topic_; rclcpp::Publisher::SharedPtr sync_cloud_pub_; + 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_; image_transport::Publisher reprojected_image_pub_; - rclcpp::Publisher::SharedPtr combined_pub_; + rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ std::unique_ptr reprojector_; @@ -111,8 +114,10 @@ private: std::string reprojected_image_topic_; std::string combined_compressed_topic_; int combined_jpeg_quality_; + bool publish_combined_compressed_; std::string sync_cloud_topic_; + std::string sync_cloud_slam_topic_; std::string sync_odom_topic_; std::string sync_wiwc_topic_; std::string sync_image_topic_; @@ -128,6 +133,7 @@ private: std::shared_ptr sync_; ros::Publisher sync_cloud_pub_; + ros::Publisher sync_cloud_slam_pub_; ros::Publisher sync_odom_pub_; ros::Publisher sync_wiwc_pub_; ros::Publisher sync_image_pub_; // CompressedImage diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 71268ee..0a90886 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -102,7 +102,8 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op << "\n camera_image_topic: " << camera_image_topic_ << "\n sync_* topics under: " << sync_topic_prefix_ << "\n reprojected_image_topic (depth z grayscale): " << reprojected_image_topic_ - << "\n combined_compressed_topic: " << combined_compressed_topic_); + << "\n combined_compressed_topic: " << combined_compressed_topic_ + << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); cloud_sub_.subscribe(this, cloud_slam_topic_); odom_sub_.subscribe(this, odometry_topic_); @@ -115,12 +116,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op std::placeholders::_4)); sync_cloud_pub_ = this->create_publisher(sync_cloud_topic_, 10); + 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); reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_); - combined_pub_ = this->create_publisher(combined_compressed_topic_, 10); + if (publish_combined_compressed_) { + combined_pub_ = this->create_publisher(combined_compressed_topic_, 10); + } RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + depth + combined)"); } @@ -136,6 +140,7 @@ void CloudReprojectionRosNode::loadParameters() 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); + this->declare_parameter("register_keys.send_combined_compressed", 1); cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string(); odometry_topic_ = this->get_parameter("odometry_topic").as_string(); @@ -146,8 +151,11 @@ void CloudReprojectionRosNode::loadParameters() combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string(); combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int(); combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_)); + publish_combined_compressed_ = + (this->get_parameter("register_keys.send_combined_compressed").as_int() != 0); sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam"; + 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"; @@ -235,6 +243,7 @@ void CloudReprojectionRosNode::syncCallback( const Odometry::ConstSharedPtr& wiwc_msg, const CompressedImage::ConstSharedPtr& image_compressed_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); @@ -299,31 +308,34 @@ void CloudReprojectionRosNode::syncCallback( auto depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg(); reprojected_image_pub_.publish(*depth_msg); - cv::Mat 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()); - return; + if (combined_pub_) { + cv::Mat 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()); + return; + } + + const int H = std::max(depth_vis.rows, cam_bgr.rows); + cv::Mat left = resizeToHeight(depth_vis, H); + cv::Mat right = resizeToHeight(cam_bgr, H); + cv::Mat combined; + cv::hconcat(left, right, combined); + + std::vector buf; + const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_}; + if (!cv::imencode(".jpg", combined, buf, enc_params)) { + RCLCPP_ERROR(this->get_logger(), "cv::imencode failed"); + return; + } + + CompressedImage out; + out.header = image_compressed_msg->header; + out.format = "jpeg"; + out.data.assign(buf.begin(), buf.end()); + combined_pub_->publish(out); } - - const int H = std::max(depth_vis.rows, cam_bgr.rows); - cv::Mat left = resizeToHeight(depth_vis, H); - cv::Mat right = resizeToHeight(cam_bgr, H); - cv::Mat combined; - cv::hconcat(left, right, combined); - - std::vector buf; - const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_}; - if (!cv::imencode(".jpg", combined, buf, enc_params)) { - RCLCPP_ERROR(this->get_logger(), "cv::imencode failed"); - return; - } - - CompressedImage out; - out.header = image_compressed_msg->header; - out.format = "jpeg"; - out.data.assign(buf.begin(), buf.end()); - combined_pub_->publish(out); } // ==================== ROS2 Main ==================== @@ -409,7 +421,8 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod << "\n camera_image_topic: " << camera_image_topic_ << "\n sync_prefix: " << sync_topic_prefix_ << "\n reprojected_image_topic (depth z): " << reprojected_image_topic_ - << "\n combined_compressed_topic: " << combined_compressed_topic_); + << "\n combined_compressed_topic: " << combined_compressed_topic_ + << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1); odom_sub_.subscribe(nh_, odometry_topic_, 1); @@ -420,12 +433,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod 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); reprojected_image_pub_ = nh_.advertise(reprojected_image_topic_, 1); - combined_pub_ = nh_.advertise(combined_compressed_topic_, 1); + if (publish_combined_compressed_) { + combined_pub_ = nh_.advertise(combined_compressed_topic_, 1); + } ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)"); } @@ -442,8 +458,12 @@ void CloudReprojectionRosNode::loadParameters() int jq = 85; pnh_.param("register_keys/combined_jpeg_quality", jq, 85); combined_jpeg_quality_ = std::max(1, std::min(100, jq)); + int send_combined = 1; + pnh_.param("register_keys/send_combined_compressed", send_combined, 1); + publish_combined_compressed_ = (send_combined != 0); sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam"; + 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"; @@ -506,6 +526,7 @@ void CloudReprojectionRosNode::syncCallback( const nav_msgs::OdometryConstPtr& wiwc_msg, const sensor_msgs::CompressedImageConstPtr& image_compressed_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); @@ -569,30 +590,34 @@ void CloudReprojectionRosNode::syncCallback( sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg(); reprojected_image_pub_.publish(depth_msg); - cv::Mat 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()); - return; + if (publish_combined_compressed_) { + cv::Mat 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()); + return; + } + + const int H = std::max(depth_vis.rows, cam_bgr.rows); + cv::Mat left = resizeToHeight(depth_vis, H); + cv::Mat right = resizeToHeight(cam_bgr, H); + cv::Mat combined; + cv::hconcat(left, right, combined); + + std::vector buf; + const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_}; + if (!cv::imencode(".jpg", combined, buf, enc_params)) { + ROS_ERROR("cv::imencode failed"); + return; + } + + sensor_msgs::CompressedImage out; + out.header = image_compressed_msg->header; + out.format = "jpeg"; + out.data.assign(buf.begin(), buf.end()); + combined_pub_.publish(out); } - - const int H = std::max(depth_vis.rows, cam_bgr.rows); - cv::Mat left = resizeToHeight(depth_vis, H); - cv::Mat right = resizeToHeight(cam_bgr, H); - cv::Mat combined; - cv::hconcat(left, right, combined); - - std::vector buf; - const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_}; - if (!cv::imencode(".jpg", combined, buf, enc_params)) { - ROS_ERROR("cv::imencode failed"); - return; - } - - sensor_msgs::CompressedImage out; - out.header = image_compressed_msg->header; - out.format = "jpeg"; - out.data.assign(buf.begin(), buf.end()); - combined_pub_.publish(out); } // ==================== ROS1 Main ====================