Test of behavior when ROS subscribes to a message published using a different allocator

Output:

[ INFO]: pub1 a boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > >
[ INFO]: pub2 b boost::shared_ptr<sensor_msgs::PointCloud2_<boost::fast_pool_allocator<void, boost::default_user_allocator_new_delete, std::mutex, 32u, 0u> > >
[ INFO]: sub a boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>
[ INFO]: Received message a is equal to p
[ INFO]: sub b boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>
[ INFO]: Received message b is a new instance
[ WARN]: length PC2 with frame b
[ WARN]: write PC2 with frame b
[ WARN]: read PointCloud2 with frame b

test.cpp

#include <sensor_msgs/PointCloud2.h>
#include <ros/message_forward.h>
#include <boost/pool/pool_alloc.hpp>
#include <ros/ros.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <ros/serialization.h>

namespace sensor_msgs
{
  ROS_DECLARE_MESSAGE_WITH_ALLOCATOR(PointCloud2, PC2, boost::fast_pool_allocator)
}

namespace ros
{
namespace serialization
{
template<>
struct Serializer<sensor_msgs::PC2>
{
  template<typename Stream, typename T>
  inline static void allInOne(Stream& stream, T m)
  {
    stream.next(m.header);
    stream.next(m.height);
    stream.next(m.width);
    stream.next(m.fields);
    stream.next(m.is_bigendian);
    stream.next(m.point_step);
    stream.next(m.row_step);
    stream.next(m.data);
    stream.next(m.is_dense);
  }

  template<typename Stream, typename T>
  inline static void write(Stream& stream, const T& t)
  {
    ROS_WARN("write PC2 with frame %s", t.header.frame_id.c_str());
    allInOne<Stream, const T&>(stream, t);
  }
  template<typename Stream, typename T>
  inline static void read(Stream& stream, T& t)
  {
    allInOne<Stream, T&>(stream, t);
    ROS_WARN("read PC2 with frame %s", t.header.frame_id.c_str());
  }
  template<typename T>
  inline static uint32_t serializedLength(const T& t)
  {
    ROS_WARN("length PC2 with frame %s", t.header.frame_id.c_str());
    LStream stream;
    allInOne<LStream, const T&>(stream, t);
    return stream.getLength();
  }
};

template<>
struct Serializer<sensor_msgs::PointCloud2>
{
  template<typename Stream, typename T>
  inline static void allInOne(Stream& stream, T m)
  {
    stream.next(m.header);
    stream.next(m.height);
    stream.next(m.width);
    stream.next(m.fields);
    stream.next(m.is_bigendian);
    stream.next(m.point_step);
    stream.next(m.row_step);
    stream.next(m.data);
    stream.next(m.is_dense);
  }

  template<typename Stream, typename T>
  inline static void write(Stream& stream, const T& t)
  {
    ROS_WARN("write PointCloud2 with frame %s", t.header.frame_id.c_str());
    allInOne<Stream, const T&>(stream, t);
  }
  template<typename Stream, typename T>
  inline static void read(Stream& stream, T& t)
  {
    allInOne<Stream, T&>(stream, t);
    ROS_WARN("read PointCloud2 with frame %s", t.header.frame_id.c_str());
  }
  template<typename T>
  inline static uint32_t serializedLength(const T& t)
  {
    ROS_WARN("length PointCloud2 with frame %s", t.header.frame_id.c_str());
    LStream stream;
    allInOne<LStream, const T&>(stream, t);
    return stream.getLength();
  }
};
} // namespace serialization
} // namespace ros

std::string demangle(const char* name) {

  int status = -4; // some arbitrary value to eliminate the compiler warning

  // enable c++11 by passing the flag -std=c++11 to g++
  std::unique_ptr<char, void(*)(void*)> res {
      abi::__cxa_demangle(name, NULL, NULL, &status),
      std::free
  };

  return (status==0) ? res.get() : name ;
}

sensor_msgs::PointCloud2Ptr p(new sensor_msgs::PointCloud2);
sensor_msgs::PC2Ptr c(new sensor_msgs::PC2);

void cb(sensor_msgs::PointCloud2ConstPtr msg)
{
  ROS_INFO_STREAM("sub " << msg->header.frame_id << " " << demangle(typeid(msg).name()));
  if ((void*)msg.get() == (void*)p.get())
    ROS_INFO("Received message %s is equal to p", msg->header.frame_id.c_str());
  else if ((void*)msg.get() == (void*)c.get())
    ROS_INFO("Received message %s is equal to c", msg->header.frame_id.c_str());
  else
    ROS_INFO("Received message %s is a new instance", msg->header.frame_id.c_str());
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "testy");

  ros::NodeHandle nh;
  ros::Publisher p1 = nh.advertise<sensor_msgs::PointCloud2>("a", 1);
  ros::Publisher p2 = nh.advertise<sensor_msgs::PC2>("b", 1);
  ros::Subscriber s1 = nh.subscribe("a", 10, &cb);
  ros::Subscriber s2 = nh.subscribe("b", 10, &cb);

  ros::WallDuration(0.5).sleep();

  p->header.stamp = ros::Time::now();
  p->header.frame_id = "a";

  sensor_msgs::PointCloud2Modifier mod1(*p);
  mod1.setPointCloud2FieldsByString(1, "xyz");
  mod1.resize(3);

  sensor_msgs::PointCloud2Iterator<float> x(*p, "x");
  sensor_msgs::PointCloud2Iterator<float> y(*p, "y");
  sensor_msgs::PointCloud2Iterator<float> z(*p, "z");

  *x = *y = *z = 1;
  ++x; ++y; ++z;
  *x = *y = *z = 2;
  ++x; ++y; ++z;
  *x = *y = *z = 3;

  ROS_INFO_STREAM("pub1 " << p->header.frame_id << " " << demangle(typeid(p).name()));
  p1.publish(p);
  
  c->header.stamp = p->header.stamp;
  c->header.seq = p->header.seq;
  c->header.frame_id = "b";
  c->point_step = p->point_step;
  c->row_step = p->row_step;
  c->is_bigendian = p->is_bigendian;
  c->width = p->width;
  c->height = p->height;
  c->is_dense = p->is_dense;
  for (const auto f : p->fields)
  {
    sensor_msgs::PC2::_fields_type::value_type f2;
    f2.datatype = f.datatype;
    f2.name = sensor_msgs::PC2::_fields_type::value_type::_name_type(f.name);
    f2.offset = f.offset;
    f2.count = f.count;
    c->fields.push_back(f2);
  }
  c->data = sensor_msgs::PC2::_data_type(p->data.begin(), p->data.end());

  ROS_INFO_STREAM("pub2 " << c->header.frame_id << " " << demangle(typeid(c).name()));
  p2.publish(c);

  ros::spinOnce();
  ros::WallDuration(0.1).sleep();
  ros::spinOnce();
}