Implementation: roscpp/NewMessageAPIProposal2

Goals

  1. Support for a future without a Message base class (will have to wait for ROS 2.0 to actually fully remove, since it's an API-breaking change).
  2. Easy to write tests against serialization code
  3. Easy to write custom serialization code
  4. Custom allocator support (allowing, for example, allocation of messages in hard-realtime)
  5. Keep backwards compatibility with current API
  6. Keep support for scripting interfaces

Goals 1-3 I believe are best solved through a templated message traits + serialization system.

Goal 4 requires generated messages to be templated by allocator, and to pass that allocator through to vector/string

Goal 5 requires default template specializations that call through to the appropriate __s_getMD5Sum() member functions.

Goal 6 requires alternative publish/service call methods which allow explicit specification of md5sums.

Note that any code here is sample only, and the exact form will likely be slightly different. The ideas are the important part here, not the specifics of implementing the template code (that can get API reviewed later). Also, I will mostly ignore services here, but they will follow the same kinds of patterns.

Feel free to inline comments (prefixed with [name]), or to respond to the ros developers list.

Why templated traits/serialization?

First, they support Goal #1. Second, they allow adaption of any arbitrary data structure to be useable as a ROS message. For example, tf's Stamped<Point> could be adapted to allow subscribe()/advertise()/publish() to work directly with it. It would also allow us to alias primitive types to correspond to the std_msgs equivalent, i.e. you could do: advertise<uint32_t>(...) and it would be compatible with std_msgs/UInt32

Third, templated serialization supports goals #2 and #3, because they make message generation much simpler. That will be explained later.

Message Traits

namespace ros::message_traits

function-form traits

These traits use functions because they are returning things that are not possible/useful at compile time

  • const char* md5sum<M>() returns md5sum of M

  • const char* datatype<M>() return datatype of M

  • const char* definition<M>() returns message definition of M

  • roslib::Header* getHeader<M>(M& msg) returns a pointer to the header inside a message if it has one, NULL otherwise

struct-form traits

These traits use structs so that they can be used at compile time to enable different serialization options/optimizations

  • struct IsPrimitive<M>::value value is true if the message is a "primitive" type, where primitive is defined as "an array of these is memcpyable". Struct-form so that it can be used at compile-time

  • struct IsFixedSize<M>::value value is true if the message is a fixed size. Struct-form so that it can be used at compile-time

  • struct HasHeader<M>::value value is true if the message has a header. Struct-form so that it can be used at compile-time

Templated Serialization

My goal for serialization is to turn this:

   1 roslib::Header _ser_header = header;
   2 bool __reset_seq = (header.seq == 0);
   3 if (__reset_seq) _ser_header.seq = seq;
   4 write_ptr = _ser_header.serialize(write_ptr, seq);
   5 SROS_SERIALIZE_PRIMITIVE(write_ptr, level);
   6 unsigned __ros_name_len = name.length();
   7 SROS_SERIALIZE_PRIMITIVE(write_ptr, __ros_name_len);
   8 SROS_SERIALIZE_BUFFER(write_ptr, name.c_str(), __ros_name_len);
   9 unsigned __ros_msg_len = msg.length();
  10 SROS_SERIALIZE_PRIMITIVE(write_ptr, __ros_msg_len);
  11 SROS_SERIALIZE_BUFFER(write_ptr, msg.c_str(), __ros_msg_len);
  12 unsigned __ros_file_len = file.length();
  13 SROS_SERIALIZE_PRIMITIVE(write_ptr, __ros_file_len);
  14 SROS_SERIALIZE_BUFFER(write_ptr, file.c_str(), __ros_file_len);
  15 unsigned __ros_function_len = function.length();
  16 SROS_SERIALIZE_PRIMITIVE(write_ptr, __ros_function_len);
  17 SROS_SERIALIZE_BUFFER(write_ptr, function.c_str(), __ros_function_len);
  18 SROS_SERIALIZE_PRIMITIVE(write_ptr, line);
  19 uint32_t __topics_size = topics.size();
  20 SROS_SERIALIZE_PRIMITIVE(write_ptr, __topics_size);
  21 for (size_t i = 0; i < __topics_size; i++)
  22 {
  23   unsigned __ros_topics_i__len = topics[i].length();
  24 SROS_SERIALIZE_PRIMITIVE(write_ptr, __ros_topics_i__len);
  25 SROS_SERIALIZE_BUFFER(write_ptr, topics[i].c_str(), __ros_topics_i__len);
  26 }
  27 return write_ptr;

into something like this:

   1 write_ptr = serialize(write_ptr, header);
   2 write_ptr = serialize(write_ptr, level);
   3 write_ptr = serialize(write_ptr, name);
   4 write_ptr = serialize(write_ptr, msg);
   5 write_ptr = serialize(write_ptr, file);
   6 write_ptr = serialize(write_ptr, function);
   7 write_ptr = serialize(write_ptr, line);
   8 write_ptr = serialize(write_ptr, topics);
   9 return write_ptr;

This is done by specialization of serialize/deserialize for all of our primitive ROS types, e.g.:

   1 template<>
   2 inline uint8_t* serialize<uint32_t>(uint32_t val, uint8_t* buf)
   3 {
   4   *((uint32_t*)buf) = val;
   5   return buf + sizeof(uint32_t);
   6 }
   7 
   8 template<>
   9 inline uint8_t* serialize<float>(float val, uint8_t* buf)
  10 {
  11 ...
  12 }

In that vein, the API would look something like (in the ros::serialization namespace):

  • uint8_t* serialize<M>(const uint8_t* buffer, const M& val) serialize value into buffer, return next free position in buffer

  • uint8_t* deserialize<M>(const uint8_t* buffer, const M& val) deserialize beginning of buffer into value, return next position in buffer

    • deserialize would be able to throw an "invalid serialized buffer" exception if the buffer is not valid for this message

This solves goals #2 and 3. 2 because all the serialization/deserialization code can now be easily individually performance and correctness tested without needing a generated message. 3 because writing custom serializers becomes much easier.

Message Generator

With templated serialization, the message generators can become extremely small and dumb, since they don't need to know anything about the types they are outputting. All they need to know is:

  • primitive message type -> C++ type

  • package/Message -> package::Message

Custom Allocator Support

Each message would be templated on the allocator type, with the common one typedefed to be the normal name, e.g.:

   1 template<template<class T> class Allocator>
   2 class MyMsg_
   3 {
   4 public:
   5   std::vector<uint32_t, Allocator<uint32_t> > a;
   6 };
   7 typedef MyMsg_<std::allocator> MyMsg;
   8 typedef MyMsg_<realtime::Allocator> MyMsgRealtime;

Compatibility

With existing custom messages

The default traits/serialization can simply call into existing virtual functions, e.g.:

   1 template<typename M>
   2 uint8_t* serialize(uint8_t* buffer, const M& m)
   3 {
   4   return m.serialize(buffer);
   5 }

And the generated messages can still include the same methods they do now, but they will simply call into the templated versions.

With scripting interfaces

Once the Message base class is actually removed, we can either provide:

  • Publisher::publish(<message>, const std::string& md5sum)

  • service::call(req, res, const std::string& service_md5sum)

  • ServiceClient::call(req, res, const std::string& service_md5sum)

or new traits:

  • md5sumFromInstance<M>(M&)

  • serviceMD5SumFromInstance<M>(M&)

Wiki: roscpp/NewMessageAPIProposal (last edited 2010-02-07 19:03:45 by BrianGerkey)