Robotics StackExchange | Archived questions

message_filters::sync::ApproximateTime does not call the callback

I am trying to subscribe to a pointcloud and it's normal from to different subscribers and pass it on to a common function via approximate time sync. Please find minimal code below:

void OrganizedMultiPlaneSegmentation::onInit() {
  sub = private_nh.subscribe("/cloud/cloud_out_merged", 1,
                             &OrganizedMultiPlaneSegmentation::segment, this,
                             ros::TransportHints().tcpNoDelay());
}

void OrganizedMultiPlaneSegmentation::segment(
    const sensor_msgs::PointCloud2& msg) {
  ros::NodeHandle& nh_ = getNodeHandle();

  sub1_.subscribe(nh_, "/cloud/cloud_out_merged", 5);
  if (sub1_.getSubscriber().getNumPublishers() == 1) {
    // NODELET_INFO("Got a subscriber to scan, starting subscriber to merged
    // pointcloud");
  }
  sub2_.subscribe(nh_, "/cloud_out_normal", 5);
  if (sub2_.getSubscriber().getNumPublishers() == 1) {
    // NODELET_INFO("Got a subscriber to scan, starting subscriber to normal
    // pointcloud");
  }
  typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
                                         sensor_msgs::PointCloud2>
      MySyncPolicy;
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(40), sub1_, sub2_);
  //   sync.setAgePenalty(1.0);
  sync.registerCallback(boost::bind(
      &OrganizedMultiPlaneSegmentation::segmentPlanes, this, _1, _2));
}

// Callback function to be called
void OrganizedMultiPlaneSegmentation::segmentPlanes(
    const PointCloud2ConstPtr& cloud_msg1,
    const PointCloud2ConstPtr& cloud_msg2) {
  pcl::PointCloud<pointT>::Ptr cloud(new pcl::PointCloud<pointT>());
  pcl::fromROSMsg(*cloud_msg1, *cloud);
  pcl::PointCloud<pcl::Normal>::Ptr normalCloud(
      new pcl::PointCloud<pcl::Normal>());

I have tried changing queue size of the subscriber, the buffer size of the policy, and both don't seem to work. Please let me know if there is something obvious that I am doing wrong,or what can be possible causes and remedies for this. Happy to provide more information and thank you!

Asked by Pran-Seven on 2022-11-14 05:05:01 UTC

Comments

Answers

Please declare the Synchronizer as a class member. At present, it is a local variable. See this answer for more details.

BTW, please make sure that there is data with acceptable time header in both topics.

Asked by ravijoshi on 2022-11-15 01:28:11 UTC

Comments

Thank you for you suggestions Ravi! The compiler actually compiles fine and the packages also run and I am able to see the subscribed outputs, so I am not sure 1 and 2 cause any actual issues, I have seen some tutorials also declare it the way I did, but if it is good practice to declare the variables in that format, I shall make changes accordingly. 3. Okay, I was afraid that the issue was something like this, I will change it, test and update.

Yes, that is made sure, topics are being published with relevant data and timestamps. Once again, thank you fro your suggestions.

Asked by Pran-Seven on 2022-11-15 01:54:39 UTC

I am sorry for the misunderstanding. I was answering another question which was tagged as ROS 2. Please ignore 1 and 2. The only problem I see could be related to the local variable sync. Please make it a class variable, as said in the answer above.

Asked by ravijoshi on 2022-11-15 03:15:57 UTC