The Deserializer

Or... "Reverse engineering of boost::serialization"

Once the schema is available in the form of a RosIntrospection::ROSTypeList we are able to extract valuable information from a "raw" message.

We don't have the support of the C++ typesystem, which was provided by the included file generated by the IDL compiler, therefore the fields of the message can not be "composed" into a struct or class.

The only data structure that can contain our data is currently a flat structure that stores simple key-value pairs:

  typedef struct{
    StringTree tree;
    std::vector< std::pair<StringTreeLeaf, double> > value;
    std::vector< std::pair<StringTreeLeaf, SString> > name;
    std::vector< std::pair<SString, double> > renamed_value;
  }ROSTypeFlat;

StringTree is a data structure inspired by Trie. The only difference is that each node contains an entire substring, not a single character. This is also known as "suffix string tree".

StringTreeLeaf is a terminal node of the tree; most of the user just need to know that the name of the key can be built using StringTreeLeaf::toStr().

The data structure itself reveals some of the main limitations of the parser:

• It is not well suited for large arrays (hundred or thousand of elements), such as images, maps, point clouds, etc. Very large arrays are simply discarded. This may change in the future.

• A double is used as a "conservative" type to store any builtin integral type. This makes the code simpler and avoid the need of type erasing techniques, that would be less efficient anyway.

• SString is used. It is an alternative implementation of std::string with a better Small Object Optimization. It is considerably faster than std:string in many cases, because it use stack allocation instead of heap allocation.

Example 3

Let's see how the parser and the deserializer work together with a slightly more complex type, sensor_msgs::JointState.

Let's suppose that a publisher sends this instance of sensor_msgs::JointState using a ROS topic (the code related to ROS and publishing is ignored here):

joint_state.header.seq = 2016;
joint_state.header.stamp.sec  = 1234;
joint_state.header.stamp.nsec = 567*1000*1000;
joint_state.header.frame_id = "pippo";

joint_state.name.resize( 2 );
joint_state.position.resize( 2 );
joint_state.velocity.resize( 2 );
joint_state.effort.resize( 2 );

joint_state.name[0] = "first_joint";
joint_state.position[0]= 10;
joint_state.velocity[0]= 11;
joint_state.effort[0]= 12;

joint_state.name[1] = "second_joint";
joint_state.position[1]= 20;
joint_state.velocity[1]= 21;
joint_state.effort[1]= 22;
  
  //publish this on a ros topic...

On the subscriber side we want to read this data but we don't know at compilation time that it is a sensor_msgs::JointState. To solve this problem we need the support of an usefull but not well know class: topic_tools::ShapeShifter

//callback subscribed to the topic
void DataStreamROS::topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg)
{
    using namespace RosIntrospection;

    // Declared "static" because we want to reuse ROSTypeList if possible.
    static std::map<std::string, ROSTypeList> registered_types;

    auto& datatype_name = msg->getDataType();

    // If not stored, parse it and store it.
    if( registered_types.find( datatype_name ) == registered_types.end() )
    {
        registered_type[datatype_name] = 
            buildRosFlatType(datatype_name, msg->getMessageDefinition() );
    }
    // allocate a buffer and copy the raw message.
    std::vector<uint8_t> buffer( msg->size() ); 
    ros::serialization::OStream stream(buffer, sizeof(buffer));
    msg->write(stream);

    // Important: use a COPY of the pointer.
    uint8_t* buffer_ptr = &buffer[0];
    
    SString topicname( topic_name.data(), topic_name.length() );

    ROSTypeFlat flat_container;
    buildRosFlatType( registered_type[datatype_name], // the "dictionary"
                      ROSType(datatype_name), // the current type to be deserialized
                      topicname, // name to be used as "root" ot the tree
                      &buffer_ptr, //pointer to raw data
                      flat_container //output
                    );
                                       
    for(auto& it: flat_container.value) {
        std::cout << it.first.toStr() << " >> " << it.second << std::endl;
    }
    std::cout << "----" << std::endl;
    for(auto& it: flat_container.name_id) {
        std::cout << it.first.toStr() << " >> " << it.second << std::endl;
    }
}

The expected output is:

JointState.header.seq >> 2016.0
JointState.header.stamp >> 1234.57
JointState.position.0 >> 10.0
JointState.position.1 >> 20.0
JointState.velocity.0 >> 11.0
JointState.velocity.1 >> 21.0
JointState.effort.0 >> 12.0
JointState.effort.1 >> 23.0
----
JointState.header.frame_id >> pippo
JointState.name.0 >> first_joint
JointState.name.1 >> second_joint