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

Using message_filters::Synchronizer inside class

asked 2022-09-26 08:41:37 -0600

Janebek gravatar image

updated 2022-09-26 10:24:24 -0600

ravijoshi gravatar image

Hi all!

Thanks to @ravijoshi,I have solved this one topic receiving problem,the link about that is here #q406915

But when I try to receive two topics at the same time, I don't know how to put the function message_filters::Synchronizer in my class member. The code is shown below.

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(mNh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(mNh, "/camera/depth/image", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ClientHandler::GrabRGBD,this,_1,_2));

Does anyone know how to solve this problem?

Please help me, thank you so much!

edit retag flag offensive close merge delete

Comments

Hi @ravijoshi, can you please help me on this? Thank you so much!

Janebek gravatar image Janebek  ( 2022-09-26 08:50:55 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
2

answered 2022-09-26 10:22:15 -0600

ravijoshi gravatar image

updated 2022-09-26 10:23:56 -0600

Generally, I would like to see some progress or efforts instead of answering directly because this is how we learn new things by exploring and trying. Anyway, below is the code which is extended from the previous answer. In order to test it, I used image and camera info. So please change the camera info to depth accordingly.

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>

#include <memory>

using namespace sensor_msgs;
using namespace message_filters;

class ClientHandler
{
public:
  ClientHandler(ros::NodeHandle& nh, std::string& rgb_topic, std::string& cam_info_topic);
  void MyCallback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info);

private:
  using MySyncPolicy = sync_policies::ApproximateTime<Image, CameraInfo>;

  ros::NodeHandle _nh;
  message_filters::Subscriber<Image> _rgb_sub;
  message_filters::Subscriber<CameraInfo> _camera_info_sub;
  std::shared_ptr<message_filters::Synchronizer<MySyncPolicy> > _sync;
};

ClientHandler::ClientHandler(ros::NodeHandle& nh, std::string& rgb_topic, std::string& cam_info_topic) : _nh(nh)
{
  _rgb_sub.subscribe(_nh, rgb_topic, 1);
  _camera_info_sub.subscribe(_nh, cam_info_topic, 1);

  _sync = std::make_shared<message_filters::Synchronizer<MySyncPolicy> >(10);
  _sync->connectInput(_rgb_sub, _camera_info_sub);
  _sync->registerCallback(boost::bind(&ClientHandler::MyCallback, this, _1, _2));
}

void ClientHandler::MyCallback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  ROS_INFO("Sequence: [%d] [%d]", image->header.seq, cam_info->header.seq);
}

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

  std::string rgbTopic = "/usb_cam/image_raw";
  std::string camInfoTopic = "/usb_cam/camera_info";
  ClientHandler ClientHandler(n, rgbTopic, camInfoTopic);

  ros::spin();
  return 0;
}

I used the usb_cam package to publish my webcam and used the above node to subscribe, as shown below:

$ rosrun usb_cam usb_cam_node
$ rosrun beginner_tutorials listener_class_filter_sync 
[ INFO] [1664204780.217148024]: Sequence: [0] [368]
[ INFO] [1664204780.249005396]: Sequence: [1] [369]
[ INFO] [1664204780.280544030]: Sequence: [2] [370]
[ INFO] [1664204780.316463488]: Sequence: [3] [371]
[ INFO] [1664204780.348469919]: Sequence: [4] [372]
[ INFO] [1664204780.380547970]: Sequence: [5] [373]
[ INFO] [1664204780.416397331]: Sequence: [6] [374]
[ INFO] [1664204780.448968855]: Sequence: [7] [375]
[ INFO] [1664204780.484444885]: Sequence: [8] [376]
[ INFO] [1664204780.516418570]: Sequence: [9] [377]
[ INFO] [1664204780.548350255]: Sequence: [10] [378]
edit flag offensive delete link more

Comments

Thank you so much! And thank you for you suggestion, I already changed the answer to a more specific and general way to present the solution!

Janebek gravatar image Janebek  ( 2022-09-26 10:46:55 -0600 )edit

I am glad you made it work.

ravijoshi gravatar image ravijoshi  ( 2022-09-26 21:31:18 -0600 )edit
0

answered 2022-09-26 10:18:05 -0600

Janebek gravatar image

updated 2022-09-26 11:01:21 -0600

I firgured it out!

Put the below code into class member.

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> *sync;

And then in the ClientHandler::ClientHandler function put those two lines.

sync = new message_filters::Synchronizer<sync_pol>(sync_pol(10), rgb_sub,depth_sub);
sync->registerCallback(boost::bind(&ClientHandler::GrabRGBD,this,_1,_2));

For a more general view, based on @ravijoshi 's code, I modified it into two topics.

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/Image.h>

using namespace sensor_msgs;
using namespace message_filters;

class ClientHandler
{
public:
  ClientHandler(ros::NodeHandle& nh, std::string& topic_name_rgb,std::string& topic_name_sub);
  void GrabRGBD(const ImageConstPtr& image_rgb,const ImageConstPtr& image_depth);

private:
  ros::NodeHandle n;
  std::string topic_rgb,topic_depth;

  //add members in two topics situation
  message_filters::Subscriber<Image> rgb_sub;
  message_filters::Subscriber<Image> depth_sub;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
  message_filters::Synchronizer<sync_pol> *sync;
};

ClientHandler::ClientHandler(ros::NodeHandle& nh, std::string& topic_name_rgb,std::string& topic_name_depth) : n(nh), topic_rgb(topic_name_rgb),topic_depth(topic_name_depth)
{
  // the following declaration does not work. hence it is commented.
  // the subscriber is in a local scope
  // message_filters::Subscriber<Image> rgb_sub(n, topic, 1);

  // let's make them a class member and then call subscribe as shown below
  rgb_sub.subscribe(n, topic_rgb, 1);
  depth_sub.subscribe(n,topic_depth,1);

  // here is for the synchronizer
   sync = new message_filters::Synchronizer<sync_pol>(sync_pol(10), rgb_sub,depth_sub);

  // now bind a callback
   sync->registerCallback(boost::bind(&ClientHandler::GrabRGBD,this,_1,_2));
}

void ClientHandler::GrabRGBD(const ImageConstPtr& image_rgb , const ImageConstPtr& image_depth)
{
  ROS_INFO("Sequence: [%d] [%d]", image_rgb->header.seq, imgage_depth->header.seq);
}

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

  std::string topic_rgb = "/usb_cam/image_raw";
  std::string topic_depth = "/usb_cam/camera_info";
  ClientHandler ClientHandler(n, topic_rgb,topic_depth);

  ros::spin();
  return 0;
}

Now it can receive two topics at the same time!

Hope this will help you guys with the same problem!

edit flag offensive delete link more

Comments

Using raw pointers could be dangerous. That is why smart pointers are recommended.

ravijoshi gravatar image ravijoshi  ( 2022-09-26 21:30:25 -0600 )edit

Got it! Thank you!

Janebek gravatar image Janebek  ( 2022-09-26 23:10:02 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2022-09-26 08:41:37 -0600

Seen: 206 times

Last updated: Sep 26 '22