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

MessageFilter ApproximateTime doesn't call callback function

asked 2021-07-06 10:45:31 -0500

mebasoglu gravatar image

Hello, I'm trying to implement MessageFilter in a class. I'm using ROS1 Melodic on Ubuntu 18.04. The code compiles without any errors but when I play my bag file, the callback function aren't called. I checked some other questions but I couldn't find a solution. Here is my code :

Main:

#include <ros/ros.h>
#include "object_localization/CloudPainter.hpp"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "cloud_painter");
    ros::NodeHandle node_handle("~");

    object_localization::CloudPainter cloud_painter(node_handle);

    ros::spin();
    return 0;
}

CloudPainter.hpp:

#pragma once

#include <ros/ros.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

typedef message_filters::Subscriber<sensor_msgs::PointCloud2> PointCloudSubscriber;
typedef message_filters::Subscriber<sensor_msgs::Image> ImageSubscriber;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
                                                        sensor_msgs::Image>
    MySyncPolicy;

namespace object_localization
{
class CloudPainter
{
public:
    CloudPainter(ros::NodeHandle& t_node_handle);

private:
    bool readParameters();

    void messageFilterCallback(const sensor_msgs::PointCloud2ConstPtr& t_point_cloud,
                    const sensor_msgs::ImageConstPtr& t_image);

    ros::NodeHandle& m_node_handle;
    std::string m_lidar_topic;
    std::string m_camera_topic;
    PointCloudSubscriber m_point_cloud_subscriber;
    ImageSubscriber m_image_subscriber;
    message_filters::Synchronizer<MySyncPolicy> m_synchronizer;
};
}  // namespace object_localization

CloudPainter.cpp:

#include "object_localization/CloudPainter.hpp"

namespace object_localization
{
CloudPainter::CloudPainter(ros::NodeHandle& t_node_handle)
: m_node_handle(t_node_handle)
, m_synchronizer(MySyncPolicy(10), m_point_cloud_subscriber, m_image_subscriber)
{
    if (!readParameters())
    {
        ROS_ERROR("Could not read parameters.");
        ros::requestShutdown();
    }

    ROS_INFO("Lidar topic: : %s", m_lidar_topic.c_str());
    ROS_INFO("Camera topic: : %s", m_camera_topic.c_str());

    m_point_cloud_subscriber.subscribe(m_node_handle, m_lidar_topic,  1);
    m_image_subscriber.subscribe(m_node_handle, m_camera_topic,  1);
    m_synchronizer.registerCallback(
        boost::bind(&CloudPainter::messageFilterCallback, this, _1, _2));

    ROS_INFO("Successfully launched node.");
}

bool CloudPainter::readParameters()
{
    if (!m_node_handle.getParam("lidar_topic", m_lidar_topic))
    {
        return false;
    }
    if (!m_node_handle.getParam("camera_topic", m_camera_topic))
    {
        return false;
    }
    return true;
}

void CloudPainter::messageFilterCallback(const sensor_msgs::PointCloud2ConstPtr& t_point_cloud,
                                        const sensor_msgs::ImageConstPtr& t_image)
{
    ROS_INFO("Callback.");
}
}  // namespace object_localization

Output:

[ INFO] [1625585829.773481527]: Lidar topic: : velodyne_points
[ INFO] [1625585829.774161339]: Camera topic: : pylon_camera_node/image_rect
[ INFO] [1625585829.777213271]: Successfully launched node.
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2021-07-06 12:01:55 -0500

gvdhoorn gravatar image

updated 2021-07-06 12:08:34 -0500

You appear to be only passing a private NodeHandle to your CloudPainter ctor:

ros::NodeHandle node_handle("~");
object_localization::CloudPainter cloud_painter(node_handle);

That's why you need to prefix your topics with /, otherwise CloudPainter will try to subscribe to /cloud_painter/velodyone_points and /cloud_painter/pylon_camera_node/image_rect which don't exist.

With the / prefix, the ros::Subscriber will consider the topic names already fully resolved, and thus will not start searching for them in the private namespace of the node.

Ok, I found my problem. I didn't use "/" before topic names. So I changed "velodyne_points" to "/velodyne_points"

the better solution would be to either:

  • use a regular NodeHandle for your subscriptions (ie: not a private one), or
  • use remapping (and #q303611) to remap default topic names to the topics you wish to subscribe to. In your case that could be a points and a image topic, which you then remap to /velodyone_points and /pylon_camera_node/image_rect during rosrun or roslaunch of your node

    With roslaunch, that could look something like this:

    <launch>
      <node name="cloud_painter" type="cloud_painter" pkg="...">
        <remap from="image" to="/pylon_camera_node/image_rect" />
        <remap from="points" to="/velodyne_points" />
      </node>
    </launch>
    

Configuring topic names via ROS parameters is a bit of an anti-pattern (although opinions differ on that: #q342777).

edit flag offensive delete link more

Comments

Thank you.

mebasoglu gravatar image mebasoglu  ( 2021-07-06 13:05:00 -0500 )edit
0

answered 2021-07-06 11:32:41 -0500

mebasoglu gravatar image

Ok, I found my problem. I didn't use "/" before topic names. So I changed "velodyne_points" to "/velodyne_points" and I can see "Callback." in my terminal now.

edit flag offensive delete link more

Question Tools

Stats

Asked: 2021-07-06 10:45:31 -0500

Seen: 252 times

Last updated: Jul 06 '21