Skip to content

Instantly share code, notes, and snippets.

@peci1
Created Jul 1, 2020
Embed
What would you like to do?
Test of behavior when ROS subscribes to a message published using a different allocator
[ 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
#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();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment