Nodelet zero-copy doesn't seem to work, source and destination addresses don't match
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] < ...