From 78b8d371ea451d93f51acda8fedf6ff20b69cbb8 Mon Sep 17 00:00:00 2001 From: jingyang-huang <1178065793@qq.com> Date: Thu, 9 Apr 2026 15:51:52 +0800 Subject: [PATCH] change into compressed image to publish overlay --- CMakeLists.txt | 1 - config/control_command.yaml | 3 + include/cloud_reprojection_ros_node.hpp | 11 +- script/ros1_build_ros.sh | 128 +++++++++++++++++++++ src/cloud_reprojection_ros.cpp | 142 +++++++++++++++++------- 5 files changed, 237 insertions(+), 48 deletions(-) create mode 100755 script/ros1_build_ros.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 4bf049f..16cfec0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -341,7 +341,6 @@ elseif(ROS_VERSION STREQUAL "ROS2") sensor_msgs nav_msgs cv_bridge - image_transport pcl_conversions message_filters ) diff --git a/config/control_command.yaml b/config/control_command.yaml index e256fda..07531e4 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -79,6 +79,9 @@ register_keys: combined_jpeg_quality: 85 # 0: 不发布 combined 拼接 JPEG，不创建该 publisher（省 CPU/带宽）；1: 发布 depth|camera 拼接图 send_combined_compressed: 0 + # 0: 不发布 overlay（不创建 publisher）；1: 发布 sensor_msgs/CompressedImage JPEG，话题 {sync_topic_prefix}/overlay_img/compressed + send_overlay: 1 + overlay_jpeg_quality: 85 # 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 8b44e6f..5eabfbd 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -16,10 +16,8 @@ limitations under the License. #ifdef ROS2 #include #include - #include #include #include - #include #include #include #include @@ -52,7 +50,6 @@ 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_; @@ -62,7 +59,9 @@ private: std::string sync_topic_prefix_; std::string combined_compressed_topic_; int combined_jpeg_quality_; + int overlay_jpeg_quality_; bool publish_combined_compressed_; + bool send_overlay_; message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; @@ -86,7 +85,7 @@ private: rclcpp::Publisher::SharedPtr sync_wiwc_pub_; rclcpp::Publisher::SharedPtr sync_image_pub_; - image_transport::Publisher overlay_image_pub_; + rclcpp::Publisher::SharedPtr overlay_compressed_pub_; // optional if send_overlay_ rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ std::unique_ptr reprojector_; @@ -113,7 +112,9 @@ private: std::string sync_topic_prefix_; std::string combined_compressed_topic_; int combined_jpeg_quality_; + int overlay_jpeg_quality_; bool publish_combined_compressed_; + bool send_overlay_; std::string sync_cloud_topic_; std::string sync_cloud_slam_topic_; @@ -138,7 +139,7 @@ private: ros::Publisher sync_wiwc_pub_; ros::Publisher sync_image_pub_; // CompressedImage - ros::Publisher overlay_image_pub_; + ros::Publisher overlay_compressed_pub_; ros::Publisher combined_pub_; std::unique_ptr reprojector_; diff --git a/script/ros1_build_ros.sh b/script/ros1_build_ros.sh new file mode 100755 index 0000000..28218ad --- /dev/null +++ b/script/ros1_build_ros.sh @@ -0,0 +1,128 @@ +#!/bin/bash + +# Get the directory where the script is located (Odin_ROS_Driver directory) +PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)" +# Calculate the workspace root directory (contains devel, build, src) +WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")" +# Workspace source directory (contains all packages) +WORKSPACE_SRC="${WORKSPACE_ROOT}/src" +PROJECT_NAME="odin_ros_driver" + +# Define color codes +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' + +# Clean workspace function +clean_workspace() { + echo -e "${YELLOW}Cleaning build directories${NC}" + + # Clean build artifacts in workspace + rm -rf "${WORKSPACE_ROOT}/build" + rm -rf "${WORKSPACE_ROOT}/install" + rm -rf "${WORKSPACE_ROOT}/log" + rm -rf "${WORKSPACE_ROOT}/devel" + + echo -e "${GREEN}Cleanup complete${NC}" +} + +# Run node function +run_node() { + echo -e "${YELLOW}Running ROS1 node${NC}" + + # Check if environment file exists + if [ ! -f "${WORKSPACE_ROOT}/devel/setup.bash" ]; then + echo -e "${RED}Could not find devel/setup.bash, please build the project with ./build_ros1.sh first${NC}" + return 1 + fi + + # Source environment and run node + source "${WORKSPACE_ROOT}/devel/setup.bash" + +} + +# Build workspace function +build_workspace() { + echo -e "${YELLOW}Workspace structure:${NC}" + echo " Workspace root: ${WORKSPACE_ROOT}" + echo " Source directory: ${WORKSPACE_SRC}" + echo " Package directory: ${PKG_DIR}" + echo " ROS version: ROS1" + + echo -e "${YELLOW}Starting ROS1 project build...${NC}" + + # Clean + cd $WS_DIR + rm -rf build devel install + + # Ensure ROS1 environment is loaded + if [ -f "/opt/ros/noetic/setup.bash" ]; then + source "/opt/ros/noetic/setup.bash" + elif [ -f "/opt/ros/melodic/setup.bash" ]; then + source "/opt/ros/melodic/setup.bash" + else + echo -e "${RED}Could not find ROS1 setup.bash file. Please ensure ROS1 is installed.${NC}" + return 1 + fi + + # Create temporary package.xml + if [ -f "${PKG_DIR}/package_ros1.xml" ]; then + echo "Creating temporary package.xml (using package_ros1.xml)" + cp "${PKG_DIR}/package_ros1.xml" "${PKG_DIR}/package.xml" + TEMP_PACKAGE=true + elif [ -f "${PKG_DIR}/package.xml" ]; then + echo "Using existing package.xml" + else + echo -e "${RED}Could not find package.xml in package directory${NC}" + return 1 + fi + + # Set build system variable + export BUILD_SYSTEM=ROS1 + + # Switch to workspace root and build + cd "${WORKSPACE_ROOT}" || return 1 + catkin_make -DBUILD_SYSTEM=ROS1 -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j$(nproc) + BUILD_RESULT=$? + + # If build successful, source environment + if [[ $BUILD_RESULT -eq 0 ]]; then + echo -e "${GREEN}ROS1 build successful, loading environment: source devel/setup.bash${NC}" + source "${WORKSPACE_ROOT}/devel/setup.bash" + else + echo -e "${RED}ROS1 build failed, please check error logs${NC}" + fi + + +} + +# Help function +show_help() { + echo -e "${YELLOW}Usage:${NC}" + echo " ./build_ros.sh # Build project" + echo " ./build_ros.sh -c # Clean build artifacts" + echo " ./build_ros.sh -h # Show help information" + echo "" + echo -e "${YELLOW}Current configuration:${NC}" + echo " Project name: ${PROJECT_NAME}" + echo " Package directory: ${PKG_DIR}" + echo " Workspace root: ${WORKSPACE_ROOT}" + echo " Source directory: ${WORKSPACE_SRC}" +} + +# Main +case "$1" in + -c|--clean) + clean_workspace + ;; + -r|--run) + run_node + ;; + -h|--help) + show_help + ;; + *) + build_workspace + ;; +esac \ No newline at end of file diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 6565d18..a8ffc02 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -162,9 +162,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op << "\n wiwc_topic: " << wiwc_topic_ << "\n camera_image_topic: " << camera_image_topic_ << "\n sync_* topics under: " << sync_topic_prefix_ - << "\n overlay_img_topic: " << sync_overlay_image_topic_ + << "\n overlay_compressed_topic: " << sync_overlay_image_topic_ << "\n combined_compressed_topic: " << combined_compressed_topic_ - << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); + << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off") + << "\n send_overlay: " << (send_overlay_ ? "on" : "off")); cloud_sub_.subscribe(this, cloud_slam_topic_); odom_sub_.subscribe(this, odometry_topic_); @@ -182,12 +183,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op sync_wiwc_pub_ = this->create_publisher(sync_wiwc_topic_, 10); sync_image_pub_ = this->create_publisher(sync_image_topic_, 10); - overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_); + if (send_overlay_) { + overlay_compressed_pub_ = + this->create_publisher(sync_overlay_image_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 + overlay + combined)"); + RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)"); } void CloudReprojectionRosNode::loadParameters() @@ -196,31 +200,34 @@ 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("overlay_image_topic", "/odin1/overlay_img"); this->declare_parameter("register_keys.sync_camera_topic", "/odin1/image/compressed"); 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); + this->declare_parameter("register_keys.send_overlay", 1); + this->declare_parameter("register_keys.overlay_jpeg_quality", 85); cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string(); odometry_topic_ = this->get_parameter("odometry_topic").as_string(); wiwc_topic_ = this->get_parameter("wiwc_topic").as_string(); - // sync_overlay_image_topic_ = this->get_parameter("overlay_image_topic").as_string(); camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string(); sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string(); 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_)); + overlay_jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int(); + overlay_jpeg_quality_ = std::max(1, std::min(100, overlay_jpeg_quality_)); publish_combined_compressed_ = (this->get_parameter("register_keys.send_combined_compressed").as_int() != 0); + send_overlay_ = (this->get_parameter("register_keys.send_overlay").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"; - sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img"; + sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed"; // Load camera parameters from calib.yaml file directly std::string package_path = get_package_source_directory(); @@ -316,9 +323,7 @@ void CloudReprojectionRosNode::syncCallback( CompressedImage sync_image_msg = *image_compressed_msg; sync_image_msg.header.stamp = sync_stamp; - sync_cloud_slam_pub_->publish(sync_cloud_slam_msg); - sync_odom_pub_->publish(sync_odom_msg); - sync_wiwc_pub_->publish(sync_wiwc_msg); + pcl::PointCloud cloud_odom; pcl::fromROSMsg(*cloud_msg, cloud_odom); @@ -371,31 +376,52 @@ void CloudReprojectionRosNode::syncCallback( if (cloud_cam_msg.header.frame_id.empty()) { cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id; } + + + const bool need_combined = (combined_pub_ != nullptr); + cv::Mat depth_vis; + if (need_combined) { + depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); + if (depth_vis.empty()) { + return; + } + } + + 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()); + return; + } + } + + sync_cloud_slam_pub_->publish(sync_cloud_slam_msg); + sync_odom_pub_->publish(sync_odom_msg); + sync_wiwc_pub_->publish(sync_wiwc_msg); sync_cloud_pub_->publish(cloud_cam_msg); sync_image_pub_->publish(sync_image_msg); - cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); - if (depth_vis.empty()) { - return; + if (send_overlay_ && overlay_compressed_pub_) { + cv::Mat overlay_vis = overlayProjectedCloudOnImage( + cam_bgr, cloud_cam, reprojector_->getCameraParams()); + if (!overlay_vis.empty()) { + std::vector obuf; + const std::vector oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_}; + if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) { + RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)"); + return; + } + CompressedImage omsg; + omsg.header = image_compressed_msg->header; + omsg.format = "jpeg"; + omsg.data.assign(obuf.begin(), obuf.end()); + overlay_compressed_pub_->publish(omsg); + } } - 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; - } - - cv::Mat overlay_vis = overlayProjectedCloudOnImage( - cam_bgr, cloud_cam, reprojector_->getCameraParams()); - if (overlay_vis.empty()) { - return; - } - - auto overlay_msg = cv_bridge::CvImage(image_compressed_msg->header, "bgr8", overlay_vis).toImageMsg(); - overlay_image_pub_.publish(*overlay_msg); - if (combined_pub_) { const int H = std::max(depth_vis.rows, cam_bgr.rows); cv::Mat left = resizeToHeight(depth_vis, H); @@ -500,9 +526,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod << "\n wiwc_topic: " << wiwc_topic_ << "\n camera_image_topic: " << camera_image_topic_ << "\n sync_prefix: " << sync_topic_prefix_ - << "\n overlay_image_topic (depth z): " << sync_overlay_image_topic_ + << "\n overlay_compressed_topic: " << sync_overlay_image_topic_ << "\n combined_compressed_topic: " << combined_compressed_topic_ - << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); + << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off") + << "\n send_overlay: " << (send_overlay_ ? "on" : "off")); cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1); odom_sub_.subscribe(nh_, odometry_topic_, 1); @@ -518,7 +545,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod sync_wiwc_pub_ = nh_.advertise(sync_wiwc_topic_, 1); sync_image_pub_ = nh_.advertise(sync_image_topic_, 1); - overlay_image_pub_ = nh_.advertise(sync_overlay_image_topic_, 1); + if (send_overlay_) { + overlay_compressed_pub_ = + nh_.advertise(sync_overlay_image_topic_, 1); + } if (publish_combined_compressed_) { combined_pub_ = nh_.advertise(combined_compressed_topic_, 1); } @@ -531,22 +561,28 @@ 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("overlay_image_topic", sync_overlay_image_topic_, std::string("/odin1/reprojected_image")); pnh_.param("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed")); 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; pnh_.param("register_keys/combined_jpeg_quality", jq, 85); combined_jpeg_quality_ = std::max(1, std::min(100, jq)); + int ojq = 85; + pnh_.param("register_keys/overlay_jpeg_quality", ojq, 85); + overlay_jpeg_quality_ = std::max(1, std::min(100, ojq)); int send_combined = 1; pnh_.param("register_keys/send_combined_compressed", send_combined, 1); publish_combined_compressed_ = (send_combined != 0); + int send_ov = 1; + pnh_.param("register_keys/send_overlay", send_ov, 1); + send_overlay_ = (send_ov != 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"; + sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed"; // Load camera parameters CloudReprojector::CameraParams cam_params; @@ -662,23 +698,45 @@ void CloudReprojectionRosNode::syncCallback( } sync_cloud_pub_.publish(cloud_cam_msg); - cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); - if (depth_vis.empty()) { - return; + const bool need_combined = publish_combined_compressed_; + cv::Mat depth_vis; + if (need_combined) { + depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); + if (depth_vis.empty()) { + return; + } } - sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg(); - overlay_image_pub_.publish(depth_msg); - - if (publish_combined_compressed_) { - cv::Mat cam_bgr = decodeCompressedToBgr( + 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()); return; } + } + if (send_overlay_) { + cv::Mat overlay_vis = + overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams()); + if (!overlay_vis.empty()) { + std::vector obuf; + const std::vector oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_}; + if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) { + ROS_ERROR("cv::imencode failed (overlay)"); + return; + } + sensor_msgs::CompressedImage omsg; + omsg.header = image_compressed_msg->header; + omsg.format = "jpeg"; + omsg.data.assign(obuf.begin(), obuf.end()); + overlay_compressed_pub_.publish(omsg); + } + } + + if (publish_combined_compressed_) { 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);