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

change into compressed image to publish overlay

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 15:51:52 +08:00
parent cdfe249111
commit 78b8d371ea
5 changed files with 237 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cloud_reprojection_ros.cpp
+100 -42
View File
@@ -162,9 +162,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_* topics under: " << sync_topic_prefix_
<< "\n overlay_img_topic: " << sync_overlay_image_topic_
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
@@ -182,12 +183,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<CompressedImage>(sync_image_topic_, 10);
overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_);
if (send_overlay_) {
overlay_compressed_pub_ =
this->create_publisher<CompressedImage>(sync_overlay_image_topic_, 10);
}
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + overlay + combined)");
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
@@ -196,31 +200,34 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
this->declare_parameter<std::string>("overlay_image_topic", "/odin1/overlay_img");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image/compressed");
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
this->declare_parameter<int>("register_keys.send_overlay", 1);
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
// sync_overlay_image_topic_ = this->get_parameter("overlay_image_topic").as_string();
camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_));
overlay_jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int();
overlay_jpeg_quality_ = std::max(1, std::min(100, overlay_jpeg_quality_));
publish_combined_compressed_ =
(this->get_parameter("register_keys.send_combined_compressed").as_int() != 0);
send_overlay_ = (this->get_parameter("register_keys.send_overlay").as_int() != 0);
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
@@ -316,9 +323,7 @@ void CloudReprojectionRosNode::syncCallback(
CompressedImage sync_image_msg = *image_compressed_msg;
sync_image_msg.header.stamp = sync_stamp;
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
sync_odom_pub_->publish(sync_odom_msg);
sync_wiwc_pub_->publish(sync_wiwc_msg);
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -371,31 +376,52 @@ void CloudReprojectionRosNode::syncCallback(
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
const bool need_combined = (combined_pub_ != nullptr);
cv::Mat depth_vis;
if (need_combined) {
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
}
}
cv::Mat cam_bgr;
if (send_overlay_ || need_combined) {
cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
}
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
sync_odom_pub_->publish(sync_odom_msg);
sync_wiwc_pub_->publish(sync_wiwc_msg);
sync_cloud_pub_->publish(cloud_cam_msg);
sync_image_pub_->publish(sync_image_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
if (send_overlay_ && overlay_compressed_pub_) {
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)");
return;
}
CompressedImage omsg;
omsg.header = image_compressed_msg->header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_->publish(omsg);
}
}
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (overlay_vis.empty()) {
return;
}
auto overlay_msg = cv_bridge::CvImage(image_compressed_msg->header, "bgr8", overlay_vis).toImageMsg();
overlay_image_pub_.publish(*overlay_msg);
if (combined_pub_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
@@ -500,9 +526,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_prefix: " << sync_topic_prefix_
<< "\n overlay_image_topic (depth z): " << sync_overlay_image_topic_
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
odom_sub_.subscribe(nh_, odometry_topic_, 1);
@@ -518,7 +545,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);
overlay_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_overlay_image_topic_, 1);
if (send_overlay_) {
overlay_compressed_pub_ =
nh_.advertise<sensor_msgs::CompressedImage>(sync_overlay_image_topic_, 1);
}
if (publish_combined_compressed_) {
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
}
@@ -531,22 +561,28 @@ void CloudReprojectionRosNode::loadParameters()
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
pnh_.param<std::string>("overlay_image_topic", sync_overlay_image_topic_, std::string("/odin1/reprojected_image"));
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
int jq = 85;
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
int ojq = 85;
pnh_.param<int>("register_keys/overlay_jpeg_quality", ojq, 85);
overlay_jpeg_quality_ = std::max(1, std::min(100, ojq));
int send_combined = 1;
pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
publish_combined_compressed_ = (send_combined != 0);
int send_ov = 1;
pnh_.param<int>("register_keys/send_overlay", send_ov, 1);
send_overlay_ = (send_ov != 0);
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters
CloudReprojector::CameraParams cam_params;
@@ -662,23 +698,45 @@ void CloudReprojectionRosNode::syncCallback(
}
sync_cloud_pub_.publish(cloud_cam_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
const bool need_combined = publish_combined_compressed_;
cv::Mat depth_vis;
if (need_combined) {
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
}
}
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
overlay_image_pub_.publish(depth_msg);
if (publish_combined_compressed_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
cv::Mat cam_bgr;
if (send_overlay_ || need_combined) {
cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
ROS_ERROR("cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
}
if (send_overlay_) {
cv::Mat overlay_vis =
overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
ROS_ERROR("cv::imencode failed (overlay)");
return;
}
sensor_msgs::CompressedImage omsg;
omsg.header = image_compressed_msg->header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_.publish(omsg);
}
}
if (publish_combined_compressed_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);