wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update cloud reprojection node to sync image+cloud, looks sync good

Browse Source
This commit is contained in:
jingyang-huang
2026-04-06 12:18:55 +08:00
parent f54d29f40a
commit 0ec1300ab5
9 changed files with 424 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+14 -9
View File
@@ -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}/
include/cloud_reprojection_ros_node.hpp
+42 -8
View File
@@ -17,22 +17,20 @@ limitations under the License.
#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/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#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<PointCloud2> cloud_sub_;
message_filters::Subscriber<Odometry> odom_sub_;
message_filters::Subscriber<Odometry> wiwc_sub_;
message_filters::Subscriber<CompressedImage> image_compressed_sub_;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, CompressedImage> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
std::string sync_cloud_topic_;
std::string sync_odom_topic_;
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_pub_;
rclcpp::Publisher<Odometry>::SharedPtr sync_odom_pub_;
rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_pub_;
image_transport::Publisher reprojected_image_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;
std::unique_ptr<CloudReprojector> 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<sensor_msgs::PointCloud2> cloud_sub_;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
message_filters::Subscriber<sensor_msgs::CompressedImage> image_compressed_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::CompressedImage> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> 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<CloudReprojector> 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
include/cloud_reprojector.hpp
+5
View File
@@ -57,6 +57,10 @@ public:
cv::Mat reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& 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<pcl::PointXYZRGB>& 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<pcl::PointXYZRGB>& cloud_in_cam) const;
cv::Mat projectCloudToImageDepth(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const;
CameraParams camera_params_;
ExtrinsicParams extrinsic_params_;
include/image_overlay_node.hpp
+15 -15
View File
@@ -16,15 +16,14 @@ limitations under the License.
#ifdef ROS2
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <mutex>
#else
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#endif
#include <opencv2/opencv.hpp>
@@ -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<Image>::SharedPtr reproj_sub_;
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
rclcpp::Publisher<CompressedImage>::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 <mutex>
@@ -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
launch_ROS2/odin1_ros2.launch.py
+2 -13
View File
@@ -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
package.xml
+17 -18
View File
@@ -2,28 +2,27 @@
<package format="3">
<name>odin_ros_driver</name>
<version>0.0.1</version>
<description>ROS driver for Odin sensor</description>
<description>ROS2 driver for Odin sensor</description>
<maintainer email="[email protected]">rlk</maintainer>
<license>Apache 2.0</license>
<!-- ROS1 uses catkin as the build tool -->
<buildtool_depend>catkin</buildtool_depend>
<!-- ROS1 dependencies -->
<depend>roscpp</depend>
<!-- ROS2 uses colcon as the build tool -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- ROS2 dependencies -->
<depend>rclcpp</depend>
<!-- System dependencies -->
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<!-- System dependencies -->
<depend>eigen</depend>
<depend>opencv</depend>
<depend>yaml-cpp</depend>
<!-- Specify build type as catkin -->
<export>
<build_type>catkin</build_type>
</export>
</package>
<depend>pcl_conversions</depend>
<depend>message_filters</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<!-- Specify build type as ament -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/cloud_reprojection_ros.cpp
+171 -53
View File
@@ -13,10 +13,45 @@ limitations under the License.
#include "cloud_reprojection_ros_node.hpp"
#include <algorithm>
#include <fstream>
#include <sys/stat.h>
#include <thread>
#include <chrono>
#include <vector>
#include <cstdint>
#ifdef ROS2
#include <sensor_msgs/msg/compressed_image.hpp>
#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<double>(target_h) / static_cast<double>(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<int>(len), CV_8UC1, const_cast<uint8_t*>(data));
return cv::imdecode(raw, cv::IMREAD_COLOR);
}
} // namespace
#ifdef ROS2
#include <functional>
@@ -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<Sync>(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<Sync>(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<PointCloud2>(sync_cloud_topic_, 10);
sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<CompressedImage>(sync_image_topic_, 10);
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
combined_pub_ = this->create_publisher<CompressedImage>(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<std::string>("odometry_topic", "/odin1/odometry");
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
this->declare_parameter<std::string>("reprojected_image_topic", "/odin1/reprojected_image");
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);
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<pcl::PointXYZRGB> 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<uchar> buf;
const std::vector<int> 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<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3));
sync_ = std::make_shared<Sync>(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<sensor_msgs::PointCloud2>(sync_cloud_topic_, 1);
sync_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_odom_topic_, 1);
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(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<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>("reprojected_image_topic", reprojected_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));
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<pcl::PointXYZRGB> 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<uchar> buf;
const std::vector<int> 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 ====================
src/cloud_reprojector.cpp
+66
View File
@@ -13,6 +13,7 @@ limitations under the License.
#include "cloud_reprojector.hpp"
#include <cmath>
#include <limits>
CloudReprojector::CloudReprojector()
{
@@ -85,6 +86,25 @@ cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>
return projectCloudToImage(cloud_in_cam);
}
cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloud<pcl::PointXYZRGB>& 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB>& 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<pcl::PointXY
}
}
return img;
}
cv::Mat CloudReprojector::projectCloudToImageDepth(const pcl::PointCloud<pcl::PointXYZRGB>& 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<float>::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<float>(pt.z));
z_max = std::max(z_max, static_cast<float>(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<int>(std::round(uv[0]));
v_int = static_cast<int>(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<uchar>(
255.0f * (static_cast<float>(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;
}
src/image_overlay_node.cpp
+92 -89
View File
@@ -13,39 +13,59 @@ limitations under the License.
#include "image_overlay_node.hpp"
#include <algorithm>
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<double>(target_h) / static_cast<double>(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<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/overlay_image");
this->declare_parameter<double>("register_keys.overlay_alpha", 0.6);
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/combined_image/compressed");
this->declare_parameter<int>("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<Image>(
reprojected_topic_, 10,
std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1));
camera_sub_ = this->create_subscription<Image>(
camera_topic_, 10,
std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1));
overlay_pub_ = this->create_publisher<sensor_msgs::msg::Image>(overlay_topic_, 10);
combined_pub_ = this->create_publisher<CompressedImage>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<uchar> buf;
const std::vector<int> 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<cv::Vec3b>(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<cv::Vec3b>(y, x);
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
static_cast<uchar>(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<ImageOverlayNode>();
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<std::string>("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image");
pnh_.param<std::string>("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted");
pnh_.param<std::string>("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image");
pnh_.param<double>("register_keys/overlay_alpha", alpha_, 0.6);
pnh_.param<std::string>("register_keys/overlay_output_topic", output_topic_, "/odin1/combined_image/compressed");
int q = 85;
pnh_.param<int>("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<sensor_msgs::Image>(overlay_topic_, 10);
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<uchar> buf;
const std::vector<int> 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<cv::Vec3b>(y, x);
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
static_cast<uchar>(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 ====================