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

Nodelet zero-copy doesn't seem to work, source and destination addresses don't match

asked 2021-01-09 22:39:05 -0500

mmiles19 gravatar image

As the title says, I am trying to essentially "prove" that zero copy between nodelets works and I'm having trouble. I've got a ZeroCopyTestSenderNodelet as follows:

#include "ros/ros.h"
#include "nodelet/nodelet.h"
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Int32.h>
namespace ros_test_package
{
    class ZeroCopyTestSenderNodelet : public nodelet::Nodelet
    {
    public:
        inline virtual void onInit()
        {
            NODELET_INFO("[ZeroCopyTestSender]\tInitializing...");
            count_ = 0;
            getPrivateNodeHandle().param("rate", rate_, double(0.5));
            pub_ = getNodeHandle().advertise<std_msgs::Int32>("test_int", 1);
            pub_loop_ = getNodeHandle().createTimer(ros::Duration(1.0f/rate_), boost::bind(&ZeroCopyTestSenderNodelet::pubLoopFunc, this, _1));
            NODELET_INFO("[ZeroCopyTestSender]\tInitialized.");
        };
        inline void pubLoopFunc(const ros::TimerEvent& event)
        {
            std_msgs::Int32 this_int_msg;
            this_int_msg.data = count_++;
            NODELET_INFO("[ZeroCopyTestSender]\t--> value %d, address 0x%08X...", this_int_msg.data, &(this_int_msg.data));
            pub_.publish(this_int_msg);
            ros::spinOnce();
        }
    private:
        ros::Publisher pub_;
        ros::Timer pub_loop_;
        double rate_;
        int count_;
    };
} // namespace ros_test_package
PLUGINLIB_EXPORT_CLASS(ros_test_package::ZeroCopyTestSenderNodelet, nodelet::Nodelet)

and a ZeroCopyTestReceiverNodelet as follows:

#include "ros/ros.h"
#include "nodelet/nodelet.h"
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Int32.h>
namespace ros_test_package
{
    class ZeroCopyTestReceiverNodelet : public nodelet::Nodelet
    {
    public:
        inline virtual void onInit()
        {
            NODELET_INFO("[ZeroCopyTestReceiver]\tInitializing...");
            sub_ = getNodeHandle().subscribe("test_int", 1, &ZeroCopyTestReceiverNodelet::callback, this);
            NODELET_INFO("[ZeroCopyTestReceiver]\tInitialized.");
        };
        inline void callback(const std_msgs::Int32ConstPtr test_int_msg)
        {
            NODELET_INFO("[ZeroCopyTestReceiver]\t<-- value %d, address 0x%08X.", test_int_msg->data, &(test_int_msg->data));
        }
    private:
        ros::Subscriber sub_;
    };
} // namespace ros_test_package
PLUGINLIB_EXPORT_CLASS(ros_test_package::ZeroCopyTestReceiverNodelet, nodelet::Nodelet)

I am launching a sender and two receivers on the same manager using the following:

<launch>
    <node pkg="nodelet" type="nodelet" name="zero_copy_test_manager" args="manager" output="screen"/>
    <node pkg="nodelet" type="nodelet" name="zero_copy_test_sender_nodelet" args="load ros_test_package/ZeroCopyTestSenderNodelet zero_copy_test_manager" output="screen"/>
    <node pkg="nodelet" type="nodelet" name="zero_copy_test_receiver_nodelet" args="load ros_test_package/ZeroCopyTestReceiverNodelet zero_copy_test_manager" output="screen"/>
    <node pkg="nodelet" type="nodelet" name="zero_copy_test_receiver_nodelet2" args="load ros_test_package/ZeroCopyTestReceiverNodelet zero_copy_test_manager" output="screen"/>
</launch>

The output of running this is as follows, and is similar regardless if there is only 1 or 2 receivers launched:

[/zero_copy_test_manager@1610252429.626189985] Initializing nodelet with 12 worker threads.
[/zero_copy_test_manager@1610252429.635256318] [ZeroCopyTestSender] Initializing...
[/zero_copy_test_manager@1610252429.638284332] [ZeroCopyTestSender] Initialized.
[/zero_copy_test_manager@1610252429.649166515] [ZeroCopyTestReceiver]   Initializing...
[/zero_copy_test_manager@1610252429.655558553] [ZeroCopyTestReceiver]   Initialized.
[/zero_copy_test_manager@1610252429.659189946] [ZeroCopyTestReceiver]   Initializing...
[/zero_copy_test_manager@1610252429.663768935] [ZeroCopyTestReceiver]   Initialized.
[/zero_copy_test_manager@1610252431.639207762] [ZeroCopyTestSender] --> value 0, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252431.640040360] [ZeroCopyTestReceiver]   <-- value 0, address 0xE40017CC.
[/zero_copy_test_manager@1610252431.640214038] [ZeroCopyTestReceiver]   <-- value 0, address 0xE40017CC.
[/zero_copy_test_manager@1610252433.639092537] [ZeroCopyTestSender] --> value 1, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252433.639549131] [ZeroCopyTestReceiver]   <-- value 1, address 0xE40017CC.
[/zero_copy_test_manager@1610252433.639722691] [ZeroCopyTestReceiver]   <-- value 1, address 0xE40017CC.
[/zero_copy_test_manager@1610252435.638775083] [ZeroCopyTestSender] --> value 2, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252435.639248751] [ZeroCopyTestReceiver]   <-- value 2, address 0xE400182C.
[/zero_copy_test_manager@1610252435.639409818] [ZeroCopyTestReceiver]   <-- value 2, address 0xE400182C.
[/zero_copy_test_manager@1610252437.638406888] [ZeroCopyTestSender] --> value 3, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252437.638697812] [ZeroCopyTestReceiver]   <-- value 3, address 0xE400182C.
[/zero_copy_test_manager@1610252437.638795855] [ZeroCopyTestReceiver]   <-- value 3, address 0xE400182C.
[/zero_copy_test_manager@1610252439.639133032] [ZeroCopyTestSender] --> value 4, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252439.639587543] [ZeroCopyTestReceiver]   <-- value 4, address 0xD800151C.
[/zero_copy_test_manager@1610252439.639744379] [ZeroCopyTestReceiver]   <-- value 4, address 0xD800151C.
[/zero_copy_test_manager@1610252441.638872904] [ZeroCopyTestSender] --> value 5, address 0x0CF8E46C...
[/zero_copy_test_manager@1610252441.639297394] [ZeroCopyTestReceiver]   < ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-01-10 06:26:00 -0500

gvdhoorn gravatar image

updated 2021-01-10 06:30:46 -0500

You have this in your timer callback:

inline void pubLoopFunc(const ros::TimerEvent& event)
{
    std_msgs::Int32 this_int_msg;
    [..]
    pub_.publish(this_int_msg);
    ros::spinOnce();
}

to be able to skip the serialisation step, roscpp needs a pointer, and your message object created on the heap. This makes sense, as how would it provide your this_int_msg to subscribers after it has gone out of scope?

From the wiki/nodelet page, section Publishing from a Nodelet:

If you want the no-copy pub/sub to work you must publish your messages as shared_ptrs. See roscpp/Overview/Publishers and Subscribers#Intraprocess_Publishing for more details.

The linked page then shows the following example:

ros::Publisher pub = nh.advertise<std_msgs::String>("topic_name", 5);
std_msgs::StringPtr str(new std_msgs::String);
str->data = "hello world";
pub.publish(str);

You'll have to adapt your publishing nodelet to do this as well. I would expect your INFO messages to show matching addresses after you've made those changes.

(you could of course use std::make_shared<..>(..), I believe the example is from before we had C++11)

edit flag offensive delete link more

Comments

An additional comment: calling spinOnce() yourself in a callback is typically unnecessary, and in the case of nodelets even more so I believe. The manager already runs spinner(s) for you.

gvdhoorn gravatar image gvdhoorn  ( 2021-01-10 06:27:00 -0500 )edit

This was it! Thanks so much! I had tried making this_int_msg a member but hadn't tried making it a pointer. Thanks again!

mmiles19 gravatar image mmiles19  ( 2021-01-11 14:35:23 -0500 )edit

As a follow up, could you explain how even when the msg it created on the heap, it seems to be coming from the same address (0x0CF8E46C)? Is this a compiler optimization or something?

mmiles19 gravatar image mmiles19  ( 2021-01-11 15:14:10 -0500 )edit

As a follow up, could you explain how even when the msg it created on the heap, it seems to be coming from the same address? Is this a compiler optimization or something?

It's bad form to ask follow-up questions in comments under answers to other questions. Visibility is practically zero for such follow-ups. It would be better to ask a new question (possibly while referring to this one).

gvdhoorn gravatar image gvdhoorn  ( 2021-01-11 15:15:26 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-01-09 22:39:05 -0500

Seen: 276 times

Last updated: Jan 10 '21