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
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 ====================