diff --git a/config/control_command.yaml b/config/control_command.yaml index cc6a95f..9104df6 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -6,7 +6,7 @@ register_keys: # 0: use odin internal system time as data time stamp, typical and recommended; # 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users # 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis - use_host_ros_time: 0 # Changed to 0 to use device timestamp without PTP offset correction + use_host_ros_time: 0 # Changed to 0 to use device timestamp without PTP offset correction streamctrl: 1 # 0: off; 1: on @@ -63,15 +63,20 @@ register_keys: # cloud reprojection demo, projects cloud_slam to camera image using odometry # Processed on host device - sendreprojection: 0 # 0: off; 1: on + sendreprojection: 1 # 0: off; 1: on - # image overlay settings - overlays reprojected points on camera image - # Processed on host device - sendoverlay: 0 # 0: off; 1: on - overlay_reprojected_topic: "/odin1/reprojected_image" # reprojected image topic - overlay_camera_topic: "/odin1/image" # camera image topic (undistorted) - overlay_output_topic: "/odin1/overlay_image" # output overlay image topic - overlay_alpha: 0.6 # blend alpha (0.0-1.0, higher = more reproj color) + # Legacy overlay node (optional); cloud_reprojection now does 4-way sync + combined jpeg + sendoverlay: 0 + overlay_reprojected_topic: "/odin1/reprojected_image" + overlay_camera_topic: "/odin1/image" + 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" + sync_topic_prefix: "/odin1/sync" + combined_compressed_topic: "/odin1/combined_image/compressed" + combined_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 ef3c96b..5311566 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -17,22 +17,20 @@ limitations under the License. #include #include #include + #include #include #include #include #include - #include - #include #include #else #include #include #include + #include #include #include #include - #include - #include #include #endif @@ -55,28 +53,46 @@ 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_; std::string odometry_topic_; 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_; message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; message_filters::Subscriber wiwc_sub_; + message_filters::Subscriber image_compressed_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; typedef message_filters::Synchronizer Sync; std::shared_ptr sync_; + std::string sync_cloud_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_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_; std::unique_ptr reprojector_; void loadParameters(); void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg, const Odometry::ConstSharedPtr& odom_msg, - const Odometry::ConstSharedPtr& wiwc_msg); + const Odometry::ConstSharedPtr& wiwc_msg, + const CompressedImage::ConstSharedPtr& image_compressed_msg); }; #else class CloudReprojectionRosNode @@ -90,23 +106,41 @@ private: std::string cloud_slam_topic_; std::string odometry_topic_; 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_; + + std::string sync_cloud_topic_; + std::string sync_odom_topic_; + std::string sync_wiwc_topic_; + std::string sync_image_topic_; message_filters::Subscriber cloud_sub_; message_filters::Subscriber odom_sub_; message_filters::Subscriber wiwc_sub_; + message_filters::Subscriber image_compressed_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::CompressedImage> MySyncPolicy; typedef message_filters::Synchronizer Sync; std::shared_ptr sync_; + ros::Publisher sync_cloud_pub_; + ros::Publisher sync_odom_pub_; + ros::Publisher sync_wiwc_pub_; + ros::Publisher sync_image_pub_; // CompressedImage + ros::Publisher reprojected_image_pub_; + ros::Publisher combined_pub_; std::unique_ptr reprojector_; void loadParameters(); void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const nav_msgs::OdometryConstPtr& odom_msg, - const nav_msgs::OdometryConstPtr& wiwc_msg); + const nav_msgs::OdometryConstPtr& wiwc_msg, + const sensor_msgs::CompressedImageConstPtr& image_compressed_msg); }; #endif diff --git a/include/cloud_reprojector.hpp b/include/cloud_reprojector.hpp index 77939fc..e6c73b5 100644 --- a/include/cloud_reprojector.hpp +++ b/include/cloud_reprojector.hpp @@ -57,6 +57,10 @@ public: cv::Mat reprojectCloud(const pcl::PointCloud& cloud_odom, const OdomPose& odom_pose); + /** Same transform as reprojectCloud, but draw points using camera-frame depth z (grayscale) instead of RGB. */ + cv::Mat reprojectCloudDepth(const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose); + void setPointRadius(int radius) { point_radius_ = radius; } int getPointRadius() const { return point_radius_; } @@ -76,6 +80,7 @@ private: Eigen::Matrix4d odomPoseToMatrix(const OdomPose& odom) const; cv::Mat projectCloudToImage(const pcl::PointCloud& cloud_in_cam) const; + cv::Mat projectCloudToImageDepth(const pcl::PointCloud& cloud_in_cam) const; CameraParams camera_params_; ExtrinsicParams extrinsic_params_; diff --git a/include/image_overlay_node.hpp b/include/image_overlay_node.hpp index 4501b57..ec73abd 100644 --- a/include/image_overlay_node.hpp +++ b/include/image_overlay_node.hpp @@ -16,15 +16,14 @@ limitations under the License. #ifdef ROS2 #include #include + #include #include #include #else #include #include + #include #include - #include - #include - #include #endif #include @@ -39,25 +38,26 @@ public: private: using Image = sensor_msgs::msg::Image; + using CompressedImage = sensor_msgs::msg::CompressedImage; std::string reprojected_topic_; std::string camera_topic_; - std::string overlay_topic_; - double alpha_; // blend alpha for overlay + std::string output_topic_; + int jpeg_quality_; rclcpp::Subscription::SharedPtr reproj_sub_; rclcpp::Subscription::SharedPtr camera_sub_; - rclcpp::Publisher::SharedPtr overlay_pub_; + rclcpp::Publisher::SharedPtr combined_pub_; - // Cache latest images cv::Mat latest_reproj_img_; cv::Mat latest_camera_img_; - std_msgs::msg::Header latest_header_; + std_msgs::msg::Header latest_reproj_header_; + std_msgs::msg::Header latest_camera_header_; std::mutex mutex_; void reprojCallback(const Image::ConstSharedPtr& msg); void cameraCallback(const Image::ConstSharedPtr& msg); - void publishOverlay(); + void publishHcatCompressed(); }; #else #include @@ -71,21 +71,21 @@ private: std::string reprojected_topic_; std::string camera_topic_; - std::string overlay_topic_; - double alpha_; // blend alpha for overlay + std::string output_topic_; + int jpeg_quality_; ros::Subscriber reproj_sub_; ros::Subscriber camera_sub_; - ros::Publisher overlay_pub_; + ros::Publisher combined_pub_; - // Cache latest images cv::Mat latest_reproj_img_; cv::Mat latest_camera_img_; - std_msgs::Header latest_header_; + std_msgs::Header latest_reproj_header_; + std_msgs::Header latest_camera_header_; std::mutex mutex_; void reprojCallback(const sensor_msgs::ImageConstPtr& msg); void cameraCallback(const sensor_msgs::ImageConstPtr& msg); - void publishOverlay(); + void publishHcatCompressed(); }; #endif diff --git a/launch_ROS2/odin1_ros2.launch.py b/launch_ROS2/odin1_ros2.launch.py index 6f51d6b..7f0b645 100644 --- a/launch_ROS2/odin1_ros2.launch.py +++ b/launch_ROS2/odin1_ros2.launch.py @@ -65,17 +65,7 @@ def generate_launch_description(): parameters=[reprojection_params] ) - # Image overlay node - overlays reprojected points on camera image - overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml') - with open(overlay_config_path, 'r') as f: - overlay_params = yaml.safe_load(f) - image_overlay_node = Node( - package='odin_ros_driver', - executable='image_overlay_node', - name='image_overlay_node', - output='screen', - parameters=[overlay_params] - ) + # Combined jpeg is published by cloud_reprojection_ros2_node (left=depth z, right=camera BGR) # Create RViz2 node - loads specified configuration file rviz_node = Node( @@ -93,7 +83,6 @@ def generate_launch_description(): ld.add_action(host_sdk_node) ld.add_action(pcd2depth_node) ld.add_action(cloud_reprojection_node) - ld.add_action(image_overlay_node) - ld.add_action(rviz_node) # Add RViz node + # ld.add_action(rviz_node) # Add RViz node return ld diff --git a/package.xml b/package.xml index 17f851d..b277f0b 100755 --- a/package.xml +++ b/package.xml @@ -2,28 +2,27 @@ odin_ros_driver 0.0.1 - ROS driver for Odin sensor + ROS2 driver for Odin sensor rlk Apache 2.0 - - - catkin - - - roscpp + + ament_cmake + + rclcpp + std_msgs sensor_msgs nav_msgs + geometry_msgs cv_bridge image_transport - - - eigen - opencv - yaml-cpp - - - - catkin - - + pcl_conversions + message_filters + tf2 + tf2_ros + tf2_geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index f928467..a4fcd0b 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -13,10 +13,45 @@ limitations under the License. #include "cloud_reprojection_ros_node.hpp" +#include #include #include #include #include +#include +#include + +#ifdef ROS2 +#include +#endif + +namespace { + +cv::Mat resizeToHeight(const cv::Mat& src, int target_h) +{ + if (src.empty() || target_h <= 0) { + return src.clone(); + } + if (src.rows == target_h) { + return src.clone(); + } + const double scale = static_cast(target_h) / static_cast(src.rows); + cv::Mat out; + cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR); + return out; +} + +/** Decode JPEG/PNG payload from sensor_msgs/CompressedImage to BGR. */ +cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len) +{ + if (!data || len == 0) { + return cv::Mat(); + } + cv::Mat raw(1, static_cast(len), CV_8UC1, const_cast(data)); + return cv::imdecode(raw, cv::IMREAD_COLOR); +} + +} // namespace #ifdef ROS2 #include @@ -60,23 +95,34 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op { loadParameters(); - RCLCPP_INFO_STREAM(this->get_logger(), + RCLCPP_INFO_STREAM(this->get_logger(), "\n cloud_slam_topic: " << cloud_slam_topic_ << "\n odometry_topic: " << odometry_topic_ << "\n wiwc_topic: " << wiwc_topic_ - << "\n reprojected_image_topic: " << reprojected_image_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 combined_compressed_topic: " << combined_compressed_topic_); 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_); - sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_); - sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_); + sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4)); + + sync_cloud_pub_ = this->create_publisher(sync_cloud_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); - RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized successfully"); + RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + depth + combined)"); } void CloudReprojectionRosNode::loadParameters() @@ -86,11 +132,25 @@ void CloudReprojectionRosNode::loadParameters() 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("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); 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(); + 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_)); + + sync_cloud_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"; // Load camera parameters from calib.yaml file directly std::string package_path = get_package_source_directory(); @@ -172,12 +232,14 @@ void CloudReprojectionRosNode::loadParameters() void CloudReprojectionRosNode::syncCallback( const PointCloud2::ConstSharedPtr& cloud_msg, const Odometry::ConstSharedPtr& odom_msg, - const Odometry::ConstSharedPtr& wiwc_msg) + const Odometry::ConstSharedPtr& wiwc_msg, + const CompressedImage::ConstSharedPtr& image_compressed_msg) { - // Debug: print that syncCallback is called - static int sync_count = 0; - // RCLCPP_INFO(this->get_logger(), "=== syncCallback called, count: %d ===", ++sync_count); - + sync_cloud_pub_->publish(*cloud_msg); + sync_odom_pub_->publish(*odom_msg); + sync_wiwc_pub_->publish(*wiwc_msg); + sync_image_pub_->publish(*image_compressed_msg); + pcl::PointCloud cloud_odom; pcl::fromROSMsg(*cloud_msg, cloud_odom); @@ -187,41 +249,16 @@ void CloudReprojectionRosNode::syncCallback( return; } - // Extract real-time extrinsics from WIWC message covariance fields - // pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values) Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity(); Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity(); for (int i = 0; i < 16; ++i) { T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i]; T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i]; } - - // Update extrinsics if valid (not identity matrix) + bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6; bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6; - - // // Debug print to compare with host_sdk_sample values - // static int print_count = 0; - // if (print_count++) { - // // Extract rotation (3x3) and translation (3x1) from T_CL - // Eigen::Matrix3d RCL = T_CL.block<3,3>(0,0); - // Eigen::Vector3d TCL = T_CL.block<3,1>(0,3); - // RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RCL (3x3 rotation from T_CL) ===\n" << RCL); - // RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TCL (3x1 translation from T_CL) ===\n" << TCL.transpose()); - - // // Extract rotation (3x3) and translation (3x1) from T_IL - // Eigen::Matrix3d RIL = T_IL.block<3,3>(0,0); - // Eigen::Vector3d TIL = T_IL.block<3,1>(0,3); - // RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RIL (3x3 rotation from T_IL) ===\n" << RIL); - // RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TIL (3x1 translation from T_IL) ===\n" << TIL.transpose()); - - // RCLCPP_INFO(this->get_logger(), "T_CL_valid: %d, T_IL_valid: %d", T_CL_valid, T_IL_valid); - // if (T_CL_valid && T_IL_valid) { - // Eigen::Matrix4d Tic = CloudReprojector::calculateTic(T_CL, T_IL); - // RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection calculated Tic ===\n" << Tic); - // } - // } - + if (T_CL_valid && T_IL_valid) { reprojector_->updateExtrinsics(T_CL, T_IL); } @@ -239,10 +276,39 @@ void CloudReprojectionRosNode::syncCallback( odom_msg->pose.pose.position.z ); - cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose); + cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); + if (depth_vis.empty()) { + return; + } - auto img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg(); - reprojected_image_pub_.publish(*img_msg); + 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; + } + + 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 ==================== @@ -325,18 +391,28 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_ << "\n odometry_topic: " << odometry_topic_ << "\n wiwc_topic: " << wiwc_topic_ - << "\n reprojected_image_topic: " << reprojected_image_topic_); + << "\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_); 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); - sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_); - sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3)); + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_); + sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4)); + + sync_cloud_pub_ = nh_.advertise(sync_cloud_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); - ROS_INFO("CloudReprojectionRosNode initialized successfully"); + ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)"); } void CloudReprojectionRosNode::loadParameters() @@ -345,6 +421,17 @@ void CloudReprojectionRosNode::loadParameters() 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("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)); + + sync_cloud_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"; // Load camera parameters CloudReprojector::CameraParams cam_params; @@ -401,8 +488,14 @@ void CloudReprojectionRosNode::loadParameters() void CloudReprojectionRosNode::syncCallback( const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const nav_msgs::OdometryConstPtr& odom_msg, - const nav_msgs::OdometryConstPtr& wiwc_msg) + const nav_msgs::OdometryConstPtr& wiwc_msg, + const sensor_msgs::CompressedImageConstPtr& image_compressed_msg) { + sync_cloud_pub_.publish(cloud_msg); + sync_odom_pub_.publish(odom_msg); + sync_wiwc_pub_.publish(wiwc_msg); + sync_image_pub_.publish(image_compressed_msg); + pcl::PointCloud cloud_odom; pcl::fromROSMsg(*cloud_msg, cloud_odom); @@ -412,16 +505,13 @@ void CloudReprojectionRosNode::syncCallback( return; } - // Extract real-time extrinsics from WIWC message covariance fields - // pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values) Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity(); Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity(); for (int i = 0; i < 16; ++i) { T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i]; T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i]; } - - // Update extrinsics if valid (not identity matrix) + bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6; bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6; if (T_CL_valid && T_IL_valid) { @@ -441,10 +531,38 @@ void CloudReprojectionRosNode::syncCallback( odom_msg->pose.pose.position.z ); - cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose); + cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose); + if (depth_vis.empty()) { + return; + } - sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg(); - reprojected_image_pub_.publish(img_msg); + 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; + } + + 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 ==================== diff --git a/src/cloud_reprojector.cpp b/src/cloud_reprojector.cpp index f45d0d3..784c1bf 100644 --- a/src/cloud_reprojector.cpp +++ b/src/cloud_reprojector.cpp @@ -13,6 +13,7 @@ limitations under the License. #include "cloud_reprojector.hpp" #include +#include CloudReprojector::CloudReprojector() { @@ -85,6 +86,25 @@ cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud return projectCloudToImage(cloud_in_cam); } +cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose) +{ + if (!initialized_) + { + return cv::Mat(); + } + + Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose); + Eigen::Matrix4d T_imu_odom = T_odom_imu.inverse(); + Eigen::Matrix4d T_cam_imu = extrinsic_params_.Tic.inverse(); + Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom; + + pcl::PointCloud cloud_in_cam; + pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom); + + return projectCloudToImageDepth(cloud_in_cam); +} + cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud& cloud_in_cam) const { cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3); @@ -124,5 +144,51 @@ cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud& cloud_in_cam) const +{ + cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3); + img.setTo(cv::Scalar(255, 255, 255)); + + float z_min = std::numeric_limits::max(); + float z_max = 0.f; + for (const auto& pt : cloud_in_cam) + { + if (pt.z > 0.01f) + { + z_min = std::min(z_min, static_cast(pt.z)); + z_max = std::max(z_max, static_cast(pt.z)); + } + } + // dynamic z range, only for visualization, not accurate for depth calculation + float z_rng = z_max - z_min; + if (z_rng < 1e-4f) + { + z_rng = 1.f; + } + + for (const auto& pt : cloud_in_cam) + { + if (pt.z <= 0.01) + continue; + + int u_int, v_int; + Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z); + Eigen::Vector2d uv = camera_model_->world2cam(pt_cam); + u_int = static_cast(std::round(uv[0])); + v_int = static_cast(std::round(uv[1])); + + if (u_int >= 0 && u_int < camera_params_.image_width && + v_int >= 0 && v_int < camera_params_.image_height) + { + const uchar g = static_cast( + 255.0f * (static_cast(pt.z) - z_min) / z_rng); + cv::circle(img, cv::Point(u_int, v_int), point_radius_, + cv::Scalar(g, g, g), -1); + } + } + return img; } \ No newline at end of file diff --git a/src/image_overlay_node.cpp b/src/image_overlay_node.cpp index 2e1f4c0..7f2aa82 100644 --- a/src/image_overlay_node.cpp +++ b/src/image_overlay_node.cpp @@ -13,39 +13,59 @@ limitations under the License. #include "image_overlay_node.hpp" +#include + +namespace { + +cv::Mat resizeToHeight(const cv::Mat& src, int target_h) +{ + if (src.empty() || target_h <= 0) { + return src.clone(); + } + if (src.rows == target_h) { + return src.clone(); + } + const double scale = static_cast(target_h) / static_cast(src.rows); + cv::Mat out; + cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR); + return out; +} + +} // namespace + #ifdef ROS2 // ==================== ROS2 Implementation ==================== ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options) : Node("image_overlay_node", options) { - // Read from register_keys (same structure as control_command.yaml) this->declare_parameter("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image"); this->declare_parameter("register_keys.overlay_camera_topic", "/odin1/image/undistorted"); - this->declare_parameter("register_keys.overlay_output_topic", "/odin1/overlay_image"); - this->declare_parameter("register_keys.overlay_alpha", 0.6); + this->declare_parameter("register_keys.overlay_output_topic", "/odin1/combined_image/compressed"); + this->declare_parameter("register_keys.overlay_jpeg_quality", 85); reprojected_topic_ = this->get_parameter("register_keys.overlay_reprojected_topic").as_string(); camera_topic_ = this->get_parameter("register_keys.overlay_camera_topic").as_string(); - overlay_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string(); - alpha_ = this->get_parameter("register_keys.overlay_alpha").as_double(); + output_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string(); + jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int(); + jpeg_quality_ = std::max(1, std::min(100, jpeg_quality_)); - RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s", + RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str()); - RCLCPP_INFO(this->get_logger(), "Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_); + RCLCPP_INFO(this->get_logger(), "Publishing compressed hcat to: %s (jpeg q=%d)", + output_topic_.c_str(), jpeg_quality_); - // Independent subscriptions - no synchronization needed reproj_sub_ = this->create_subscription( reprojected_topic_, 10, std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1)); - + camera_sub_ = this->create_subscription( camera_topic_, 10, std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1)); - overlay_pub_ = this->create_publisher(overlay_topic_, 10); + combined_pub_ = this->create_publisher(output_topic_, 10); - RCLCPP_INFO(this->get_logger(), "ImageOverlayNode initialized (no-sync mode)"); + RCLCPP_INFO(this->get_logger(), "ImageOverlayNode: left=camera, right=reprojected, no-sync mode"); } void ImageOverlayNode::reprojCallback(const Image::ConstSharedPtr& msg) @@ -55,13 +75,13 @@ void ImageOverlayNode::reprojCallback(const Image::ConstSharedPtr& msg) { std::lock_guard lock(mutex_); latest_reproj_img_ = cv_ptr->image.clone(); - latest_header_ = msg->header; + latest_reproj_header_ = msg->header; } } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (reproj): %s", e.what()); return; } - publishOverlay(); + publishHcatCompressed(); } void ImageOverlayNode::cameraCallback(const Image::ConstSharedPtr& msg) @@ -71,19 +91,21 @@ void ImageOverlayNode::cameraCallback(const Image::ConstSharedPtr& msg) { std::lock_guard lock(mutex_); latest_camera_img_ = cv_ptr->image.clone(); + latest_camera_header_ = msg->header; } } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (camera): %s", e.what()); return; } - publishOverlay(); + publishHcatCompressed(); } -void ImageOverlayNode::publishOverlay() +void ImageOverlayNode::publishHcatCompressed() { - cv::Mat reproj_copy, camera_copy; - std_msgs::msg::Header header_copy; - + cv::Mat reproj_copy; + cv::Mat camera_copy; + std_msgs::msg::Header out_header; + { std::lock_guard lock(mutex_); if (latest_reproj_img_.empty() || latest_camera_img_.empty()) { @@ -91,52 +113,37 @@ void ImageOverlayNode::publishOverlay() } reproj_copy = latest_reproj_img_.clone(); camera_copy = latest_camera_img_.clone(); - header_copy = latest_header_; + out_header = latest_camera_header_; } - if (reproj_copy.size() != camera_copy.size()) { - RCLCPP_WARN(this->get_logger(), - "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)", - reproj_copy.cols, reproj_copy.rows, - camera_copy.cols, camera_copy.rows); + const int H = std::max(camera_copy.rows, reproj_copy.rows); + cv::Mat left = resizeToHeight(camera_copy, H); + cv::Mat right = resizeToHeight(reproj_copy, H); + + cv::Mat combined; + cv::hconcat(left, right, combined); + + std::vector buf; + const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, jpeg_quality_}; + if (!cv::imencode(".jpg", combined, buf, enc_params)) { + RCLCPP_ERROR(this->get_logger(), "cv::imencode failed"); return; } - // Create overlay using alpha blending - // Replace white background in reproj with camera image, keep colored points - cv::Mat overlay = camera_copy.clone(); - - // Blend: where reproj has color (non-white), show reproj color semi-transparently - // where reproj is white (background), show camera image - - for (int y = 0; y < reproj_copy.rows; ++y) { - for (int x = 0; x < reproj_copy.cols; ++x) { - cv::Vec3b reproj_pixel = reproj_copy.at(y, x); - // Check if pixel is not white (has point cloud color) - if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) { - // Blend reproj color with camera color - cv::Vec3b cam_pixel = camera_copy.at(y, x); - overlay.at(y, x) = cv::Vec3b( - static_cast(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]), - static_cast(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]), - static_cast(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2]) - ); - } - // else: keep camera image (already in overlay) - } - } - - auto overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg(); - overlay_pub_->publish(*overlay_msg); + CompressedImage out; + out.header = out_header; + out.format = "jpeg"; + out.data.assign(buf.begin(), buf.end()); + combined_pub_->publish(out); } // ==================== ROS2 Main ==================== int main(int argc, char **argv) { rclcpp::init(argc, argv); - + auto node = std::make_shared(); - + rclcpp::spin(node); rclcpp::shutdown(); return 0; @@ -148,22 +155,22 @@ int main(int argc, char **argv) ImageOverlayNode::ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh) : nh_(nh), pnh_(pnh) { - // Read from register_keys (same structure as control_command.yaml) pnh_.param("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image"); pnh_.param("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted"); - pnh_.param("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image"); - pnh_.param("register_keys/overlay_alpha", alpha_, 0.6); + pnh_.param("register_keys/overlay_output_topic", output_topic_, "/odin1/combined_image/compressed"); + int q = 85; + pnh_.param("register_keys/overlay_jpeg_quality", q, 85); + jpeg_quality_ = std::max(1, std::min(100, q)); ROS_INFO("Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str()); - ROS_INFO("Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_); + ROS_INFO("Publishing compressed hcat to: %s (jpeg q=%d)", output_topic_.c_str(), jpeg_quality_); - // Independent subscriptions - no synchronization needed reproj_sub_ = nh_.subscribe(reprojected_topic_, 10, &ImageOverlayNode::reprojCallback, this); camera_sub_ = nh_.subscribe(camera_topic_, 10, &ImageOverlayNode::cameraCallback, this); - overlay_pub_ = nh_.advertise(overlay_topic_, 10); + combined_pub_ = nh_.advertise(output_topic_, 10); - ROS_INFO("ImageOverlayNode initialized (no-sync mode)"); + ROS_INFO("ImageOverlayNode: left=camera, right=reprojected, no-sync mode"); } void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg) @@ -173,13 +180,13 @@ void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg) { std::lock_guard lock(mutex_); latest_reproj_img_ = cv_ptr->image.clone(); - latest_header_ = msg->header; + latest_reproj_header_ = msg->header; } } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception (reproj): %s", e.what()); return; } - publishOverlay(); + publishHcatCompressed(); } void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg) @@ -189,19 +196,21 @@ void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg) { std::lock_guard lock(mutex_); latest_camera_img_ = cv_ptr->image.clone(); + latest_camera_header_ = msg->header; } } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception (camera): %s", e.what()); return; } - publishOverlay(); + publishHcatCompressed(); } -void ImageOverlayNode::publishOverlay() +void ImageOverlayNode::publishHcatCompressed() { - cv::Mat reproj_copy, camera_copy; - std_msgs::Header header_copy; - + cv::Mat reproj_copy; + cv::Mat camera_copy; + std_msgs::Header out_header; + { std::lock_guard lock(mutex_); if (latest_reproj_img_.empty() || latest_camera_img_.empty()) { @@ -209,34 +218,28 @@ void ImageOverlayNode::publishOverlay() } reproj_copy = latest_reproj_img_.clone(); camera_copy = latest_camera_img_.clone(); - header_copy = latest_header_; + out_header = latest_camera_header_; } - if (reproj_copy.size() != camera_copy.size()) { - ROS_WARN_THROTTLE(2, "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)", - reproj_copy.cols, reproj_copy.rows, camera_copy.cols, camera_copy.rows); + const int H = std::max(camera_copy.rows, reproj_copy.rows); + cv::Mat left = resizeToHeight(camera_copy, H); + cv::Mat right = resizeToHeight(reproj_copy, H); + + cv::Mat combined; + cv::hconcat(left, right, combined); + + std::vector buf; + const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, jpeg_quality_}; + if (!cv::imencode(".jpg", combined, buf, enc_params)) { + ROS_ERROR("cv::imencode failed"); return; } - // Create overlay using alpha blending - cv::Mat overlay = camera_copy.clone(); - - for (int y = 0; y < reproj_copy.rows; ++y) { - for (int x = 0; x < reproj_copy.cols; ++x) { - cv::Vec3b reproj_pixel = reproj_copy.at(y, x); - if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) { - cv::Vec3b cam_pixel = camera_copy.at(y, x); - overlay.at(y, x) = cv::Vec3b( - static_cast(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]), - static_cast(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]), - static_cast(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2]) - ); - } - } - } - - sensor_msgs::ImagePtr overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg(); - overlay_pub_.publish(overlay_msg); + sensor_msgs::CompressedImage out; + out.header = out_header; + out.format = "jpeg"; + out.data.assign(buf.begin(), buf.end()); + combined_pub_.publish(out); } // ==================== ROS1 Main ====================