How to synchronize 6 topics
We give 6 topics to sensorsync.cpp code directly. But This code prints only this [ WARN] [1663583390.233119254]: Messages of type 0 arrived out of order (will print only once) [ WARN] [1663583391.690687003]: Messages of type 5 arrived out of order (will print only once) [ WARN] [1663583393.224967495]: Messages of type 2 arrived out of order (will print only once) [ WARN] [1663583394.067357683]: Messages of type 3 arrived out of order (will print only once) [ WARN] [1663583394.582947448]: Messages of type 1 arrived out of order (will print only once)
how to solve this?
void sync_callback(const PointCloud2::ConstPtr& lidar, const ImageConstPtr& image, const can_stdConstPtr &can, const ImageConstPtr& FOV, const GazePointConstPtr& gaze, const NavSatFixConstPtr &xsense)
{
ROS_INFO("Time synced");
}
void second_callback(const PointCloud2::ConstPtr& lidar, const ImageConstPtr& image, const can_stdConstPtr &can, const ImageConstPtr& FOV, const GazePointConstPtr& gaze, const NavSatFixConstPtr &xsense)
{
lidar_pub.publish(lidar);
camera_pub.publish(image);
can_pub.publish(can);
fov_pub.publish(FOV);
gaze_pub.publish(gaze);
xsense_pub.publish(xsense);
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"sensorsync");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
//message filter를 통해 subscribe 해주는 과정
message_filters::Subscriber<PointCloud2> lidar_sub(nh,"/livox/lidar",1);
message_filters::Subscriber<Image> image_sub(nh,"/zed/zed_node/rgb/image_rect_color",1);
message_filters::Subscriber<can_std> can_sub(nh,"/msg_k",1);
message_filters::Subscriber<Image> fov_sub(nh,"/FOV",1);
message_filters::Subscriber<GazePoint> gaze_sub(nh,"/gaze",1);
message_filters::Subscriber<NavSatFix> xsense_sub(nh,"/gnss",1);
lidar_pub = nh.advertise<sensor_msgs::PointCloud2>("lidar_sub2",1);
camera_pub = it.advertise("camera_sub2",1);
can_pub = nh.advertise<kaaican::can_std> ("can_sub2",1);
fov_pub = it.advertise("fov_sub2",1);
gaze_pub = nh.advertise<tracker::GazePoint> ("gaze_sub2",1);
xsense_pub = nh.advertise<sensor_msgs::NavSatFix> ("xsense_sub2",1);
typedef sync_policies::ApproximateTime<PointCloud2,Image,can_std,Image,GazePoint,NavSatFix> MySyncPolicy;
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10),lidar_sub, image_sub, can_sub, fov_sub, gaze_sub, xsense_sub);
sync.registerCallback(boost::bind(&sync_callback,_1,_2,_3,_4,_5,_6));
sync.registerCallback(boost::bind(&second_callback,_1,_2,_3,_4,_5,_6));
ros::spin();
return 0;
This is my sensorsync code(cpp)
Asked by woongchan on 2022-09-19 06:01:24 UTC
Comments
Why do you use
registerCallback
twice? Why can't publish the data from first callback?Asked by ravijoshi on 2022-09-19 23:26:56 UTC
A bit more information about your setup would be useful as well - I see a ZED camera node, FOV (Field Of View?), lidars and other nodes for geolocalization? (NavSatFix). Do you want to have every topic message synchronized?
Asked by ljaniec on 2022-09-20 06:38:31 UTC