indigo jade kinetic lunar   Documentation Status

Overview

This simple library extracts information from a ROS Message, even when its type is unknown at compilation time.

Have you ever wanted to build an app that can subscribe to any topic and extract its content, or can read data from any rosbag? What if the topic and/or the bag contains user defined ROS types ignored at compilation time?

The common solution in the ROS ecosystem is to use Python, that provides the needed introspection. Tools, for instance, like rqt_plot and rqt_bag took this approach. This library implements a C++ alternative.

Ros Type Introspection

The library is composed of three main modules:

• Parser: it performs introspection of a Ros Message using the schema stored in ros::message_traits::Definition.

• Deserializer: using the schema built by the parsed, it can extract the actual values from a raw message.

• Renamer: last but not least, the library offers as well an easy way to remap/rename the data using a simple set of rules. This can be very handy in multiple scenarios that are very common in ROS.

This library is particularly useful to extract data from two type-erasing classes provided by ROS itself:

1. topic_tools::ShapeShifter: a type used to subscribe to any topic, regardless of the original type.

2. rosbag::MessageInstance: the generic type commonly used to read data from a ROS bag.

API Stability

• The C++ API is NOT stable. It might evolve in the future. Your feedback is welcome.

Usage

The Parser

To introspect a ROS message we need its definition and datatype.

A single function called RosIntrospection::buildROSTypeMapFromDefinition extracts a list of types which will be stored in RosIntrospection::ROSTypeList.

Suppose for instance that ROSTypeList contains the definition of geometry_msgs::Pose: we can print the list of fields and their type as follows:

   1 void printRosTypeList(const RosIntrospection::ROSTypeList& type_list)
   2 {
   3     for (const ROSMessage& msg: type_list) {
   4         std::cout << "\n" << msg.type().baseName() <<" : " << std::endl;
   5         for (const ROSField& field : msg.fields() ){
   6             std::cout  << "\t" << field.name()
   7                        <<" : " << field.type().baseName() << std::endl;
   8         }
   9     }
  10 }

The output will be

   geometry_msgs/Pose :
      position : geometry_msgs/Point
      orientation : geometry_msgs/Quaternion

   geometry_msgs/Point :
      x : float64
      y : float64
      z : float64

   geometry_msgs/Quaternion :
      x : float64
      y : float64
      z : float64
      w : float64

In other words, not only geometry_msgs::Pose but also geometry_msgs::Point and geometry_msgs::Quaternion is extracted by RosIntrospection::buildROSTypeMapFromDefinition.

The Deserializer

Once the schema is available in the form of a 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 gencpp, 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 a flat vector that stores simple key-value pairs. We avoid using std::map to make the container more cache-friendly.

   1   typedef struct{
   2     StringTree tree;
   3     std::vector< std::pair<StringTreeLeaf, double> >  value;
   4     std::vector< std::pair<StringTreeLeaf, SString> > name;
   5     std::vector< std::pair<SString, double> > renamed_value;
   6   }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.

Let's consider for example an instance of sensor_msgs::JointState built as follows:

   1 sensor_msgs::JointState joint_state;
   2 
   3 joint_state.header.seq = 2016;
   4 joint_state.header.stamp.sec  = 1234;
   5 joint_state.header.stamp.nsec = 567*1000*1000;
   6 joint_state.header.frame_id = "base_frame";
   7 
   8 joint_state.name.resize( 2 );
   9 joint_state.position.resize( 2 );
  10 joint_state.velocity.resize( 2 );
  11 joint_state.effort.resize( 2 );
  12 
  13 joint_state.name[0] = "first_joint";
  14 joint_state.position[0]= 10;
  15 joint_state.velocity[0]= 11;
  16 joint_state.effort[0]= 12;
  17 
  18 joint_state.name[1] = "second_joint";
  19 joint_state.position[1]= 20;
  20 joint_state.velocity[1]= 21;
  21 joint_state.effort[1]= 22;

If we print the Key/Value pairs of ROSTypeFlat::value and ROSTypeFlat::name we get this output:

/// Elements of ROSTypeFlat::value
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

/// Elements of ROSTypeFlat::name
JointState.header.frame_id >> base_frame
JointState.name.0 >> first_joint
JointState.name.1 >> second_joint

The Renamer

At first this functionality doesn't look like much, but you will quickly realize that it is extremely convenient in a number of common use cases. For the records, the decision to use StringTree is mostly focused on making the renamer considerably faster. Let's consider again the frequently used type sensor_msgs::JointState.

It uses the field "name" to assign an identifier to a specific index in the other arrays (position, velocity, effort). This means of course that you have to serialize, deserialize and compare these strings in every single message.

As we saw in the previous section, the Deserialized is order-dependant, while the Key-Value approach in ROS is order-independent. What would happen if the order of the identifiers is shuffled in different message? What if another message contains only the a single JointState with

• JointState.name.0 = second_joint ?

If you have ever tried to visualize a tf or tf2 message in rqt_plot, you know what I am talking about...

What we ideally want in the JointState example is:

JointState.header.seq >> 2016.0
JointState.header.stamp >> 1234.57
JointState.header.frame_id >> base_frame

JointState.first_joint.pos >> 10
JointState.first_joint.vel >> 11
JointState.first_joint.eff >> 12

JointState.second_joint.pos >> 20
JointState.second_joint.vel >> 21
JointState.second_joint.eff >> 22

This can be achieved using the function:

   1   void applyNameTransform(const std::vector<SubstitutionRule> &rules,
   2                           ROSTypeFlat* container);                      

This function will use a set of rules to fill the vector ROSTypeFlat::renamed_value using the data contained in

• ROSTypeFlat::value

and

• ROSTypeFlat::name.

The rules used to perform the remapping in the JointState example are:

   1 std::vector<SubstitutionRule> rules;
   2 rules.push_back( SubstitutionRule( "position.#", "name.#", "@.pos" ));
   3 rules.push_back( SubstitutionRule( "velocity.#", "name.#", "@.vel" ));  
   4 rules.push_back( SubstitutionRule( "effort.#", "name.#", "@.eff" ));

These rules are pretty easy to use once they are understood. For instance, let's consider the following example:

• the rule SubstitutionRule( "position.#", "name.#", "@.pos" )

  is using JointState.name.0 = first_joint

  to convert JointState.position.0 = 10

  into JointState.first_joint.pos = 10

The first argument, "position.#", means: "find any element in ROSTypeFlat::value which contains the pattern [position.#] where # is a number".

• JointState.position.0 = 10

The second argument, "name.#", means: "find the element in ROSTypeFlat::name which contains the pattern [name.#] where # is the same number found in the previous pattern".

• JointState.name.0 = first_joint

The third argument, "@.pos", means: "substitute the pattern found in 1. with this string, where the symbol @ represents the name found in 2". The final result is therefore:

• JointState.first_joint.pos = 10