ROS 2 移行（rclcpp::Time） #C++

はじめに

今回の記事では、ROSから ROS 2 へ移行する際にTimeクラスに関するコーディングで変更する必要のあった内容について記載します。
Timeクラスは仕様がいくつか変更されており、ROSと同じコーディングができない部分がありましたので、これから移行を考えている方の参考になれば幸いです。

この記事はROSに関する投稿の一部です。
目次はこちら

対象読者

既にROSを触ったことがある人で、ROSから ROS 2 への移行を考えているエンジニア

前提条件

今回の記事は、以下の環境で動かすことを前提に記載しています。

	条件
OS	Ubuntu 22.04
ROS	ROS 2 humble

Timeクラス関連仕様の差異

クラス

ROSのWallTimeクラスとTimeクラスは、ROS 2 ではTimeクラスに集約され、Clockクラスのクロックタイプで使用する時間タイプを指定します。

クロックタイプ	内容	備考
RCL_SYSTEM_TIME	システムクロック	ros::WallTimeと同等
RCL_ROS_TIME	ROS 2 システム上のクロック	ros::Timeと同等
RCL_STEADY_TIME	ハードウェアクロック

日付比較

Timeクラスに集約されたこともあり、異なる時間タイプ間の比較を行うと、例外が発生します。
「can't compare times with different time sources」

Zeroの扱い

Zeroは未初期化の扱いというのは一緒ですが、isZeroメソッドは無くなりました。
Zeroは時間タイプが無いので、初期化済みの値と比較すると、異なる時間タイプ間の比較となり、前述の日付比較に記載した例外が発生します。

ナノ秒の扱い

ros::Timeでは、secは秒のみでnsecはsec中のナノ秒でしたので、secとnsecを合わせてナノ秒の現時点時刻を取得していましたが、rclcpp::Timeでは、nanosecondsは秒も含まれた値となり、secondsは小数点以下でマイクロ秒までが取得できます。

現在時間取得の変更

ros::Time::now() -> get_clock()->now() に置き換えます。

RCL_ROS_TIME以外の時間を使用したい場合は、rclcpp::Clock(クロックタイプ).now() で取得します。

Zero比較の変更

差異の中でも、Zeroの扱い変更に伴う部分が、原因がわかるまでに苦労しました。

上記リンクのIssueに書かれているように、Zeroの値は入れられますが初期値として扱うことができませんでした。
そのため、Zeroはnullptrで判断するように変更しました。
1ナノ秒で初期化することも考えましたが、初期化されたものを未初期化とするのは違和感があるので、nullptrを採用しました。

ナノ秒取得の変更

ROSでは、秒とナノ秒は別々のプロパティで取得していましたが、ROS 2 では秒も含めたナノ秒のみになりました。

(sec * std::pow(10, 9)) + nsec → nanoseconds()になります。

seconds()は小数点以下がマイクロ秒の値になります。

秒：1703735654、ナノ秒：197938808 の場合
seconds() ⇒ 1703735654.197939
nanoseconds() ⇒ 1703735654197938808

また、秒・ミリ秒・マイクロ秒・ナノ秒などを変換するマクロが用意されています。

マクロ	内容	値の例
RCL_NS_TO_S	ナノ秒から秒を抜き出す。	1703735654
RCL_NS_TO_MS	ナノ秒からミリ秒までを抜き出す。	1703735654197
RCL_NS_TO_US	ナノ秒からマイクロ秒までを抜き出す。	1703735654197939
RCL_S_TO_NS	秒をナノ秒の桁にする。	1703735654000000000
RCL_MS_TO_NS	ミリ秒をナノ秒の桁にする。	1703735654197000000
RCL_US_TO_NS	マイクロ秒をナノ秒の桁にする。	1703735654197939000

コーディング例

変更点をふまえた、簡単なコーディング修正例を記載します。

ROSの場合

ros::Time pre_ = ros::Time(0);

void test() {
  for (int i = 0; i < 2; i++) {
    compare(ros::Time::now());
    ROS_INFO("time: %u.%09u", pre_.sec, pre_.nsec);
  }
}

void compare(ros::Time& time) {
  if (pre_.isZero() || (!pre_.isZero() && time > pre_)) {
    pre_ = time;
  }
}

ROS 2 の場合 (わざと時間変換マクロを使用しています）

std::shared_ptr<rclcpp::Time> pre_ = nullptr;

void test() {
  for (int i = 0; i < 2; i++) {
    std::unique_ptr<rclcpp::Time> now = std::make_unique<rclcpp::Time>(get_clock()->now());
    compare(std::move(now));
    RCLCPP_INFO(
        get_logger(), "time: %lld.%09lld", RCL_NS_TO_S(pre_->nanoseconds()),
        pre_->nanoseconds() - RCL_S_TO_NS(RCL_NS_TO_S(pre_->nanoseconds())));
  }
}

void compare(std::shared_ptr<rclcpp::Time>& time) {
  if (pre_ == nullptr || (pre_ != nullptr && time->nanoseconds() > pre_->nanoseconds()) {
    pre_ = time;
  }
}

参考文献

Migrating from ROS to ROS 2

rcutilsソース

おわりに

実際に ROS 2 にマイグレーションする中で発生した問題は、すぐに原因がわかるものもあれば、調査に時間を要するものもありました。
一般的な内容は多くの方が投稿されているので、原因特定が大変だったり、置き換え方法を工夫した点について、今後も公開していきたいと思います。