ROS演習４-2019：トピックで通信しよう！

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

1. Publisher

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

#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>("/cmd_vel", 10);
    // マスターにgeometry_msgs::Twist型のデータを送ることを伝える
    // マスターは/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_make
$ roscore
$ rosrun my_teleop my_teleop_node

なお、catkin_makeでビルドを失敗する場合はCMakeLists.txtやpackage.xmlが間違っている場合があります。その場合はこのファイルmy_teleop.tgzをクリックしてダウンロードし、~/catkin_ws/srcの中にコピーしてください。次の手順で展開したあとにcatkin_makeを再実行してください。
$ cd ~/catkin_ws/src
$ tar xvzf my_teleop.tgz

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("/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_make
$ rosrun my_subscriber my_subscriber_node

なお、catkin_makeでビルドを失敗する場合はCMakeLists.txtやpackage.xmlが間違っている場合があります。その場合はこのファイルmy_subscriber.tgzをクリックしてダウンロードし、~/catkin_ws/srcの中にコピーしてください。次の手順で展開したあとにcatkin_makeを再実行してください。
$ cd ~/catkin_ws/src
$ tar xvzf my_subscriber.tgz

3. シミュレータでの確認
演習3のturtlebot3シミュレータを起動し、my_teleop_nodeとmy_subscriber_nodeの動作を確認しましょう。シミュレータを次のコマンドで起動します。
$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch

次にmy_teleop_nodeを起動した端末にマウスカーソルを持って行いきます。
f, b, l, rキーでロボットが移動したら成功。キーを押した後はエンターキーを押す。
このシミュレータの場合は、/cmd_velをPublishするだけでロボットがその速度で移動します。

次にSubscriberのプログラムが動作しているか確認するために、my_subscriber_nodeを起動した端末を見ましょう。速度が表示されていたら成功です。

お疲れ様！

終わり

4. ホームワーク

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