技術標籤：ROSc++

做感測器資料融合時，常常會需要用到多個數據，即需要同時訂閱多個話題。那麼，如何同步這些感測器資料的時間輟，並將它們放入一個回撥函式中進行處理呢？

參考文件：

1. http://wiki.ros.org/message_filters
2. ROS自帶多感測器時間同步機制Time Synchronizer測試
3. ros訊息時間同步與回撥

有些訊息型別會帶有一個頭部資料結構，如下所示。資訊中帶有時間輟資料，可以通過這個資料進行時間同步。

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id

• 以下是一種同步的方式：Time Synchronizer

The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

#include <message_filters/subscriber.h>
#include
 
 <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
 

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

• 另外一種是基於策略的同步方式，也是通過訊息頭部資料的時間輟進行同步。

Policy-Based Synchronizer [ROS 1.1+]：

The Synchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

The Synchronizer filter is templated on a policy that determines how to synchronize the channels. There are currently two policies: ExactTime and ApproximateTime.

1. 當需要同步的所有訊息都帶有時間輟的頭部資料：ExactTime

   The message_filters::sync_policies::ExactTime policy requires messages to have exactly the same timestamp in order to match. Your callback is only called if a message has been received on all specified channels with the same exact timestamp. The timestamp is read from the header field of all messages (which is required for this policy).

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

  typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

由於該同步策略是當所有需同步的話題的時間輟嚴格相等時，才會觸發回撥函式。這就會導致以下一些問題：

• 回撥函式的觸發頻率必然小於等於這些話題中最小的釋出頻率；
• 回撥函式的觸發並不十分穩定，有時候甚至會出現長時間不被觸發的情況。如下圖所示，某一次的間隔甚至長達10s左右。

2. ROS提供了另外一種方法來實現資料的同步：ApproximateTime。與需要時間輟完全相同的ExactTime不同，該方法允許話題之間的時間輟存在一定的偏差。

   The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);

  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

從下圖可以看出，雖然該方法允許時間之間存在偏差，但實際上偏差並不大。而且比起上一種方法，這個方法的回撥函式的觸發頻率快多了。

關於ApproximateTime，我還有一個不解的地方，這裡做一下記錄：

If not all messages have a header field from which the timestamp could be determined, see below for a workaround.
If some messages are of a type that doesn’t contain the header field, ApproximateTimeSynchronizer refuses by default adding such messages.

以上這兩句話，似乎自相矛盾。不知道是不是我理解的問題。。。從時間同步的角度看，話題訊息內容中應該必須要帶上時間輟資訊才能進行同步，但第一句話卻說可以允許一些訊息不帶時間輟？

另外需要注意的是，使用message_filters時，需要在CMakeLists.txt和package.xml中新增相關依賴：

# CMakeLists.txt
find_package( catkin REQUIRED COMPONENTS
	 ...
	message_filters
)
# package.xml
find_package( catkin REQUIRED COMPONENTS
	<build_depend>message_filters</build_depend>
	<build_export_depend>message_filters</build_export_depend>
	<exec_depend>message_filters</exec_depend>
)