0
$\begingroup$

I need to subscribe to two different topics (PointCloud2 and PoseStamped) in a synchronized manner. First thing I did was to write a simple callback function and test if message_filters was working correctly, the following code works as I want

rclcpp::Node::SharedPtr g_node = nullptr;
void exact_sync_callback(const sensor_msgs::msg::PointCloud2::SharedPtr pointcloud_msg, const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg)
{
    // Do stuff
}

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

  rclcpp::init(argc, argv);
  g_node = rclcpp::Node::make_shared("simple_time_sync");

  message_filters::Subscriber<sensor_msgs::msg::PointCloud2> pointcloud_sub(g_node, "/point_cloud/cloud_registered");
  message_filters::Subscriber<geometry_msgs::msg::PoseStamped> pose_sub(g_node, "/pose");

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, geometry_msgs::msg::PoseStamped> approximate_policy;
  message_filters::Synchronizer<approximate_policy>syncApproximate(approximate_policy(10), pointcloud_sub, pose_sub);
  syncApproximate.registerCallback(exact_sync_callback);
  rclcpp::spin(g_node);
  return 0;

}

I then wanted to convert the code into a class as I usually do with ROS2 node. I am able to compile and run the following but it doesn't print anything, it looks like he is not receiving the topics correctly

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, geometry_msgs::msg::PoseStamped> approximate_policy;

class SynchSubscriber : public rclcpp::Node
{
public:

    message_filters::Subscriber<sensor_msgs::msg::PointCloud2> subscription_temp_1_;
    message_filters::Subscriber<geometry_msgs::msg::PoseStamped> subscription_temp_2_;
  
    SynchSubscriber() : Node("exact_time_subscriber")
    {
        subscription_temp_1_.subscribe(this, "/point_cloud/cloud_registered");
        subscription_temp_2_.subscribe(this, "/pose");

        message_filters::Synchronizer<approximate_policy> syncApproximate(approximate_policy(10), subscription_temp_1_, subscription_temp_2_);
        syncApproximate.registerCallback(std::bind(&SynchSubscriber::topic_callback, this, _1, _2));
    }

private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& tmp_1, const geometry_msgs::msg::PoseStamped::ConstSharedPtr& tmp_2) const
    {
        RCLCPP_INFO(this->get_logger(), "I heard: '%d' and '%d'", tmp_1->header.stamp.sec, tmp_2->header.stamp.sec);
    }

};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SynchSubscriber>());
    rclcpp::shutdown();

    return 0;
}   

Does anyone know why the latter is not working correctly?

EDIT

I was able to find a fix, I needed do reset the Synchronize object

class SynchSubscriber : public rclcpp::Node
{
public:

    message_filters::Subscriber<sensor_msgs::msg::PointCloud2> subscription_temp_1_;
    message_filters::Subscriber<geometry_msgs::msg::PoseStamped> subscription_temp_2_;
    std::shared_ptr<message_filters::Synchronizer<approximate_policy>> sync_;
  
    SynchSubscriber() : Node("exact_time_subscriber")
    {
        subscription_temp_1_.subscribe(this, "/point_cloud/cloud_registered");
        subscription_temp_2_.subscribe(this, "/pose");

        sync_.reset(new message_filters::Synchronizer<approximate_policy>(approximate_policy(10), subscription_temp_1_, subscription_temp_2_));

        sync_->registerCallback(std::bind(&SynchSubscriber::topic_callback, this, _1, _2));
    }


private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& tmp_1, const geometry_msgs::msg::PoseStamped::ConstSharedPtr& tmp_2) const
    {
        RCLCPP_INFO(this->get_logger(), "I heard: '%d' and '%d'", tmp_1->header.stamp.sec, tmp_2->header.stamp.sec);
    }

};

```
$\endgroup$
1
  • $\begingroup$ Instead of editing the question to add the solution, you should post the solution as an answer and accept it. That way, the question is marked as resolved and does not keep popping up. $\endgroup$
    – JRTG
    Commented May 26 at 11:58

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.