change into compressed image to publish overlay

CMakeLists.txt

@@ -341,7 +341,6 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         sensor_msgs
         nav_msgs
         cv_bridge
-        image_transport
         pcl_conversions
         message_filters
     )

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}/

include/cloud_reprojection_ros_node.hpp

@@ -16,10 +16,8 @@ limitations under the License.
 #ifdef ROS2
     #include <rclcpp/rclcpp.hpp>
     #include <sensor_msgs/msg/point_cloud2.hpp>
-    #include <sensor_msgs/msg/image.hpp>
     #include <sensor_msgs/msg/compressed_image.hpp>
     #include <nav_msgs/msg/odometry.hpp>
-    #include <image_transport/image_transport.hpp>
     #include <cv_bridge/cv_bridge.h>
     #include <message_filters/subscriber.h>
     #include <message_filters/sync_policies/approximate_time.h>
@@ -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<PointCloud2> cloud_sub_;
     message_filters::Subscriber<Odometry> odom_sub_;
@@ -86,7 +85,7 @@ private:
     rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
     rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_pub_;
 
-    image_transport::Publisher overlay_image_pub_;
+    rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_;  // optional if send_overlay_
     rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;  // optional if publish_combined_compressed_
 
     std::unique_ptr<CloudReprojector> 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<CloudReprojector> reprojector_;

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

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<Odometry>(sync_wiwc_topic_, 10);
     sync_image_pub_ = this->create_publisher<CompressedImage>(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<CompressedImage>(sync_overlay_image_topic_, 10);
+    }
     if (publish_combined_compressed_) {
         combined_pub_ = this->create_publisher<CompressedImage>(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<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
     this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
     this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
-    this->declare_parameter<std::string>("overlay_image_topic", "/odin1/overlay_img");
     this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image/compressed");
     this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
     this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
     this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
     this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
+    this->declare_parameter<int>("register_keys.send_overlay", 1);
+    this->declare_parameter<int>("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<pcl::PointXYZRGB> 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 (send_overlay_ && overlay_compressed_pub_) {
-    if (depth_vis.empty()) {
+        cv::Mat overlay_vis = overlayProjectedCloudOnImage(
-        return;
+            cam_bgr, cloud_cam, reprojector_->getCameraParams());
+        if (!overlay_vis.empty()) {
+            std::vector<uchar> obuf;
+            const std::vector<int> 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<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
     sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);
 
-    overlay_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_overlay_image_topic_, 1);
+    if (send_overlay_) {
+        overlay_compressed_pub_ =
+            nh_.advertise<sensor_msgs::CompressedImage>(sync_overlay_image_topic_, 1);
+    }
     if (publish_combined_compressed_) {
         combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
     }
@@ -531,22 +561,28 @@ void CloudReprojectionRosNode::loadParameters()
     pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
     pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
     pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
-    pnh_.param<std::string>("overlay_image_topic", sync_overlay_image_topic_, std::string("/odin1/reprojected_image"));
     pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
     pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
     pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
     int jq = 85;
     pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
     combined_jpeg_quality_ = std::max(1, std::min(100, jq));
+    int ojq = 85;
+    pnh_.param<int>("register_keys/overlay_jpeg_quality", ojq, 85);
+    overlay_jpeg_quality_ = std::max(1, std::min(100, ojq));
     int send_combined = 1;
     pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
     publish_combined_compressed_ = (send_combined != 0);
+    int send_ov = 1;
+    pnh_.param<int>("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);
+    const bool need_combined = publish_combined_compressed_;
-    if (depth_vis.empty()) {
+    cv::Mat depth_vis;
-        return;
+    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();
+    cv::Mat cam_bgr;
-    overlay_image_pub_.publish(depth_msg);
+    if (send_overlay_ || need_combined) {
-
+        cam_bgr = decodeCompressedToBgr(
-    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;
         }
+    }
 
+    if (send_overlay_) {
+        cv::Mat overlay_vis =
+            overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams());
+        if (!overlay_vis.empty()) {
+            std::vector<uchar> obuf;
+            const std::vector<int> 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);