Building the ROS message synchronizer I need

To do this, I had to learn about the ROS message_filters and rosbag APIs.

For my purposes, I need to synchronize messages coming from three topics: sensor_msgs/Image from /cam0/image_raw, sensor_msgs/Image from /cam1/image_raw, and sensor_msgs/Imu from /imu0. The two Images should arrive synchronized: if there are \(n\) images from /cam0/image_raw, then there should be \(n \pm 1\) images from /cam1/image_raw (due to earlier startup/shutdown). The systems behavior is different in the first \(t_\mathrm{init}\) time since the initial message came in, as it’s an initialization phase.

Most of the times, you’d work with a ROS bag; a container for ROS messages that can be used to replay them over their ROS topics. When you have a ROS bag, you can use it in two ways:

• Play it externally with rosbag play, and let your ROS node subscribe to the topics, as if it were a live feed. This works just like any other node. Here, you can synchronize messages by time, using the following except:

  ros::NodeHandle nh;
  
  message_filters::Subscriber<sensor_msgs::Image> cam0_sub(nh, "/cam0/image_raw", 1);
  message_filters::Subscriber<sensor_msgs::Image> cam1_sub(nh, "/cam1/image_raw", 1);
  
  using sync_pol = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image>;
  message_filters::Synchronizer<sync_pol> sync(sync_pol(10), cam0_sub, cam1_sub);
  
  sync.registerCallback(&some_callback_fn);
  

  In the first bit, you create a ROS node as usual. In the second part, you create message_filters::Subscribers, instead of the usual ros::Subscriber sub = nh.subscribe("/cam0/image_raw", 1, callback). Message filters are designed so you can chain them. Message filters have inputs (often a message filter itself; message_filters::Subscriber is the exception to the rule), and output behavior.

  In the third code paragraph, we create a message_filters::Synchronizer, templated on some synchronization policy. The default message_filters::TimeSynchronizer uses a policy that requires the messages to have the exact same timestamp (synchronization policy message_filters::sync_policies::ExactTime), but as that often is not the real-world case, we’ll be using a message_filters::sync_policies::ApproximateTime policy. That does mean a bit of extra writing though :) . These policies (and thus also message_filters::TimeSynchronizer) accept up to 9 input filters, their types must be included in the template, and the instances passed to the constructor of the synchronizer.

  Finally, in the fourth line, we register the callback function, which will need to accept two arguments of the ConstPtr version of the message type.

  Note that this is not able to collect messages, like we need for the IMU messages.

• Use the rosbag API to create a rosbag::View, filter the bag with a rosbag::Query, and iterate over the items in a foreach loop. This works a bit differently, and I’ll show how to use it with an example.

  First, we’ll set up the rosbag::Bag object to read from the file:

  std::string bag_filename = "/path/to/the.bag";
  rosbag::Bag bag;
  bag.open(bag_filename, rosbag::bagmode::Read);
  

  We will be creating a rosbag::View of this bag; a filtered-down set of messages. To filter them, we’ll be using queries. As I know the topic names I’m interested in, I’ll be using rosbag::TopicQuery. Its constructor accepts a std::string or std::vector<std::string> of topic names, so let’s create the view:

  std::string cam0_topic = "/cam0/image_raw";
  std::string cam1_topic = "/cam1/image_raw";
  std::string imu_topic = "/imu0";
  std::vector<std::string> topics;
  topics.push_back(cam0_topic);
  topics.push_back(cam1_topic);
  topics.push_back(imu_topic);
  
  rosbag::View view(bag, rosbag::TopicQuery(topics));
  

  We may iterate over the entries in the view by using a foreach loop. The iterated item m will be of type rosbag::MessageInstance, a type-agnostic object. It needs some inspection to figure out which message you are processing. For instance, you can request the currently contained data type by m.getDataType(), and the topic the message came from with m.getTopic(). You can also just use m.instantiate<T>() with T the desired message type. The returned object is a pointer to that message; if that pointer isn’t NULL, everything went fine and you can process it. If it is NULL, it was a message of a different type. Note that this method doesn’t suffice in our case, as we need to distinguish between images from two different cameras.

  In the example below, I inspect the messages based on their topic, and just to be safe, inspect the pointers for nullness:

  for (const rosbag::MessageInstance m : view) {
      if (m.getTopic() == imu_topic) {
          sensor_msgs::ImuConstPt imu = m.instantiate<sensor_msgs::Imu>();
          if (imu != NULL) {
              // process IMU
          }
      }
      else if (m.getTopic() == cam0_topic) {
          sensor_msgs::ImageConstPtr image0 = m.instantiate<sensor_msgs::Image>();
          if (image0 != NULL) {
              // process image0
          }
      }
      else if (m.getTopic() == cam1_topic) {
          sensor_msgs::ImageConstPtr image1 = m.instantiate<sensor_msgs::Image>();
          if (image1 != NULL) {
              // process image1
          }
      }
  }
  

  Note that you can’t use callbacks in this way, nor use the message_filters API. Synchronization has to be done by hand.

I have decided to use the message_filters API, as it offers some synchronization functionality to build on. I’ve made a SynchroBuffer class that encapsulates everything that needs to be done: a message_filters::Subscriber for each of the three topics; message_filters::Synchronizer<sync_pol> sync that synchronizes all three; and callbacks registered on sync and on the Subscriber of the IMU topic. The IMU callback logs the messages to a std::deque, and calls SynchroBuffer::process_data(). sync’s callback stores all three data members, and also calls SynchroBuffer::process_data().

SynchroBuffer::process_data() checks if sync’s most recent IMU message also occurs in the std::deque of stored IMU data (it might be that either callback is behind a little bit on the other). If so, call the right ORB_SLAM2::System tracking function. An example can be found in this file; it’s a work-in-progress as it segfaults at this stage, but the structure should be clear.