ROS講座76 ROSノードの分類とmessage_filter

環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	Noetic

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

ROSのノードは「入力の本数」と「出力のタイミング」を考えると以下の表のように分類できます。ここの名称は独自の物なので注意。

	出力は一定周期	出力は入力に合わせたタイミング
入力が1つ	(1)タイマー駆動	(2)イベント駆動
入力が複数	(1)タイマー駆動	(3)片側イベント駆動 or (4)message_filter

(1)タイマー駆動型
main関数の中のfor文が一定周期で回っているorタイマーが動いていてその中でメインの処理が動いています。入力をいったんバッファして、そのバッファの値を使って処理します。入力が無くても一定周期で出力を行います。

• 〇入力はどうであれ一定の周期で出力を行いたい
• ×入力がないときでも処理負荷をもってしまう
• ×入力から出力までに遅延が発生する（複数のノードをつなげると無視できない大きさになります）

(2)イベント駆動型
main関数の中のfor文の中身はほとんどなく、subscriberのCallback関数でほとんどの処理を行います。

• 〇入力したら即座に反応して出力する
• 〇入力が無いときは処理しない
• ×入力の周期やタイミングによって動作が不安定になる

(3)片側イベント駆動型
イベント駆動っぽく動かしたいが、複数の入力に対応するものです。特定の1つの入力が来た時に処理を行います。他の入力はバッファリングをして最新のデータを利用します。

(4)message_filter
ほぼ定期的な複数の入力が来た時に「いい感じ」のタイミングで処理を行います。

(1)タイマー駆動型の例

adv_lecture/src/adv_nodetype_timer_driven.cpp

#include <geometry_msgs/PointStamped.h>
#include <ros/ros.h>

geometry_msgs::PointStamped input1_last;
void input1_callback(const geometry_msgs::PointStamped& point_msg)
{
  input1_last = point_msg;
}

geometry_msgs::PointStamped input2_last;
void input2_callback(const geometry_msgs::PointStamped& point_msg)
{
  input2_last = point_msg;
}

float hz = 10;
int main(int argc, char** argv)
{
  ros::init(argc, argv, "adv_nodetype_timerdriven");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");
  ros::Publisher output_pub = nh.advertise<geometry_msgs::PointStamped>("output", 10);
  ros::Subscriber input1_sub = nh.subscribe("input1", 10, input1_callback);
  ros::Subscriber input2_sub = nh.subscribe("input2", 10, input2_callback);
  pnh.getParam("hz", hz);

  ros::Rate loop_rate(hz);
  while (ros::ok())
  {
    geometry_msgs::PointStamped point_msg;

    point_msg.point.x = input1_last.point.x + input2_last.point.x;
    output_pub.publish(point_msg);

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

出力周期を決めるhzというparamがあると便利です。

入力にかかわらず一定周期で出力します。

(2)イベント駆動型の例

adv_lecture/src/adv_nodetype_event_driven.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>

ros::Publisher output_pub;
void input1_callback(const geometry_msgs::PointStamped& point_msg)
{
  output_pub.publish(point_msg);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "adv_nodetype_event_driven");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");
  output_pub = nh.advertise<geometry_msgs::PointStamped>("output", 10);
  ros::Subscriber input1_sub = nh.subscribe("input1", 10, input1_callback);

  ros::spin();
  return 0;
}

出力のpublisherをグローバル空間で宣言する必要があります。

入力が来たタイミングで出力を行います。

(3)片側イベント駆動型

adv_lecture/src/adv_nodetype_semi_event_driven.cpp

#include <geometry_msgs/PointStamped.h>
#include <ros/ros.h>

geometry_msgs::PointStamped input2_last;
void input2_callback(const geometry_msgs::PointStamped& point_msg)
{
  input2_last = point_msg;
}

ros::Publisher output_pub;
void input1_callback(const geometry_msgs::PointStamped& point_msg)
{
  geometry_msgs::PointStamped point_data;
  point_data.point.x = point_msg.point.x + input2_last.point.x;
  output_pub.publish(point_data);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "adv_nodetype_semi_event_driven");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");
  output_pub = nh.advertise<geometry_msgs::PointStamped>("output", 10);
  ros::Subscriber input1_sub = nh.subscribe("input1", 10, input1_callback);
  ros::Subscriber input2_sub = nh.subscribe("input2", 10, input2_callback);

  ros::spin();
  return 0;
}

片側の入力（ここではinput1）のタイミングに合わせて出力します。

(4)message_filter

message_filterはrosの機能でROSトピックのheaderの時間を見て時間が近いものをリンクしてsubscriberを起動する機能です。message_filterを使用するにはトピックは全てheaderを持っている型でないといけません。

/adv_nodetype_message_filter.cpp

#include <geometry_msgs/PointStamped.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <ros/ros.h>

ros::Publisher output_pub;
void sync_callback(const geometry_msgs::PointStamped& point1_msg, const geometry_msgs::PointStamped& point2_msg)
{
  geometry_msgs::PointStamped point_msg;
  point_msg.header.stamp = ros::Time::now();
  point_msg.point.x = point1_msg.point.x + point2_msg.point.x;
  output_pub.publish(point_msg);
}
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped>
    MySyncPolicy;
int main(int argc, char** argv)
{
  ros::init(argc, argv, "adv_nodetype_message_filter");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");
  output_pub = nh.advertise<geometry_msgs::PointStamped>("output", 10);

  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub1(nh, "input1", 10);
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub2(nh, "input2", 10);
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), point_sub1, point_sub2);
  sync.registerCallback(&sync_callback);

  printf("start\n");
  ros::spin();
  return 0;
}

入力周期がまちまちでもよい感じのタイミングで出力してくれます。

message_filterの欠点

message_filterはApproximateTimeモードであっても最短の組をセットにしてcallbackを呼ぶことに重きを置いています。そのために以下の「期待」のようなタイミングではなく、「実際」の様に次のメッセージが来て、最短の組が確定したタイミングで送信されます。

目次ページへのリンク

ROS講座の目次へのリンク