From cdfe249111861c44f377ac8daca810da955918eb Mon Sep 17 00:00:00 2001 From: jingyang-huang <1178065793@qq.com> Date: Thu, 9 Apr 2026 15:51:26 +0800 Subject: [PATCH] add overlay image on c++ for debug; unify synced sensor output --- config/control_command.yaml | 1 + include/cloud_reprojection_ros_node.hpp | 8 +- script/build_ros.sh | 128 ------------------------ src/cloud_reprojection_ros.cpp | 126 ++++++++++++++++++----- src/image_overlay_node.cpp | 2 +- 5 files changed, 109 insertions(+), 156 deletions(-) delete mode 100755 script/build_ros.sh diff --git a/config/control_command.yaml b/config/control_command.yaml index e78cd69..e256fda 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -101,6 +101,7 @@ register_keys: relocalization_map_abs_path: "" # must be set for Relocalization mode or will fail # To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1" + save_map: 0 mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/ mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index 0d8671f..8b44e6f 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -60,7 +60,6 @@ private: std::string wiwc_topic_; std::string camera_image_topic_; std::string sync_topic_prefix_; - std::string reprojected_image_topic_; std::string combined_compressed_topic_; int combined_jpeg_quality_; bool publish_combined_compressed_; @@ -79,6 +78,7 @@ private: std::string sync_odom_topic_; std::string sync_wiwc_topic_; std::string sync_image_topic_; + std::string sync_overlay_image_topic_; rclcpp::Publisher::SharedPtr sync_cloud_pub_; rclcpp::Publisher::SharedPtr sync_cloud_slam_pub_; @@ -86,7 +86,7 @@ private: rclcpp::Publisher::SharedPtr sync_wiwc_pub_; rclcpp::Publisher::SharedPtr sync_image_pub_; - image_transport::Publisher reprojected_image_pub_; + image_transport::Publisher overlay_image_pub_; rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ std::unique_ptr reprojector_; @@ -111,7 +111,6 @@ private: std::string wiwc_topic_; std::string camera_image_topic_; std::string sync_topic_prefix_; - std::string reprojected_image_topic_; std::string combined_compressed_topic_; int combined_jpeg_quality_; bool publish_combined_compressed_; @@ -121,6 +120,7 @@ private: std::string sync_odom_topic_; std::string sync_wiwc_topic_; std::string sync_image_topic_; + std::string sync_overlay_image_topic_; message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; @@ -138,7 +138,7 @@ private: ros::Publisher sync_wiwc_pub_; ros::Publisher sync_image_pub_; // CompressedImage - ros::Publisher reprojected_image_pub_; + ros::Publisher overlay_image_pub_; ros::Publisher combined_pub_; std::unique_ptr reprojector_; diff --git a/script/build_ros.sh b/script/build_ros.sh deleted file mode 100755 index 28218ad..0000000 --- a/script/build_ros.sh +++ /dev/null @@ -1,128 +0,0 @@ -#!/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 0a90886..6565d18 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -51,6 +51,67 @@ cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len) return cv::imdecode(raw, cv::IMREAD_COLOR); } +cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr, + const pcl::PointCloud& cloud_in_cam, + const CloudReprojector::CameraParams& cam_params, + int point_radius = 2, + float min_depth = 0.5f, + float max_depth = 30.0f) +{ + if (camera_bgr.empty()) { + return cv::Mat(); + } + + cv::Mat overlay = camera_bgr.clone(); + mini_vikit::PolynomialCamera camera_model( + cam_params.image_width, cam_params.image_height, + cam_params.A11, cam_params.A22, + cam_params.u0, cam_params.v0, + cam_params.A12, + cam_params.k2, cam_params.k3, cam_params.k4, + cam_params.k5, cam_params.k6, cam_params.k7); + + std::vector pixels; + std::vector depths; + pixels.reserve(cloud_in_cam.size()); + depths.reserve(cloud_in_cam.size()); + + for (const auto& pt : cloud_in_cam) { + if (pt.z <= min_depth || pt.z >= max_depth) { + continue; + } + + const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z)); + const int u = static_cast(std::round(uv.x())); + const int v = static_cast(std::round(uv.y())); + if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) { + continue; + } + + pixels.emplace_back(u, v); + depths.emplace_back(static_cast(pt.z)); + } + + if (pixels.empty()) { + return overlay; + } + + cv::Mat depth_values(static_cast(depths.size()), 1, CV_32F, depths.data()); + cv::Mat clipped, norm_u8, colors; + cv::min(cv::max(depth_values, min_depth), max_depth, clipped); + clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f)); + clipped.convertTo(norm_u8, CV_8U); + cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET); + + for (int i = 0; i < colors.rows; ++i) { + const cv::Vec3b color = colors.at(i, 0); + cv::circle(overlay, pixels[static_cast(i)], point_radius, + cv::Scalar(color[0], color[1], color[2]), -1); + } + + return overlay; +} + } // namespace #ifdef ROS2 @@ -101,7 +162,7 @@ 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 reprojected_image_topic (depth z grayscale): " << reprojected_image_topic_ + << "\n overlay_img_topic: " << sync_overlay_image_topic_ << "\n combined_compressed_topic: " << combined_compressed_topic_ << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); @@ -121,12 +182,12 @@ 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); - reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_); + overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_); 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)"); + RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + overlay + combined)"); } void CloudReprojectionRosNode::loadParameters() @@ -135,7 +196,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("reprojected_image_topic", "/odin1/reprojected_image"); + 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"); @@ -145,7 +206,7 @@ void CloudReprojectionRosNode::loadParameters() 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(); - reprojected_image_topic_ = this->get_parameter("reprojected_image_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(); @@ -159,6 +220,7 @@ void CloudReprojectionRosNode::loadParameters() 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"; // Load camera parameters from calib.yaml file directly std::string package_path = get_package_source_directory(); @@ -243,10 +305,20 @@ 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); + const auto sync_stamp = image_compressed_msg->header.stamp; + + PointCloud2 sync_cloud_slam_msg = *cloud_msg; + sync_cloud_slam_msg.header.stamp = sync_stamp; + Odometry sync_odom_msg = *odom_msg; + 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; + 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); @@ -295,28 +367,36 @@ 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.stamp = sync_stamp; if (cloud_cam_msg.header.frame_id.empty()) { cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id; } 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; } - 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; + } + + 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_) { - 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); @@ -420,7 +500,7 @@ 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 reprojected_image_topic (depth z): " << reprojected_image_topic_ + << "\n overlay_image_topic (depth z): " << sync_overlay_image_topic_ << "\n combined_compressed_topic: " << combined_compressed_topic_ << "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")); @@ -438,7 +518,7 @@ 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); - reprojected_image_pub_ = nh_.advertise(reprojected_image_topic_, 1); + overlay_image_pub_ = nh_.advertise(sync_overlay_image_topic_, 1); if (publish_combined_compressed_) { combined_pub_ = nh_.advertise(combined_compressed_topic_, 1); } @@ -451,7 +531,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("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image")); + 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")); @@ -588,7 +668,7 @@ void CloudReprojectionRosNode::syncCallback( } sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg(); - reprojected_image_pub_.publish(depth_msg); + overlay_image_pub_.publish(depth_msg); if (publish_combined_compressed_) { cv::Mat cam_bgr = decodeCompressedToBgr( diff --git a/src/image_overlay_node.cpp b/src/image_overlay_node.cpp index 7f2aa82..9400656 100644 --- a/src/image_overlay_node.cpp +++ b/src/image_overlay_node.cpp @@ -39,7 +39,7 @@ cv::Mat resizeToHeight(const cv::Mat& src, int target_h) ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options) : Node("image_overlay_node", options) { - this->declare_parameter("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image"); + this->declare_parameter("register_keys.overlay_reprojected_topic", "/odin1/overlay_img"); this->declare_parameter("register_keys.overlay_camera_topic", "/odin1/image/undistorted"); this->declare_parameter("register_keys.overlay_output_topic", "/odin1/combined_image/compressed"); this->declare_parameter("register_keys.overlay_jpeg_quality", 85);