ROS講座48 ROSの時間システム

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

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


インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ROSの時間はシミュレーションなどを正しく行うために独自の時間システムを持ちます。シミュレーションやrosbagの再生をするときにはこの点を知らないと、勘違いをすることがあるのでその点を解説します。

# ROSの時間形式
ROSの時間は`int32 sec`と`int32 nsec`の組で表されます。`sec`はunix epoch timeで`nsec`は秒以下をナノ秒単位で表します。

# ROS TimeとWall Time
ROSは2種類の時間システムを持ちます。Wall TimeはコンピューターのUnix時間を表します。つまり`date`コマンドで取得する値と同じです。常に実時間を表すので、処理時間を計測することに使えます。
一方ROS TimeはROSのシステム上での時間を示します。デフォルトではにWall TimeとROSTimeは同じですが、rosparamで`/use_sim_time`をtrueにして、ROSトピックの`/clock`をpublishすることで、ROS Timeを変更することが出来て、過去の時間になったり、倍速で進んだりします。これが起こる状況は以下の2つがあります。

* gazebo simulationを行っている時  
処理落ちしたり、ゆっくりシミュレーションを行うなど、「時間がゆがむ」ことがあります。これにROSの処理を合わせることができます。
* rosbagを再生している時  
長いrosbagを数倍速で再生するときがあります。それに伴う処理も合わせて早くすることができます。

# rosbag再生におけるROS Timeの挙動

## ソースコード
```c++:adv_lecture/src/adv_time.cpp
#include "ros/ros.h"
#include "std_msgs/Empty.h"
#include <time.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "adv_time");
  ros::NodeHandle n;
  ros::Publisher tick_pub = n.advertise<std_msgs::Empty>("time_tick", 10);

  ros::Time::waitForValid();
  ros::Time ros_begin = ros::Time::now();
  ros::WallTime wall_begin = ros::WallTime::now();

  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    ROS_INFO("TICK");
    std_msgs::Empty msg;
    tick_pub.publish(msg);

    ros::Time ros_now = ros::Time::now();
    ros::Duration ros_duration = ros_now - ros_begin;
    ROS_INFO("ROS: %u.09%u", ros_duration.sec, ros_duration.nsec);

    ros::WallTime wall_now = ros::WallTime::now();
    ros::WallDuration wall_duration = wall_now - wall_begin;
    ROS_INFO("WALL:%u.%09u", wall_duration.sec, wall_duration.nsec);

    char date[64];
    time_t t = ros::Time::now().sec;
    strftime(date, sizeof(date), "%Y/%m/%d %a %H:%M:%S", localtime(&t));
    ROS_INFO("%s", date);

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

* `ros::Time::now()`はROS timeでの現時刻を取得する関数です。ros::Time同士の差は時刻ではなく時間になり、これは`ros::Duration`という型で表されます。演算としては以下が可能です。
    * `ros::Time` = `ros::Time` ± `ros::Duration` (右辺を交換するとコンパイルエラー、結果がマイナスになると実行時エラー)
    * `ros::Duration` = `ros::Time` - `ros::Time`
    * `ros::Duration` = `ros::Duration` ± `ros::Duration`
* `ros::WallTime::now()`はwall timeでの現時刻を取得します。これの差は`ros::WallDuration`になります。
* ros::Time.secをヒューマンリーダブルな形にするには`strftime`関数を使えば良いでしょう。

## 実行
以下の3つを実行します。各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:1つ目のターミナル
roscore
```

2つ目のターミナルではまず`/use_sim_time`にtrueを設定します。そのあとrosbagを再生します。`-r 2.0`は2倍速で再生、`--clock`は/clockのROSトピックをpublishするオプションです。

```shell:2つ目のターミナル
rosparam set /use_sim_time true
roscd adv_lecture
rosbag play resources/empty.bag -r 2.0 -l --clock
```

```shell:3つ目のターミナル
rosrun  adv_lecture adv_time 
```

3つ目のターミナルでは以下のような出力を得ます。ROS timeを見ると約1秒ごとに動いているように見えますが、wall timeを見ると0.5秒ごとになっていて実際は2倍速になっていることがわかります。

```text
[ INFO] [1588465509.046435395, 1534860896.979195167]: TICK
[ INFO] [1588465509.046755713, 1534860896.979195167]: ROS: 0.0921598734
[ INFO] [1588465509.046948414, 1534860896.979195167]: WALL:0.000713369
[ INFO] [1588465509.047135942, 1534860896.979195167]: 2018/08/21 Tue 23:14:56
[ INFO] [1588465509.539869812, 1534860897.962216420]: TICK
[ INFO] [1588465509.540049497, 1534860897.962216420]: ROS: 1.094619987
[ INFO] [1588465509.540087078, 1534860897.962216420]: WALL:0.493926072
[ INFO] [1588465509.540166700, 1534860897.962216420]: 2018/08/21 Tue 23:14:57
[ INFO] [1588465510.045031840, 1534860898.974928931]: TICK
[ INFO] [1588465510.045159103, 1534860898.974928931]: ROS: 2.0917332498
[ INFO] [1588465510.045183740, 1534860898.974928931]: WALL:0.999025030
[ INFO] [1588465510.045245518, 1534860898.974928931]: 2018/08/21 Tue 23:14:58
[ INFO] [1588465510.543725878, 1534860899.971824893]: TICK
[ INFO] [1588465510.543885454, 1534860899.971824893]: ROS: 3.0914228460
[ INFO] [1588465510.543920453, 1534860899.971824893]: WALL:1.497760804
[ INFO] [1588465510.543990716, 1534860899.971824893]: 2018/08/21 Tue 23:14:59
```

上の3つのターミナルでの処理を以下のlaunchでまとめて行えます。

```xml:adv_lecture/launch/double_speed.launch
<launch>
  <rosparam param="/use_sim_time">true</rosparam>
  <node name="rosbag_play" pkg="rosbag" type="play" args="$(find adv_lecture)/resources/empty.bag -r 2.0 -l --clock" />
  <node name="adv_time" pkg="adv_lecture" type="adv_time" output="screen" />
</launch>
```

## /clockの確認
上記の実行をしている時に`rostopic echo /clock`でpublishされているROS timeを確認することができます。

# 参考
[roswiki Time](http://wiki.ros.org/roscpp/Overview/Time)
[ROS time リファレンス](http://docs.ros.org/diamondback/api/rostime/html/classros_1_1Time.html)
[Wall time リファレンス](http://docs.ros.org/electric/api/rostime/html/classros_1_1WallTime.html)
[timeの変換](https://www.mm2d.net/main/prog/c/time-01.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)