Ask Your Question
0

Asynchronous message filter does not subscribe to remapped nodelet topics

asked 2019-03-27 09:37:08 -0500

jgallostra gravatar image

updated 2019-03-27 09:38:33 -0500

I'm using a custom made package that uses nodelets for RGBD image processing. The end of the processing pipeline is a nodelet called ObjectRecognitionNodelet, which publishes to the topic /camX/object_rec/output_objects (where camX is either cam1, cam2 or cam3, as I launch three processing pipelines).

My goal is to create a node that subscribes to the three output topics and processes the objects' data. For that I wrote a DispatcherNode that uses the message_filters package for synchronizing the messages of the three topics. However, when I run the node it goes through the code but does not subscribe to anything. Below are the relevant code excerpts to understand the question. Am I missing something?


ObjectRecognitionNode - publisher (wrapped by a Nodelet class to launch it as a nodelet)

ObjectRecognitionNode::ObjectRecognitionNode(ros::NodeHandle nh,
                        ros::NodeHandle private_nh)
{
    this->nh_ = nh;
    this->private_nh_ = private_nh;
    this->private_nh_.getParam("cam_name", this->cam_);
    this->node_topic_ns_ = "/" + this->cam_ + "/object_rec";
    [...]
    this->initializePublishers();
    [...]
}

void
ObjectRecognitionNode::initializePublishers()
{
    this->objects_publisher_ = this->nh_.advertise<scene_msgs::ObjectArray>(this->node_topic_ns_ + "/output_objects", 5);
}

output from rostopic list | grep object_rec

/cam1/object_rec/output_objects
/cam1/object_recognition/parameter_descriptions
/cam1/object_recognition/parameter_updates
/cam2/object_rec/output_objects
/cam2/object_recognition/parameter_descriptions
/cam2/object_recognition/parameter_updates
/cam3/object_rec/output_objects
/cam3/object_recognition/parameter_descriptions
/cam3/object_recognition/parameter_updates

DispatcherNode - subscriber

header

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
[...]
namespace objrecog_dispatcher
{
class DispatcherNode
{
    public:
        DispatcherNode(ros::NodeHandle nh,
                ros::NodeHandle private_nh);
        [...]
    private:
        [...]
        typedef message_filters::sync_policies::ApproximateTime<scene_msgs::ObjectArray, scene_msgs::ObjectArray, scene_msgs::ObjectArray> MyApproxSyncPolicy;
        [...]
        void
        inputCallback(const scene_msgs::ObjectArray::ConstPtr & cam1_objects,
                const scene_msgs::ObjectArray::ConstPtr & cam2_objects,
                const scene_msgs::ObjectArray::ConstPtr & cam3_objects);
        [...]
        void
        initializeSubscribers();
        [...]
        std::string node_topic_ns_;
        [...]
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
        [...]
        message_filters::Synchronizer<MyApproxSyncPolicy> * synchronizer_;
        message_filters::Subscriber<scene_msgs::ObjectArray> cam1_objects_subscriber_;
        message_filters::Subscriber<scene_msgs::ObjectArray> cam2_objects_subscriber_;
        message_filters::Subscriber<scene_msgs::ObjectArray> cam3_objects_subscriber_;
        [...]
}; // class DispatcherNode
} // namespace objrecog_dispatcher

source

namespace objrecog_dispatcher
{
DispatcherNode::DispatcherNode(ros::NodeHandle nh,
                ros::NodeHandle private_nh)
{
    this->nh_ = nh;
    this->private_nh_ = private_nh;
    this->node_topic_ns_ = "dispatcher";
    [...]
    this->initializeSubscribers();
    [...]
    ROS_INFO("Dispatcher created");
}

[...]

void
DispatcherNode::initializeSubscribers()
{
    // Subscribe
    this->cam1_objects_subscriber_.subscribe(this->nh_, this->node_topic_ns_ + "/input_objects_cam1", 5);
    this->cam2_objects_subscriber_.subscribe(this->nh_, this->node_topic_ns_ + "/input_objects_cam2", 5);
    this->cam3_objects_subscriber_.subscribe(this->nh_, this->node_topic_ns_ + "/input_objects_cam3", 5);
    // Initialize synchronizer
    this->synchronizer_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(5), this->cam1_objects_subscriber_, this->cam2_objects_subscriber_, this->cam3_objects_subscriber_);
    this->synchronizer_->registerCallback(boost::bind(&DispatcherNode::inputCallback, this, _1, _2, _3));
    ROS_INFO("Dispatcher subscribed");
}
} // namespace objrecog_dispatcher

int
main(int argc, char** argv)
{
    ros::init(argc, argv, "DispatcherNode");
    ros::NodeHandle nh;
    ros::NodeHandle p_nh("~");
    objrecog_dispatcher::DispatcherNode(nh, p_nh);
    ros::Rate loop_rate(15);
    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
        ROS_INFO("DISPATCHER ROS LOOP");
    }
    return 0;
}

launchfile

<launch>
  <arg name="use_machine"       default="true"/>
  <arg name="machine"           default="localhost"/>

  <machine name="localhost" address="localhost" if="$(arg use_machine)"/>

  <!-- Dispatcher Node -->
  <node pkg="objrecog_dispatcher" type="DispatcherNode" name="DispatcherNode" machine="$(arg machine)" output="screen">
    <remap from="dispatcher/input_objects_cam1" to="cam1/object_rec/output_objects"/>
    <remap from="dispatcher/input_objects_cam2" to="cam2/object_rec/output_objects"/>
    <remap from="dispatcher/input_objects_cam3" to="cam3/object_rec/output_objects"/>
  </node>
</launch>

Output from rosnode info DispatcherNode

Node [/DispatcherNode]
Publications: 
 * /objrecog_dispatcher/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /objrecog_dispatcher/parameter_updates [dynamic_reconfigure/Config]
 * /rosout [rosgraph_msgs/Log ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-03-27 10:36:46 -0500

ahendrix gravatar image

In your main, your dispatcher node is a temporary object, so it is created and then immediately destroyed:

objrecog_dispatcher::DispatcherNode(nh, p_nh);

Instead, you need to keep the DispatcherNode in a variable:

objrecog_dispatcher::DispatcherNode dispatcher_node(nh, p_nh);
edit flag offensive delete link more

Comments

That is indeed the way to fix it - such a stupid error! Thanks for reading through all the code.

jgallostra gravatar imagejgallostra ( 2019-03-27 10:48:36 -0500 )edit

Just one thing I wonder now, how come in the faulty version the DispatcherNode appeared when calling rostopic list and I could call rosnode info DispatcherNode? Ok, it was a temporary object, but apparently ROS kept some information about the node. Maybe it has something to do with the fact that it creates a dynamic reconfigure server, or maybe it wasn't destroyed at all?

jgallostra gravatar imagejgallostra ( 2019-03-27 10:51:00 -0500 )edit

The ROS node is not the DispatcherNode C++ object. The ROS node is the program, and it starts being a ROS node when you call ros::init(argc, argv, "DispatcherNode");, and continues to be a ROS node until the last NodeHandle is destroyed or ros::shutdown() is called.

ahendrix gravatar imageahendrix ( 2019-03-27 11:05:12 -0500 )edit

Ok, thanks a lot!

jgallostra gravatar imagejgallostra ( 2019-03-27 11:14:39 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-03-27 09:37:08 -0500

Seen: 80 times

Last updated: Mar 27