記事の概要
「（２）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）はプログラム中では以下のように作成されています。
// 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）はプログラム中では以下のように作成されています。
// 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 から引用すると以下の形式です。
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("受信トピック名", 受信データバッファサイズ, コールバック);
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点灯や消灯が実行されていることが分かります。