ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

message_filters::sync::ApproximateTime with variable number of topics

asked 2020-04-22 11:15:27 -0500

kastaa gravatar image

Hi,

I am still new to ROS. I am trying to approximately synchronize n cameras where n is given at run-time. I saw some examples showing how to do this with n cameras when n is given at build time. For example, given that I have a ROS node with two cameras publishing on ROS topic camera/image/cam0 and camera/image/cam1, the code below would do what I want.

void Callback(const sensor_msgs::ImageConstPtr& left_msg, const sensor_msgs::ImageConstPtr& right_msg)
{
    ROS_INFO("Callback");
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "Sync_publisher_node");
    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> left_image(nh, "camera/image/cam0", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_image(nh, "camera/image/cam1", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
    message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), left_image, right_image);

    ROS_INFO("Created Synchronizer");
    sync.registerCallback(boost::bind(Callback, _1, _2));
    ROS_INFO("Registered callback");
    ros::spin();

    return 0;
}

What I would like to do is something like this:

void syncCallback(const sensor_msgs::ImageConstPtr& left_msg, ...)
{
     ROS_INFO("Callback");
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "Sync_publisher_node");
    ros::NodeHandle nh;
    uint8_t n_cam = atoi(argv[1]);

    std::vector<message_filters::Subscriber<sensor_msgs::Image>> img_filter;
    for (size_t i=0; i < n_cam; i++)
    {
        img_filter.push_back(message_filters::Subscriber<sensor_msgs::Image>(nh, "camera/image/cam" + std::to_string(i), 1));
    }

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, <...>> SyncPolicy;
    message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), left_image, <...>);

    ROS_INFO("Created Synchronizer");
    sync.registerCallback(boost::bind(&syncCallback, <...>));
    ROS_INFO("Registered callback");
    ros::spin();

    return 0;

}

So I am wondering is there a way to dynamically construct the message_filters synchronizer? I saw the chain filter to dynamically add multiple filters together, but I am not sure how to use it with the synchronizer.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-06-24 09:11:19 -0500

bpiekny gravatar image

Did you manage to solve it? Looks like there is no such option in the ROS to synchronize a variable number of messages. I think that this sentence prevents from that: "Once each topic-specific queue contains at least one message, we find the latest message among the heads of the queues, that's the pivot."

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-04-22 11:15:27 -0500

Seen: 417 times

Last updated: Apr 22 '20