プログラミングによるトピック通信の実装

Yuji Yamauchi 演習A
目的

プログラミングによる配信と購読の実装方法を学びます．さらに，シミュレータ上のロボットをプログラミングにより制御する方法を習得します．なお，C++についてはロボットプログラミングⅡにて習得する予定の言語です．本演習の冒頭にてC++の資料を配布するので，そちらを参照しながら演習を進めます．pythonで実装したい場合にはこちらのページを参照してください．

 

プログラミングによる配信と購読

前回はノード，トピック，メッセージについて学びました．その過程でROSのコマンドを実行することで，配信及び購読ができることを確認しました．ここでは，プログラムにより配信及び購読する方法を習得します．これにより，ロボットを自律的に動かすことが可能となります．

1. パッケージの作成
新たにパッケージを作成しましょう．以前に作成したワークスペース「practice_ws」の中に新しくパッケージ「turtlesim_practice」を作成します．依存パッケージとして下記を指定してください．(必要ないパッケージもありますが，今後使用する可能性があるので全てを指定してください．)

geometry_msgs message_generation rosconsole roscpp roscpp_serialization roslib rostime std_msgs

パッケージの作成方法がわからない人はこちらのページを参照してください．

2. プログラミング
前回，ROSのコマンドを使用して配信及び購読することでシミュレータ上のロボット(亀)を動かしました．全く同じ内容をプログラムによって実現しましょう．パッケージ「turtlesim_practice」の中のsrcに「move_turtlesim.cpp」というファイルを作成し，下記のプログラムを記述します．

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
  
class turtleSim{
public:
    turtleSim();
    ~turtleSim();
    void poseCallback(const turtlesim::PoseConstPtr& pose);
    void setMoveVector(float linear_x, int cnt);
    void timerCallback(const ros::TimerEvent&);
  
private:
    ros::Publisher twist_pub;
    ros::Subscriber pose_sub;
    ros::Timer timer;
    ros::NodeHandle nh;
};
  
turtleSim::turtleSim(){
    twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this);
    timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this);
  
    geometry_msgs::Twist twist;
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;
    twist_pub.publish(twist);
}
  
turtleSim::~turtleSim(){
  
}
 
void turtleSim::poseCallback(const turtlesim::PoseConstPtr& pose){
    ROS_INFO("x:%f",pose->x);
}
  
void turtleSim::timerCallback(const ros::TimerEvent&){
    setMoveVector(0.2, 10);
    setMoveVector(-0.2, 10);
}
  
void turtleSim::setMoveVector(float linear_x, int cnt){
    int i;
    geometry_msgs::Twist twist;
    ros::Rate loop_rate(10);
  
    for(i = 0;i < cnt; i++){
        twist.linear.x = linear_x;
        twist_pub.publish(twist);
        loop_rate.sleep();
    }
}
  
int main(int argc, char **argv){
    ros::init(argc, argv, "move_turtlesim");
  
    turtleSim turtlesim;
  
    ros::spin();
    return 0;
}

3. 実行
プログラムの中身を理解する前に実行してみましょう．CMakeLists.txtを編集し，ビルドしましょう．CMakeLists.txtの編集とビルドの仕方を忘れた人は，こちらのページを参照してください．無事にビルドできたら環境変数(ヒント:setup.bash)を設定し，いよいよ作成したノードを起動します．まず，roscore及びパッケージパッケージ「turtlesim」のノード「turtlesim_node」を起動した後に，作成したノードも起動しましょう．

4. プログラムの内容

それでは，作成したプログラムの挙動を確認した上で，プログラムの中身を順番に見ていきましょう．

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>

必要なヘッダファイルを読み込んでいます．今回は2つのメッセージ「geometry_msgs/Twist」，「turtlesim/Pose」で定義されている内容を配信，購読します．メッセージは自由に作成できますが，今回は予め用意されているものを使用するため，メッセージの内容が定義されているヘッダファイルを読み込みます．

class turtleSim{
public:
    turtleSim();
    ~turtleSim();
    void poseCallback(const turtlesim::PoseConstPtr& pose);
    void setMoveVector(float linear_x, int cnt);
    void timerCallback(const ros::TimerEvent&);
   
private:
    ros::Publisher twist_pub;
    ros::Subscriber pose_sub;
    ros::Timer timer;
    ros::NodeHandle nh;
};

turtleSimというクラスを定義し，幾つかのクラスメンバを宣言します．ここでは，publicメンバとして関数メンバ，privateメンバとして変数メンバを宣言します．privateメンバでは，配信，購読，タイマーのオブジェクトを生成しています．新しく勉強したコンストラクタ，デストラクタ，スコープ演算子，コールバック関数が出てきています．忘れてしまった人は講義の冒頭で配布した資料を参照してください．

turtleSim::turtleSim(){
    twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this);
    timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this);
 
    geometry_msgs::Twist twist;
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;
    twist_pub.publish(twist);
}

コンストラクタが呼び出された際にコールバック関数の宣言と変数の初期化を行います．重要な処理をしているので，もう少し詳細に見ていきましょう．

    twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);

ノードがマスターに対して「geometry_msgs::Twist」型のメッセージでトピック「turtle1/cmd_vel」を配信するよう宣言しています．<>の中にはメッセージの型名(ただし/の代わりにスコープ演算子である::を使用)，””の中にはトピック名を入力します．advertise関数の第二引数の1000は，配信するキューの大きさを表し，データを最大で1000個まで蓄積できることを意味します．

    pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this);

購読するトピックを指定し，更新時に呼び出すコールバック関数です．<>の中にはメッセージの型名(ただし/の代わりにスコープ演算子である::を使用)，””の中にはトピック名を入力します．トピック「turtle1/pose」を購読した際には，poseCallback関数を実行します．

    timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this);

指定された間隔で呼び出すコールバック関数です．createTimer関数の第一引数であるDurationは，呼び出す間隔を表します．ros::Duration(0.1)とした場合には，0.1秒の1回(100ミリ秒に1回)の間隔で指定した関数を呼び出します．

    geometry_msgs::Twist twist;
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;
    twist_pub.publish(twist);

メッセージの型「geometry_msgs::Twist」の各要素を初期化し，トピックを配信しています．初期化しない場合には想定していない値を配信する可能性があるため，必ず値を初期化しましょう．

turtleSim::~turtleSim(){

}

今回は後始末を行う必要がないため，デストラクタで行う処理はありません．このような場合には省略可能です．

void turtleSim::poseCallback(const turtlesim::PoseConstPtr& pose){
    ROS_INFO("x:%f",pose->x);
}

指定したトピックを購読する度にposeCallbackを実行します．今回はトピックを購読した場合にROS_INFO関数によりロボットのx座標を表示しています．

void turtleSim::timerCallback(const ros::TimerEvent&){
    setMoveVector(0.2, 10);
    setMoveVector(-0.2, 10);
}

タイマー関数で指定した間隔でtimerCallbackを実行します．今回は100ミリ秒に1回の間隔でtimerCallbackを実行します．

void turtleSim::setMoveVector(float linear_x, int cnt){
    int i;
    geometry_msgs::Twist twist;
    ros::Rate loop_rate(10);
   
    for(i = 0;i < cnt; i++){
        twist.linear.x = linear_x;
        twist_pub.publish(twist);
        loop_rate.sleep();
    }
}

x軸の並進速度を設定し，トピックを配信します．ros::Rate loop_rate(10)は，1秒間に10回動作するタイマーを意味します．loop_rate.sleep()で100ミリ秒スリープすることを表します．つまり，この関数では与えられた引数の値(0.1)でx軸の並進速度を設定し，100ミリ秒ごとに配信する処理を10回繰り返しています．

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

    turtleSim turtlesim;

    ros::spin();
    return 0;
}

ROSによるプログラミングとほぼ同じ内容です．異なる点は，turtleSimクラスのオブジェクトを生成している点，ros::spin()でコールバックを待つ点です．

以上でプログラミングにより配信と購読することができます．前回に学んだノードやトピック，メッセージの情報をROSのコマンドで取得する方法と今回の内容で，ロボットを自律制御することが可能となります．

課題

1. setMoveVector関数の引数に与える値を変え，その際の挙動を予想しなさい．そして結果と比較しなさい．
2. 上記のサンプルプログラムで購読しているトピック(/turtle1/pose)のx座標以外の情報(x座標以外に4つあります)をROS_INFOにより出力しなさい．
3. ロボットに正四角形の軌跡を描かせるプログラムを作成しなさい．
4. ロボットを座標(5,5)に移動させなさい．座標は購読した情報を使用すること．
5. トピック「/turtle1/color_sensor」を購読してROS_INFOにより出力しなさい．
ヒント:色情報を購読するコールバック関数の引数の型は下記の通りです．
turtlesim::ColorConstPtr& color
6. ロボットが直進する際に，スムーズに加速，減速するような加減速制御を行いなさい．