ROS演習４-2020:トピック通信しよう！

今回はROSの通信方式であるトピックを学びます。

1. Publisher

キーボードからロボットを操縦するmy_teleopパッケージを作ろう!
ROS演習２と同じ要領でmy_teleopパッケージを作ります。忘れた人はROS演習２を見ながらやろう。今回の例ではhelloをmy_teleopに置き換えます。CMakeLists.txtもROS演習２と同様に編集する必要がある。このプログラムはメッセージの送リ手であるpublisher（配信者)プログラムの簡単な例にもなっています。プログラムの説明はソースコード内のコメントを参照してください。テキストエディタ(gedit)にコピペしてmy_teleop_node.cppと名前を付けて保存する。
保存ディレクトリは~/catkin_ws/src/my_teleop/srcの中です。

// ファイル名　my_teleop_node.cpp
#include "ros/ros.h"  // rosで必要はヘッダーファイル
#include <geometry_msgs/Twist.h> // ロボットを動かすために必要
using namespace std;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_teleop_node");
    // initでROSを初期化し、my_teleop_nodeという名前をノードにつける
    // 同じ名前のノードが複数あるとだめなので、ユニークな名前をつける

    ros::NodeHandle nh;
    // ノードハンドラの作成。ハンドラは必要時に起動される。

    ros::Publisher  pub;
    // パブリッシャの作成。トピックに対してデータを送信。

    ros::Rate rate(10);
    // ループの頻度を設定するためのオブジェクトを作成。この場合は10Hz、1秒間に10回数、1ループ100ms。

    geometry_msgs::Twist vel;
    // geometry_msgs::Twist　この型は並進速度と回転速度(vector3:3次元ベクトル) を合わせたもので、速度指令によく使われる

    pub= nh.advertise<geometry_msgs::Twist>("/create1/cmd_vel", 10);
    // マスターにgeometry_msgs::Twist型のデータを送ることを伝える
    // マスターは/create1/cmd_velトピック(1番目の引数）を購読する
    // 全てのノードにトピックができたことを知らせる(advertise)。
    // 2番目の引数はデータのバッファサイズ

    cout << "f: forward, b: backward, r: right, l:left" << endl;

    while (ros::ok()) { // このノードが使える間は無限ループする
        char key;  // 入力キーの値

        cin >> key;　// 標準入力からキーを読み込む
        cout << key << endl; // 読み込んだキーの値を標準出力へ出力

        switch (key) {
        case 'f': // fキーが押されていたら
            vel.linear.x  =  0.5;
            break;
        case 'b':
            vel.linear.x  = -0.5;
            break;
        case 'l':
            vel.angular.z =  1.0;
            break;
        case 'r':
            vel.angular.z = -1.0;
            break;
            // linear.xは前後方向の並進速度(m/s)。前方向が正。
            // angular.zは回転速度(rad/s)。反時計回りが正。
        }

        pub.publish(vel);    // 速度指令メッセージをパブリッシュ（送信）
        ros::spinOnce();     // １回だけコールバック関数を呼び出す
        vel.linear.x  = 0.0; // 並進速度の初期化
        vel.angular.z = 0.0; // 回転速度の初期化
        rate.sleep();        // 指定した周期でループするよう寝て待つ

    }

    return 0;
}

以下のコマンドでビルドしよう。
$ cd ~/catkin_ws
$ catkin build my_teleop

2. Subscriber

次に、メッセージの受け手であるsubscriber（購読者)の簡単なプログラムを示します。このプログラムはmy_teleop_nodeがパブリッシュするトピック/cmd_velをサブスクライブして、並進速度(Linear Velocity)と角速度(Angular Velocity)を標準出力に出力する簡単なプログラムです。

// ファイル名　my_subscriber_node.cpp                                            
#include <ros/ros.h>  // rosで必要はヘッダーファイル                             
#include <geometry_msgs/Twist.h> // ロボットを動かすために必要                   
#include <iostream>
using namespace std;

// コールバック関数。並進、回転速度の表示。                                      
void callback(const geometry_msgs::Twist::ConstPtr& vel) {
  cout << "Linear :" << vel->linear.x << endl;
  cout << "Angular:" << vel->angular.z << endl;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "my_subscriber_node");
  ros::NodeHandle nh;

  //subscriberの作成。トピック/cmd_velを購読する。バッファ数は10。               
  ros::Subscriber sub = nh.subscribe("/create1/cmd_vel", 10, callback);

  // コールバック関数を繰り返し呼び出す。whileループが必要な場合はspinOnce()を使\
う。                                                                             
  ros::spin();

  return 0;
}

ここで、馴染みのないコールバック関数callbackが登場しています。コールバック関数はIT用語辞典によると「コールバック関数とは、プログラム中で、呼び出し先の関数の実行中に実行されるように、あらかじめ指定しておく関数。」と定義されています。

通常はマウスで線を描くなどの処理を実装するような、あるイベントが起きた時に処理をするプログラムを実装する場合に使われます。通常は、マウスのイベント処理などの場合のようにマウスのボタンが押されたり、移動したときに自動的にコールバック関数が呼び出されますが、ROSではspinOnce()やspin()で明示的に呼び出さなければいけません。spinOnce()はコールバック関数を一度だけ呼び出すので通常はwhileループの中で使います。spin()はノードが動いている間、コールバック関数を呼び続けます。ROSではメッセージの受け渡しにコールバック関数を使います。

また、callback関数の引数はトピックのメッセージ型でT型の場合はconst T::ConstPtr &でなければいけません。::ConstPtrと馴染みのないものが出てきましたね。ConstPtrはBoostのShared Pointerにtypedefされています。このShared Pointerはスマートポインタという賢いポインタで、普通のポインタで使い終わったら必要となるdeleteしなくても自動的にメモリを開放してくれるすぐれものです。

では、今までパッケージを作った要領で、my_subscriberパッケージを作ろう。
~/catkin_ws/src/my_subscriberディレクトリの下にCMakeLists.txt, package.xml, src
などを作ることになる。上のプログラムのファイル名はmy_subscriber_node.cppとする。

以下のコマンドでビルドして実行しよう。
$ cd ~/catkin_ws
$ catkin build my_subscriber
$ rosrun my_subscriber my_subscriber_node

3. シミュレータでの確認

端末を４つ開く。
1番目の端末で次のコマンドでマスターを起動する。
- $ roscore
２番目の端末でroombaシミュレータを起動する。
- $ roslaunch ca_gazebo create_empty_world.launch
3番目の端末でmy_subscriberノードを起動する。
- $ rosrun my_subscriber my_subscriber_node
４番目の端末でmy_teleopノードを起動する。
- $ rosrun my_teleop my_teleop_node
次にmy_teleopノードを起動した端末にマウスカーソルを持って行いきます。
f, b, l, rキーでロボットが移動したら成功。キーを押した後はエンターキーを押さないと動かない。
このシミュレータの場合は、/create1/cmd_velをPublishするだけでロボットがその速度で移動します。
最後に、my_subscriberノードを起動した端末を見ましょう。速度が表示されていたら成功です。

お疲れ様！

終わり

4. ホームワーク

キーsを押すと、roombaが停止するようにmy_teleop_node.cppのソースコードを変更しよう！
my_teleop_node.cppではキーボードのf, b, r, lを押した後にエンターキーを押すと、一定の速度でturtlebot3が動き出す。キーを押すたびに並進速度が0.01 [m/s]ずつ増減、回転角度が0.1 [rad/s]ずつ変化するようにソースコードを変更しよう！
指定した時間[s]だけ、指定した速度[m/s]と角速度[rad/s]で移動するプログラムを作ろう！
roombaの移動する軌跡が1辺x[m]の正方形になるようプログラムを変更しよう！
roombaの移動する軌跡が半径x[m]の円になるようプログラムを変更しよう！
my_teleop_node.cppは、演習keyboard_teleopとは違いf, b, r, lのキーを押した後にエンターキーが必要です。エンターキーを押さずに、f, b, r, lキーを押すとロボットが移動するようにソースコードを変更しよう。
なお、my_teleop_node.cppではstd::cinでキーボードから入力しますがキーが入力されるまで待状態になります。これをブロックとよびます。エンターキーが入力されると次の処理に移ります。Linuxでブロックしないの入出力を実現する場合はselectシステムコールを使います。次のリンクが参考になります。
- ROS Answer: Keyboard key pressed