QoSプロファイルの実践と課題
ROS2 Advent Calendar 13日目の記事を担当します。 今回はROS2の通信制御を行うQoSプロファイルについて、実践してみて得られた有用と思われる気づきを共有します。
ROS 2 Eloquentで新しく増えたQoSポリシー
ずいぶんと前(2年半も前!?)にブログでも書いたのですが、ROS2の通信プロトコルに採用されたDDSは通信品質を制御するためのいくつかのオプション(QoSポリシー)を持っています。
このQoSポリシーはROS 2 Dashingまでは
- History + Depth
- Reliability
- Durability
の3つ(4つとも数えられる)だけだったのですが、ROS 2 Eloquentでは
- Lifespan: どれくらいの期間、未送信のメッセージを維持するか
- Deadline: 守るべき最小周期
- Liveliness: どの方式のハートビート(生存確認)が必要か
の3つの新たなQoSポリシーが追加されました。ROSCon 2019のAWSからの発表で詳細を聞いたのですが、こちらの新たなQoSポリシーの話はまた今度にします。どれも産業用途では重要になりそうなオプションです。
今日はこれまでの3つのQoSポリシーに関して、実際に使ってみて得られた知見を共有します。
ソースコードでのプロファイル設定
ROS 2 Dashingまでは、(Deprecatedながらも)QoSプロファイルの設定を省略することができましたが、ROS 2 Eloquentからは必ずQoSProfile
型変数を使って、create_publisher
, create_subscription
の引数にQoSプロファイルを渡す必要があります。 Pythonだと以下のように書きます。
1
2
3
4
5
6
profile = QoSProfile(depth=10, reliability=..., durability=...)
pub = self.create_publisher(
std_msgs.msg.String, 'chatter', profile)
...
sub = self.create_subscription(
std_msgs.msg.String, 'chatter', self.callback, profile)
注意が必要なのは、Publisher側とSubscriber側でQoSプロファイルに互換性がないと通信が始まらない点です。
https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings/#qos-compatibilities
つまり、Publisher側のQoSプロファイルをあらかじめ知っておき、それと同等もしくはより緩いQoSプロファイルを設定しなくては、Subscriber側はトピックを受け付けることができません。
ここで問題なのは、現在のROS2のAPIには、Publisher側のQoSプロファイルが何なのかを知る手段が(僕の調べた範囲では)用意されていないことです。そのため、ソースコードが見れる場合には、Publisher側のQoSプロファイル設定の部分を確認した上で、Subscriber側を実装したりする必要があるように感じます。ソースコードが見れない場合にはいろんなプロファイルを試す手探りが必要です。もちろん、毎回Subscriber側は一番緩いQoSプロファイルにすれば何でも受け付けられますが、それではQoSプロファイルの意味が薄れます。
おそらくDDSベンダー実装のAPIからは何かしら知る手段が実装されているものと推察しています。ROS2のAPIからも呼べるようにAPIを拡張していきたいですね。
ROS2コマンドラインインタフェースでのQoSプロファイル設定
プログラミングしているときは、特にC++の場合、QoSプロファイルを設定し忘れると、ビルドに失敗するため、すぐ気づくことができます。
ですが、コマンドラインインタフェースを使うときは、QoSプロファイルの存在を忘れがちです。ターミナルからROS1のrostopic echo
のようなときの感覚でROS2のros2 topic echo
を実行すると、トピックの受信が始まらないことがあります。Publisherがおかしいのか、DDS通信がおかしいのか、DDSベンダー実装がおかしいのか、ROS2コマンドラインインタフェースがおかしいのか、と思い悩むことが何度かありました。
ROS2ではQoSプロファイルの設定は必須であるため、ros2 topic echo
にもQoSプロファイルを設定するための引数が用意されています。落ち着いて、QoSプロファイルを設定しましょう。大体の場合、悪いは他人ではなく自分です。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
$ ros2 topic echo --help
...
Output messages from a topic
positional arguments:
topic_name Name of the ROS topic to listen to (e.g. '/chatter')
message_type Type of the ROS message (e.g. 'std_msgs/String')
optional arguments:
...
--qos-profile {system_default,sensor_data,services_default,parameters,parameter_events,action_status_default}
Quality of service preset profile to subscribe with
(default: sensor_data)
--qos-reliability {system_default,reliable,best_effort}
Quality of service reliability setting to subscribe
with (overrides reliability value of --qos-profile
option, default: best_effort)
--qos-durability {system_default,transient_local,volatile}
Quality of service durability setting to subscribe
with (overrides durability value of --qos-profile
option, default: volatile)
...
RViz2でのQoSプロファイル設定
トピックを受信する機能の塊であるRViz2にも、当然QoSプロファイル設定の項目が追加されました。
スクリーンショットの一つ目はROS1のRVizのRobotModel
プラグインの設定項目です。二つ目はROS2のRViz2の場合です。RViz2の場合にはDescription Topic
の項目に、QoSプロファイルの設定を書く部分が増えていることがわかるでしょう。
- RViz (ROS1)
- RViz2 (ROS2)
ここで、もう一つの新しい発見がありました。ROS1ではrobot_descrpition
はパラメータで渡すものでしたが、ROS2ではトピックに変更されていたのです。 ロボットモデルを動的に更新することが容易になるため、これは良い改善なのですが、ROS1の時代から使っている僕からすると驚きでした。
https://github.com/ros2/robot_state_publisher/pull/9
さらに、robot_description
トピックはTransient Local
かつDepth = 1
(ROS1のLatchedトピックみたいな挙動)でPublishされているため、RViz2のRobotModel
プラグインの設定項目にもそのように設定しないとロボットモデルがいつまで経っても表示されません。僕はこのトピックへの変更と特殊なQoSプロファイルに気づくのに数時間を要しました。
皆さんも楽しいROS2ライフを!険しい山ほど登り甲斐があります笑
Comments powered by Disqus.