0
$\begingroup$

Rosanswers logo

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]

Subscriptions: None

Services: 
 * /DispatcherNode/get_loggers
 * /DispatcherNode/set_logger_level
 * /objrecog_dispatcher/set_parameters


contacting node http://inhands-server:35689/ ...
Pid: 31771
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS

Originally posted by jgallostra on ROS Answers with karma: 30 on 2019-03-27

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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);

Originally posted by ahendrix with karma: 47576 on 2019-03-27

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by jgallostra on 2019-03-27:
That is indeed the way to fix it - such a stupid error! Thanks for reading through all the code.

Comment by jgallostra on 2019-03-27:
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?

Comment by ahendrix on 2019-03-27:
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.

Comment by jgallostra on 2019-03-27:
Ok, thanks a lot!

Improve this answer
$\endgroup$

Your Answer

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