How to synchronize 6 topics

asked 2022-09-19 06:01:24 -0500

We give 6 topics to sensorsync.cpp code directly. But This code prints only this 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)

edit retag flag offensive close merge delete

Comments

Why do you use registerCallback twice? Why can't publish the data from first callback?

ravijoshi gravatar image ravijoshi  ( 2022-09-19 23:26:56 -0500 )edit

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?

ljaniec gravatar image ljaniec  ( 2022-09-20 06:38:31 -0500 )edit