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