iRobot Create 2でROSを勉強する（３）rostopic

# 記事の概要
[「（２）roslaunch」](https://qiita.com/Kosuke_Matsui/items/3d66331c10c096c3b1ba)の続きです。

iRobot Create 2は掃除ロボット「ルンバ600シリーズ」から掃除機能を取り除き、趣味や研究用の改造を可能にしたロボットです。
このiRobot Create 2を使って、ロボット用のソフトウェアプラットフォームROSの使い方を勉強したいと思います。

本記事では、rostopicについて調べます。

<img width="20%" alt="IMG_1793.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/57182bc1-b4e1-fe6b-1a3a-ef8b304d37a0.jpeg">

# 動作確認
ROSトピックはROSノード間でメッセージを送受信するための方法です。

ここでは実際にrostopicを用いて、メッセージを送受信してみます。

## ROSノードの起動

roslaunchコマンドでROSノードを起動させます。

```console
$ roslaunch ca_driver create_2.launch
```

新しい端末を開いて、以降のコマンドはこちらに入力します。
ROSノードの関係を図で表示するために以下のコマンドを実行します。

```console
$ rosrun rqt_graph rqt_graph
```

ROSノード「ca_driver」が送信側で、ROSノード「robot_state_publisher」が受信側であることが分かります。

<img width="70%" alt="Screenshot from 2019-10-31 15-13-44.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/6f97c3e7-488f-ca61-883d-d6b7582487de.png">

このROSノード間のメッセージ送受信をROSトピックを用いて行います。

# 送信トピック
どのような送信トピック(publisher)があるのかを確認してみます。
Ctrl+Cでrqt_graphを終了させるか、新しい端末を開いて以下のコマンドを実行すると、送信トピックの一覧が表示されます。

```console
$ rostopic list -p
/battery/capacity
/battery/charge
/battery/charge_ratio
/battery/charging_state
/battery/current
/battery/temperature
/battery/voltage
/bumper
/clean_button
/day_button
/diagnostics
/dock_button
/hour_button
/ir_omni
/joint_states
/minute_button
/mode
/odom
/rosout
/rosout_agg
/spot_button
/tf
/tf_static
/wheeldrop
```

これらの送信トピックを用いてiRobot Create 2の状態の情報を取得することができます。
送信トピックを受信して確認するには、以下のコマンドを実行します。

- 充電率の受信

```console
$ rostopic echo battery/charge_ratio
data: 0.811943531036
---
data: 0.811943531036
---
data: 0.811943531036
---
data: 0.811943531036
---
data: 0.811943531036
---
data: 0.811943531036
---

```

- 距離センサなどバンパーの状態の受信

```console
$ rostopic echo bumper
header: 
  seq: 52628
  stamp: 
    secs: 1572503864
    nsecs:  49335957
  frame_id: "base_footprint"
is_left_pressed: False
is_right_pressed: False
is_light_left: False
is_light_front_left: False
is_light_center_left: False
is_light_center_right: False
is_light_front_right: True
is_light_right: False
light_signal_left: 12
light_signal_front_left: 2
light_signal_center_left: 7
light_signal_center_right: 31
light_signal_front_right: 442
light_signal_right: 284
---
header: 
  seq: 52629
  stamp: 
    secs: 1572503864
    nsecs: 149341018
  frame_id: "base_footprint"
is_left_pressed: False
is_right_pressed: False
is_light_left: False
is_light_front_left: False
is_light_center_left: False
is_light_center_right: False
is_light_front_right: True
is_light_right: False
light_signal_left: 13
light_signal_front_left: 4
light_signal_center_left: 5
light_signal_center_right: 34
light_signal_front_right: 444
light_signal_right: 281
---

```

- CLEANボタン押下状態の受信
コマンド起動後、CLEANボタンを押すたびに反応します。

```console
$ rostopic echo clean_button

---

---

---
```

# 受信トピック

どのような受信トピック(subscriber)があるのかを確認してみます。
以下のコマンドを実行すると、送信トピックの一覧が表示されます。

```console
$ rostopic list -s
/check_led
/cmd_vel
/debris_led
/define_song
/dock
/dock_led
/joint_states
/play_song
/power_led
/rosout
/set_ascii
/spot_led
/undock
```

これらの受信トピックを用いてiRobot Create 2の設定や操作ができます。
そのためには、まず使用したい受信トピックのメッセージタイプを調べます。
ここではspot_ledを試してみます。

```console
$ rostopic type spot_led
std_msgs/Bool
```

メッセージタイプがstd_msgs/Boolであることが分かりました。
次にトピックへデータを送信します。

```
$ rostopic pub -1 spot_led std_msgs/Bool True
publishing and latching message for 3.0 seconds
```

このコマンドを実行すると、iRobor Create 2のSPOTボタンのLEDが緑色に点灯します。ここでTrueをFalseにすればLEDが消灯します。
pubコマンドは以下の形式になります。

```
$ rostopic pub -1 [受信トピック] [メッセージタイプ] [送信データ]
```

-1は、rostopicを１回のメッセージ送信後に終了させることを意味しています。

# ROSノード
ROSノードがどのようにして、これらの送受信メッセージを作っているかを調べてみます。

## 送信トピック（publisher）

送信トピック（publisher）はプログラム中では以下のように作成されています。

```c++:create_autonomy/ca_driver/src/create_driver.cpp_125行目
  // Setup publishers
  odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
  clean_btn_pub_ = nh.advertise<std_msgs::Empty>("clean_button", 30);
  day_btn_pub_ = nh.advertise<std_msgs::Empty>("day_button", 30);
```
nhはクラスCreateDriverのコンストラクタに参照渡しされる引数で、クラスのプライベート変数ros::NodeHandle nh_はこの引数で初期化されます。

```c++
CreateDriver::CreateDriver(ros::NodeHandle& nh)
  : nh_(nh),
    priv_nh_("~"),
```

以下の形式を取ればいいことが分かります。

```c++
変数名 = nh.advertise<変数の型>("送信データ", 送信データバッファサイズ);
```

この送信データはros::Publisherクラスの関数void 	publish (const M &message) constを実行すれば、トピックとして送信されます。
例えばcharge_ratio_pub_はmain関数で無限ループで以下を実行して、周期的にバッテリーの充電状態を送信し続けています。なのでrostopic echoコマンドで拾うと、即座に表示されるわけです。

```c++
charge_ratio_pub_.publish(float32_msg_);
```

一方でday_btn_pub_はdayボタンを押下した時にだけトピックを送信しているため、先の動作確認ではdayボタンを押した際にのみメッセージが表示されたのでした。

```c++
  if (robot_->isDayButtonPressed())
  {
    day_btn_pub_.publish(empty_msg_);
  }
```

## 受信トピック（subscriber）

受信トピック（subscriber）はプログラム中では以下のように作成されています。

```c++:create_autonomy/ca_driver/src/create_driver.cpp_112行目
  // Setup subscribers
  cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &CreateDriver::cmdVelCallback, this);
  debris_led_sub_ = nh.subscribe("debris_led", 10, &CreateDriver::debrisLEDCallback, this);
  spot_led_sub_ = nh.subscribe("spot_led", 10, &CreateDriver::spotLEDCallback, this);
```

以下の形式を取ればいいことが分かります。
これはClass Methodsと呼ばれる形式のようです。他のサンプルでよく見るのはFunctions形式と思われます。

```c++:
変数名 = nh.subscribe("受信トピック名", 受信データバッファサイズ, &クラス::コールバック, コールバックを呼び出したオブジェクトのアドレス);
```

&クラス::コールバックはクラスのメンバ関数であるコールバックを示します。

https://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers から引用すると以下の形式です。

```c++:上記引用サイトの2.3.2_Class_Methods
void Foo::callback(const std_msgs::StringConstPtr& message)
{
}
 
...
Foo foo_object;
ros::Subscriber sub = nh.subscribe("my_topic", 1, &Foo::callback, &foo_object);
```

ちなみに、よく見るFunctions形式は以下になります


```c++:
変数名 = nh.subscribe("受信トピック名", 受信データバッファサイズ, コールバック);
```

```c++:上記引用サイトの2.3.1_Functions
void callback(const std_msgs::StringConstPtr& str)
{
...
}

...
ros::Subscriber sub = nh.subscribe("my_topic", 1, callback);
```


- SPOT LED点灯処理

SPOTボタンのLEDを点灯させるための受信トピックは以下で作られています。

```c++
  spot_led_sub_ = nh.subscribe("spot_led", 10, &CreateDriver::spotLEDCallback, this);
```


```c++:
void CreateDriver::spotLEDCallback(const std_msgs::BoolConstPtr& msg)
{
  robot_->enableSpotLED(msg->data);
}
```

rostopic pubコマンドで"spot_led"を指定することで、spotLEDCallbackが呼ばれ、メッセージタイプのstd_msgs/Boolに設定したTrueやFalseの値がmsg->dataに渡されて、LED点灯や消灯が実行されていることが分かります。

# 関連する記事
[（１）準備](https://qiita.com/Kosuke_Matsui/items/95e90b372ccb749cf1a2)
[（２）roslaunch](https://qiita.com/Kosuke_Matsui/items/3d66331c10c096c3b1ba)