記事の概要

「（２）roslaunch」の続きです。

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

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

動作確認

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

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

ROSノードの起動

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

$ roslaunch ca_driver create_2.launch

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

$ rosrun rqt_graph rqt_graph

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

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

送信トピック

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

$ 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の状態の情報を取得することができます。
送信トピックを受信して確認するには、以下のコマンドを実行します。

充電率の受信

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

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

$ 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ボタンを押すたびに反応します。

$ rostopic echo clean_button

---

---

---

受信トピック

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

$ 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を試してみます。

$ 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）はプログラム中では以下のように作成されています。

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_はこの引数で初期化されます。

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

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

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

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

charge_ratio_pub_.publish(float32_msg_);

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

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

受信トピック（subscriber）

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

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形式と思われます。

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

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

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

上記引用サイトの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形式は以下になります

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

上記引用サイトの2.3.1_Functions

void callback(const std_msgs::StringConstPtr& str)
{
...
}

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

SPOT LED点灯処理

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

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

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点灯や消灯が実行されていることが分かります。

関連する記事

（１）準備
 （２）roslaunch