ROS 2 移行（rclcpp::Time）

## はじめに

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

:::note info
この記事は[ROSに関する投稿](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d)の一部です。
目次は[こちら](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d#目次)
:::

### 対象読者

* 既に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は時間タイプが無いので、初期化済みの値と比較すると、異なる時間タイプ間の比較となり、前述の日付比較に記載した例外が発生します。

https://design.ros2.org/articles/clock_and_time.html

### ナノ秒の扱い

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の扱い変更に伴う部分が、原因がわかるまでに苦労しました。

https://github.com/ros2/rclcpp/issues/2113

https://github.com/ros2/rclcpp/issues/2025

上記リンクの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の場合

```cpp
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 の場合 (わざと時間変換マクロを使用しています）

```cpp
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

https://docs.ros.org/en/humble/How-To-Guides/Migrating-from-ROS1/Migrating-CPP-Packages.html#usages-of-ros-time

* rcutilsソース

https://github.com/ros2/rcutils/blob/humble/include/rcutils/time.h

## おわりに

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