Edited at

ROS講座03 Pub & Sub 通信


環境

この記事は以下の環境で動いています。

項目

CPU
Core i5-8250U

Ubuntu
16.04

ROS
Kinetic

インストールについてはROS講座02 インストールを参照してください。

またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。


概要

このページはROSチュートリアルのシンプルな配信者(Publisher)と購読者(Subscriber)を書く(C++)とほぼ同等の内容です。このシリーズの流儀で少し書き直しています。


Pub & Sub通信

ROSの中心はプロセス間でデータの受け渡しをすることです。ROSではプログラムの最小単位としてROSノードというものがあります(基本的にに1つのROSノードは1プロセスに対応します)。ROSNodeは基本的には単純な処理のみを行い。ロボットの複雑な処理は多数のROSノードが通信しあって共同で実行します。しかしプロセス間通信は意外と面倒で、ここで使わるフレームワークがPub&Sub通信です。ROSの通信はROSトピックといって名前が付けられます。下の図ようにbasic_simple_talkerというROSノードが送信をしてbasic_simple_listenerというROSノードが受信をするという通信を行うときに、その通信はROSトピックで表され、それにchatterという名前を付けます。2つのノードはこのROSトピックの名前を指定することで通信を行うことができます。


前準備(ROSパッケージの製作)

全てのROSのプログラムはROSパッケージに属します。まずROSパッケージを作ります。またC++のプログラムは流儀としてROSパッケージ内のsrc/ディレクトリ内に置きます。ROSパッケージはsrc/の中ならどこでもよいので、今回はsrc/ros_lecturebasic_lectureというROSパッケージを作ります


gitリポジトリを使う場合

ROS講座11 gitリポジトリを参照してください。


自分でパッケージを製作する場合


ROSパッケージを作成

cd ~/catkin_ws/src

mkdir ros_lecture
cd ~/catkin_ws/src/ros_lecture

catkin_create_pkg basic_lecture std_msgs rospy roscpp
cd ~/catkin_ws/src/ros_lecture/basic_lecture/src
touch basic_simple_talker.cpp
touch basic_simple_listener.cpp


catkin_create_pkg [package名] [依存package名1] [依存package名2] ...がrosのパッケージを作成するコマンドです。依存パッケージですが、std_msgsrospyroscppの3つを入れておけば大体よいでしょう。


ソースファイル


Publisherのコーディング


basic_lecture/src/basic_simple_talker.cpp

#include <ros/ros.h>

#include <std_msgs/String.h>

int main(int argc, char **argv){
ros::init(argc, argv, "basic_simple_talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 10);
ros::Rate loop_rate(10);

while (ros::ok()){
std_msgs::String msg;
msg.data = "hello world!";
ROS_INFO("publish: %s", msg.data.c_str());
chatter_pub.publish(msg);

ros::spinOnce();
loop_rate.sleep();
}
return 0;
}


ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 10);の最後の数字はバッファーサイズです。基本的に10程度の値を入れておけば十分です。


Subscriberのコーディング


basic_lecture/src/basic_simple_listener.cpp

#include <ros/ros.h>

#include <std_msgs/String.h>

void chatterCallback(const std_msgs::String& msg){
ROS_INFO("subscribe: %s", msg.data.c_str());
}

int main(int argc, char **argv){
ros::init(argc, argv, "basic_simple_listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("chatter", 10, chatterCallback);

ros::spin();
return 0;
}


ros::Subscriber sub = n.subscribe("chatter", 10, chatterCallback);の2番目の引数の整数値は受信バッファーサイズです。リアルタイム性の高いトピック(=常に最新の値が欲しいトピック)では小さい数字にします。また、取りこぼしがあってほしくないトピックでは大きめの値を選びます。

公式のROSwikiのtutrialでは1000という非常に大きい値になっていますが、このように大きい数字にすると処理がバグることがあるので推奨しません。


CmakeListの設定

パッケージ直下にCmakeLists.txtがありここにビルド用の情報を書き足します。今回の場合はCmakeLists.txtに以下の4行を書き加えるだけです。


CmakeLists.txtに追加

add_executable(basic_simple_talker src/basic_simple_talker.cpp)

add_executable(basic_simple_listener src/basic_simple_listener.cpp)

target_link_libraries(basic_simple_talker ${catkin_LIBRARIES})
target_link_libraries(basic_simple_listener ${catkin_LIBRARIES})


add_executable()はビルド対象にするということを、target_link_libraries()も名前の通りリンクするライブラリを指定します。基本的にこの2行を書けばビルドできます。


ビルド

cd ~/catkin_ws

catkin_make

catkinワークスペースでcatkin_makeコマンドを実行するとワークスペース中のすべてのパッケージのビルドを始めます。コンパイルエラーが出た場合はここで表示されます。


実行

roscoreを実行するターミナル、simple_talkerを実行するターミナル、simple_listenerを実行するターミナルの3つのターミナルを使います。


1つ目のターミナル

roscore



2つ目のターミナル

rosrun basic_lecture basic_simple_talker



3つ目のターミナル

rosrun basic_lecture basic_simple_listener



結果

3つ目のターミナルで以下のような表示が出てきて、/chatterトピックが受信できていることが確認できました。


参考

ROS wiki: publishとsubscribe


目次ページへのリンク

ROS講座の目次へのリンク