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

add flag for combined_img; enable send sync cloud slam

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 14:17:10 +08:00
parent e325ea577f
commit 77fac3f793
3 changed files with 85 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cloud_reprojection_ros.cpp
+76 -51
View File
@@ -102,7 +102,8 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\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_);
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
@@ -115,12 +116,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
std::placeholders::_4));
sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_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);
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
}
@@ -136,6 +140,7 @@ void CloudReprojectionRosNode::loadParameters()
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);
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
@@ -146,8 +151,11 @@ void CloudReprojectionRosNode::loadParameters()
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_));
publish_combined_compressed_ =
(this->get_parameter("register_keys.send_combined_compressed").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";
@@ -235,6 +243,7 @@ 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);
@@ -299,31 +308,34 @@ void CloudReprojectionRosNode::syncCallback(
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;
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);
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);
}
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 ====================
@@ -409,7 +421,8 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
<< "\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_);
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
odom_sub_.subscribe(nh_, odometry_topic_, 1);
@@ -420,12 +433,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4));
sync_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_topic_, 1);
sync_cloud_slam_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_slam_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);
if (publish_combined_compressed_) {
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
}
ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
}
@@ -442,8 +458,12 @@ void CloudReprojectionRosNode::loadParameters()
int jq = 85;
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
int send_combined = 1;
pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
publish_combined_compressed_ = (send_combined != 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";
@@ -506,6 +526,7 @@ void CloudReprojectionRosNode::syncCallback(
const nav_msgs::OdometryConstPtr& wiwc_msg,
const sensor_msgs::CompressedImageConstPtr& 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);
@@ -569,30 +590,34 @@ void CloudReprojectionRosNode::syncCallback(
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;
if (publish_combined_compressed_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
ROS_ERROR("cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
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);
}
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 ====================