Robotics StackExchange | Archived questions

Approximate time synchronizer, callback is not called

Hi,

My approximate time synchronizer with two topics (Camera and Liar) works fine. But When I add one more topic to it, my callback function seems never get called..

I am attaching the codes below for help. First node is the time synchronizer and second node is additional topic that I want to synchronize with other two topics (camera and lidar).

Thank you in advance.

ros::Publisher image1_pub;
ros::Publisher image2_pub;
ros::Publisher image3_pub;

void callback(const sensor_msgs::Image::ConstPtr& image1, const sensor_msgs::PointCloud2::ConstPtr& image2, const sensor_msgs::PointCloud2::ConstPtr& image3)
{

ROS_INFO("Sync_Callback");

  image1_pub.publish(image1);

  image2_pub.publish(image2);

  image3_pub.publish(image3);

}

int main(int argc, char** argv)
{

 ros::init(argc, argv, "TimeSynchronizer");

  std::string zed1_img_topic = "/zed/right/image_rect_color";

  std::string zed2_img_topic = "/velodyne_points";

  std::string zed3_img_topic = "/chatter";

  ros::NodeHandle nh;

  message_filters::Subscriber<sensor_msgs::Image> image1_sub(nh, zed1_img_topic, 1);

  message_filters::Subscriber<sensor_msgs::PointCloud2> image2_sub(nh, zed2_img_topic, 1);

  message_filters::Subscriber<sensor_msgs::PointCloud2> image3_sub(nh, zed3_img_topic, 1);

  image1_pub = nh.advertise<sensor_msgs::Image>("/synchronized" + zed1_img_topic, 1000);

  image2_pub = nh.advertise<sensor_msgs::PointCloud2>("/synchronized" + zed2_img_topic, 1000);

  image3_pub = nh.advertise<sensor_msgs::PointCloud2>("/synchronized" + zed3_img_topic, 1000);

  typedef sync_policies::ApproximateTime<Image, PointCloud2, PointCloud2> MySyncPolicy;

  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub, image3_sub);

  sync.registerCallback(boost::bind(&callback, _1, _2, _3));

  ros::spin();

  return 0;

}

Second node

include "ros/ros.h"

include "std_msgs/String.h"

include

include

include

int main (int argc, char **argv){

ros::init(argc, argv, "talker");

ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<sensor_msgs::PointCloud2>("chatter", 1000);

    ros::Rate loop_rate(10); 

int speed = 100;

    while (ros::ok()){

            sensor_msgs::PointCloud2 msg;

    msg.height = speed;

            chatter_pub.publish(msg);

            loop_rate.sleep();

    }

return 0;

}

Asked by roskinetic on 2018-10-01 19:59:48 UTC

Comments

Can you post the output of rostopic hz /chatter to confirm that the topic is being published at the correct rate. Also there seems to be some code missing from the second node source code. The msg identified has not been declared anywhere, can you post the full source code please.

Asked by PeteBlackerThe3rd on 2018-10-02 07:03:18 UTC

Thank you for your comments. I have edited the second node. It was missing "sensor_msgs:PointCoud2 msg;". The following is the output of rostopic hz /chatter and it looks the topic is being published at the correct rate. subscribed to [/chatter] average rate: 10.000 min: 0.100s max: 0.100s std dev:

Asked by roskinetic on 2018-10-02 08:10:58 UTC

The output of rostopic hz /lidar and rostopic hz /camera look like : subscribed to [/velodyne_points] average rate: 9.897 min: 0.100s max: 0.104s std dev: 0.00112s window: 9 subscribed to [/zed/right/image_rect_color] average rate: 30.435 min: 0.020s max: 0.037s std dev: 0.00271s window: 29

Asked by roskinetic on 2018-10-02 08:19:34 UTC

Hi, I met the same problem with Approximate time synchronizer, did you get the solution? Thanks in advance.

Asked by aihanbing on 2019-01-07 23:20:27 UTC

Answers

For anybody still looking for a solution, this is one way to achieve this using a depth camera as an example and you subscribe to both the color image topic and the camera_info topic:

#include <ros/ros.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h> 
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

class ImageConverter
{
    ros::NodeHandle nh_;
    message_filters::Subscriber<sensor_msgs::Image> image_sub_;
    message_filters::Subscriber<sensor_msgs::CameraInfo> camera_info_sub_;
    typedef message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> approx_sync_policy;
    std::shared_ptr<approx_sync_policy> sync;

public:
ImageConverter(ros::NodeHandle &n)
:   nh_(n)
{
    image_sub_.subscribe(nh_, "/camera/color/image_raw", 1);
    camera_info_sub_.subscribe(nh_, "/camera/color/camera_info", 1);
    sync.reset(new approx_sync_policy(image_sub_, camera_info_sub_, 10));
    sync->registerCallback(std::bind(&ImageConverter::MultiSubscriberCB, this, std::placeholders::_1, std::placeholders::_2)); 
}

~ImageConverter() {}

private:
void MultiSubscriberCB(const sensor_msgs::ImageConstPtr& msg_image, const sensor_msgs::CameraInfoConstPtr& msg_camera_info) {
    ROS_INFO("inside callback");
}
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_converter");
    ros::NodeHandle nh("~");
    ImageConverter ic(nh);

    ros::spin();
    return 0;
}

Asked by nixie on 2021-07-16 15:36:54 UTC

Comments