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

change into raw image sync, better for online inference

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 16:53:50 +08:00
parent 78b8d371ea
commit 3659ffd110
3 changed files with 43 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+2 -2
View File
@@ -72,8 +72,8 @@ register_keys:
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"
# cloud_reprojection: 4th input = raw camera (sensor_msgs/Image bgr8), e.g. /odin1/image
sync_camera_topic: "/odin1/image"
sync_topic_prefix: "/odin1/sync"
combined_compressed_topic: "/odin1/combined_image/compressed"
combined_jpeg_quality: 85
include/cloud_reprojection_ros_node.hpp
+10 -8
View File
@@ -16,6 +16,7 @@ limitations under the License.
#ifdef ROS2
#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 <cv_bridge/cv_bridge.h>
@@ -50,6 +51,7 @@ public:
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_;
@@ -66,9 +68,9 @@ private:
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_;
message_filters::Subscriber<Image> image_sub_;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, CompressedImage> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, Image> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -83,7 +85,7 @@ private:
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_slam_pub_;
rclcpp::Publisher<Odometry>::SharedPtr sync_odom_pub_;
rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_pub_;
rclcpp::Publisher<Image>::SharedPtr sync_image_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_; // optional if send_overlay_
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
@@ -94,7 +96,7 @@ private:
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_msg);
const Image::ConstSharedPtr& image_msg);
};
#else
class CloudReprojectionRosNode
@@ -126,10 +128,10 @@ private:
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_;
message_filters::Subscriber<sensor_msgs::Image> image_sub_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::CompressedImage> MySyncPolicy;
sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry, sensor_msgs::Image> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -137,7 +139,7 @@ private:
ros::Publisher sync_cloud_slam_pub_;
ros::Publisher sync_odom_pub_;
ros::Publisher sync_wiwc_pub_;
ros::Publisher sync_image_pub_; // CompressedImage
ros::Publisher sync_image_pub_; // Image
ros::Publisher overlay_compressed_pub_;
ros::Publisher combined_pub_;
@@ -148,6 +150,6 @@ private:
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg,
const nav_msgs::OdometryConstPtr& wiwc_msg,
const sensor_msgs::CompressedImageConstPtr& image_compressed_msg);
const sensor_msgs::ImageConstPtr& image_msg);
};
#endif
src/cloud_reprojection_ros.cpp
+31 -31
View File
@@ -170,9 +170,9 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
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_);
image_sub_.subscribe(this, camera_image_topic_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4));
@@ -181,7 +181,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
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);
sync_image_pub_ = this->create_publisher<Image>(sync_image_topic_, 10);
if (send_overlay_) {
overlay_compressed_pub_ =
@@ -200,7 +200,7 @@ 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>("register_keys.sync_camera_topic", "/odin1/image/compressed");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image");
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);
@@ -226,7 +226,7 @@ void CloudReprojectionRosNode::loadParameters()
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_image_topic_ = sync_topic_prefix_ + "/image";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters from calib.yaml file directly
@@ -310,9 +310,9 @@ void CloudReprojectionRosNode::syncCallback(
const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_msg)
const Image::ConstSharedPtr& image_msg)
{
const auto sync_stamp = image_compressed_msg->header.stamp;
const auto sync_stamp = image_msg->header.stamp;
PointCloud2 sync_cloud_slam_msg = *cloud_msg;
sync_cloud_slam_msg.header.stamp = sync_stamp;
@@ -320,7 +320,7 @@ void CloudReprojectionRosNode::syncCallback(
sync_odom_msg.header.stamp = sync_stamp;
Odometry sync_wiwc_msg = *wiwc_msg;
sync_wiwc_msg.header.stamp = sync_stamp;
CompressedImage sync_image_msg = *image_compressed_msg;
Image sync_image_msg = *image_msg;
sync_image_msg.header.stamp = sync_stamp;
@@ -371,7 +371,7 @@ void CloudReprojectionRosNode::syncCallback(
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
cloud_cam_msg.header = image_msg->header;
cloud_cam_msg.header.stamp = sync_stamp;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
@@ -389,11 +389,11 @@ void CloudReprojectionRosNode::syncCallback(
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());
try {
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
cam_bgr = cv_ptr->image;
} catch (const cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what());
return;
}
}
@@ -415,7 +415,7 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
CompressedImage omsg;
omsg.header = image_compressed_msg->header;
omsg.header = image_msg->header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_->publish(omsg);
@@ -437,7 +437,7 @@ void CloudReprojectionRosNode::syncCallback(
}
CompressedImage out;
out.header = image_compressed_msg->header;
out.header = image_msg->header;
out.format = "jpeg";
out.data.assign(buf.begin(), buf.end());
combined_pub_->publish(out);
@@ -534,16 +534,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
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);
image_sub_.subscribe(nh_, camera_image_topic_, 1);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_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_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);
sync_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_image_topic_, 1);
if (send_overlay_) {
overlay_compressed_pub_ =
@@ -561,7 +561,7 @@ 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>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image"));
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;
@@ -581,7 +581,7 @@ void CloudReprojectionRosNode::loadParameters()
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_image_topic_ = sync_topic_prefix_ + "/image";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters
@@ -640,12 +640,12 @@ void CloudReprojectionRosNode::syncCallback(
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg,
const nav_msgs::OdometryConstPtr& wiwc_msg,
const sensor_msgs::CompressedImageConstPtr& image_compressed_msg)
const sensor_msgs::ImageConstPtr& image_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);
sync_image_pub_.publish(image_msg);
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -692,7 +692,7 @@ void CloudReprojectionRosNode::syncCallback(
sensor_msgs::PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
cloud_cam_msg.header = image_msg->header;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
@@ -709,11 +709,11 @@ void CloudReprojectionRosNode::syncCallback(
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());
try {
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
cam_bgr = cv_ptr->image;
} catch (const cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge (sync image): %s", e.what());
return;
}
}
@@ -729,7 +729,7 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
sensor_msgs::CompressedImage omsg;
omsg.header = image_compressed_msg->header;
omsg.header = image_msg->header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_.publish(omsg);
@@ -751,7 +751,7 @@ void CloudReprojectionRosNode::syncCallback(
}
sensor_msgs::CompressedImage out;
out.header = image_compressed_msg->header;
out.header = image_msg->header;
out.format = "jpeg";
out.data.assign(buf.begin(), buf.end());
combined_pub_.publish(out);