rclpy.Rateの注意点

　PythonでROS2のアクションサーバーノードを実装して動作させているときに、rclpy.Timer.Rateの使い方に苦戦したので調査したことをまとめます。
ROS2のバージョンはHumbleです。

1.はじめに

　ROS2ノード内で、一定周期で処理をループさせるためのアイテムとしてrclpy.Timer.Rateオブジェクトがあります。
生成時に実行周期をHz単位で指定しておいて、ループ処理の内側でRate.sleep()メソッドを実行すると指定周期になるように処理をブロックしてくれます。
ただ、一般的なsleep()処理と比べて難点が多く苦しんだので、戸惑った点を紹介します。

2.戸惑ったこと

• リソース解放漏れが発生して、sleep時間が遅延する
  　一番戸惑ったのがここです。Rateをループ内で使って、センサ値の変動を監視するようなROS2ノードの動作確認中に発生しました。
  関数の中でローカル変数としてRateオブジェクトを定義してループを回して、明示的にdestroy処理をしていない場合にリソースの使用量が増え続けて、不具合を起こします。
  
  検証のためのサンプルコードが以下になります。

rate_sample.py

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
import csv

class RateSampleNode(Node):
    def __init__(self):
        super().__init__("RateSampleNode")
        self._log_file = "./csv/rate_measure1.csv"
        self._start_time = self.get_clock().now().nanoseconds
        with open(self._log_file, 'a') as f:
            writer = csv.writer(f)
            writer.writerow(['time', 'time_span'])
        self.create_timer(0.12, self._count_01_sec)
               
    def _count_01_sec(self):
        rate = self.create_rate(100)
        time_before_sleep = self.get_clock().now()
        for i in range(10):
          # 実際のノードでは、無限ループ内で条件判定をしてbreakする
          rate.sleep()
        # 時刻の差分を計算(常に0.1秒に近い値が期待される)
        time_span = self.get_clock().now().nanoseconds - time_before_sleep.nanoseconds
        self.get_logger().info(f"time span:{time_span / 1e9:.6f}")
        with open(self._log_file, 'a') as f:
            writer = csv.writer(f)
            writer.writerow([(self.get_clock().now().nanoseconds - self._start_time) / 1e9, time_span / 1e9])
        return


def main():
    rclpy.init()
    node = RateSampleNode()
    executor = MultiThreadedExecutor() # マルチスレッドにした
    executor.add_node(node)
    executor.spin()

if __name__ == '__main__':
    main()

こちらを動作させると、50秒ほどでsleep時間が正確でなくなりました。

　※ rate_sample.pyのtime_spanを秒単位にしてログに記録したものです。縦軸がtime_spanの値で横軸が経過時間です。

rclpy.timer.rateオブジェクトを生成すると、ROS2ライブラリが内部的にTimerオブジェクトを生成し、
内部オブジェクトは関数処理が終わってスコープを抜けるだけでは破棄されないようです。
そのため、関数を繰り返し実行すると内部的なTimerオブジェクトが堆積してリソースが不足し、正確に時間を計測できなくなってしまいます。

• destroy処理関連の例外が発生
  上記の遅延問題に遭遇して調査すると、githubのissueに類似の問題が紹介されていました↓

ここでは、rateオブジェクトを定義したスコープを抜ける前にrclpy.node.destroy_rateメソッドを実行して、明示的にrateオブジェクト・付随する内部Timerオブジェクトを破棄する方法が推奨されています。
筆者の問題が起きていたノード群の中で試してみると、確かに遅延は発生しなくなりましたが、
と長時間動作させると以下の例外が発生するようになりました。

cannot use Destroyable because destruction was requested

こちらについては調査不足ですが、ros2_humbleのdestroyableクラスの実装にバグがあるらしく、特にマルチスレッドにノードを通信させているときに発生するようです。

• 最初のsleepは意図した時間にならない
  以下のリンクに詳しいですが、定義の仕方や使用するタイミングによっては、rate.sleep()に渡した時間と異なるスリープ時間になってしまいます。

3.実施した対応

通常のローカル変数と異なり、rateオブジェクトは定義したスコープを抜けてもリソースが解放されないということなので、ノードクラスのメンバ変数として、コンストラクタ内で定義するように修正しました。
先程のrate_sample.pyに修正を加えると、以下のようになります。

rate_sample_no_late.py

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
import csv

class RateSampleNode(Node):
    def __init__(self):
        super().__init__("RateSampleNode2")
        self._rate = self.create_rate(100)  # rateオブジェクトはメンバ変数として定義する
        self._start_time = self.get_clock().now().nanoseconds
        self._log_file = "./csv/rate_measure2.csv"
        with open(self._log_file, 'a') as f:
            writer = csv.writer(f)
            writer.writerow(['time', 'time_span'])
        self.create_timer(0.12, self._count_01_sec)
               
    def _count_01_sec(self):
        time_before_sleep = self.get_clock().now()
        for i in range(10):
          # 実際のノードでは、無限ループ内で条件判定をしてbreakする
          self._rate.sleep()  # 同一のメンバrate変数を使いまわす
        # 時刻の差分を計算(常に0.1秒に近い値が期待される)
        time_span = self.get_clock().now().nanoseconds - time_before_sleep.nanoseconds
        self.get_logger().info(f"time span:{time_span / 1e9:.6f}")
        with open(self._log_file, 'a') as f:
            writer = csv.writer(f)
            writer.writerow([(self.get_clock().now().nanoseconds - self._start_time) / 1e9, time_span / 1e9])
        return


def main():
    rclpy.init()
    node = RateSampleNode()
    executor = MultiThreadedExecutor() # マルチスレッドにした
    executor.add_node(node)
    executor.spin()

if __name__ == '__main__':
    main()

こちらのコードを先程の、リソースが足りず遅延するrate_sample.pyと同時に動作させた結果が以下になります。

遅延すること無く、一定の周期が計測され続けています。
ただ、0.01秒待機 × 10回ループで0.1秒が記録されてほしいところが、0.09秒に近い値が多く計測されています。これは、最初のsleepは意図した時間にならない問題が発生しているせいです。
ループの1回目だけは指定した0.01秒のスリープが計測されず、一瞬で処理が終了してしまい約9回分の合計時間になっています。
こちらが問題になるケースも多いでしょうが、開発していたノードは無限ループ内にrate.sleep()を入れてセンサ値や他スレッドの処理が特定の状態になるまで監視する意図だったので、最初の1回だけポーリング周期がずれても大きな問題にはならないということで、許容しました。

補足：ROS1との比較

　rclpyのRateクラスは、ROS1時代のrospy.rateクラスの後継という位置づけに思われます。
既存のROS1コードをROS2に変換する際などにrospy.rate → rclpy.Rateと直訳したくなりますが、ROS2での周期処理は別の方法が標準的なようです。
　その別の方法とは、上記のコード例にも登場したrclpy.Node.Timerクラスの使用です。
ノードに紐付いたTimerオブジェクトに処理と周期を指定すると、周期実行されます。

class RateSampleNode(Node):
  def __init__(self):
    # self._count_01_secを0.12秒に1回実行する
    self.create_timer(0.12, self._count_01_sec)
  def _count_01_sec(self):
    # 何らかの処理

4.まとめ

ROS2のPythonライブラリであるrclpyの、周期処理を制御するためのRclpy.Timer.Rateオブジェクトの使用上の注意点でした。

けっこうクセが強くて落とし穴も多いので、rclpy.Node.Timerを活用するなど使用を避けられる場面では避けたほうが良いのかなと思いました。

destroyableクラスのエラーについても調査して、より良い解決策が判明したら追記しようと思います。
読んでいただき、ありがとうございました。