ROS topic通信 センサ値読み込み想定 メモ

ロボットソフトウェアプラットフォームであるROSのメモです。
今回はtopic通信(一方向通信で、送信側は送信しっぱなし)のサンプルを少しいじったものを。
想定としては、送信側はセンサから何らかの値を読み込みそれを送信。
受信側は、受け取ったセンサ値を用いて何かをするという状況です。


以下は送信者(publisher)側のコード。
ここではセンサ値を読み取る動作の代わりに、乱数を用いて値を取得し、送信するmsg.dataにその値を入れています。そして、msgを送信しています。

```cpp:ros_tutorial_msg_publisher.cpp
// ROSメインヘッダーファイル
#include "ros/ros.h"
// msgTutorialメッセージファイルのヘッダー
#include "irvs_ros_tutorials/msgTutorial.h"
//乱数を使う
#include <stdlib.h>

// 配信者ノードのメイン関数
int main(int argc, char **argv)
{
// ノード名の初期化
ros::init(argc, argv, "ros_tutorial_msg_publisher");

//ノードハンドルを宣言
ros::NodeHandle nh;

// 配信者ノードの宣言
// irvs_ros_tutorialsパッケージのmsgTutorialメッセージファイルを
// 利用した配信者ros_tutorial_pubを宣言
// トピック名をros_tutorial_msgとし、配信者キュー( queue )のサイズを100に。
// 配信者キューには、メッセージを送る際、メッセージデータを蓄積
ros::Publisher ros_tutorial_pub =
nh.advertise<irvs_ros_tutorials::msgTutorial>("ros_tutorial_msg", 100);

//ループの周期を1Hzに。
ros::Rate loop_rate(1);

// メッセージに使用する変数
int activity = 0;

// ros::ok()はROSの動作が正常であるならtrueを返す関数である。
while (ros::ok())
{
//センサ値を読み取る動作の疑似的試行
  activity=rand()%200;

// msgTutorialメッセージファイル形式でmsg変数を宣言する。
irvs_ros_tutorials::msgTutorial msg;

// メッセージの値を定める。
msg.data = activity;

// ROS_INFO関数を使用して、activity変数を表示
ROS_INFO("sensor_value = %d", activity);

// メッセージを発行。1秒間隔で発行される。
ros_tutorial_pub.publish(msg);

// 上で定められたループサイクルになるように、スリープに入る
loop_rate.sleep();

}
return 0;
}
```



以下は受信者(subscriber)側のコード。
受け取ったmsg内のdataの値の大きさによって表示する文字列を変えています。

```cpp:ros_tutorial_msg_subscriber.cpp
// ROSメインヘッダーファイル
#include "ros/ros.h"
// msgTutorialメッセージファイルのヘッダー
#include "irvs_ros_tutorials/msgTutorial.h"
//文字列関数を使う
#include <string.h>


// メッセージを受信したときに動作するコールバック関数を定義
// irvs_ros_tutorialsパッケージのmsgTutorialメッセージを受信
void msgCallback(const irvs_ros_tutorials::msgTutorial::ConstPtr& msg)
{
  char direction[10], RIGHT[10]="Right",LEFT[10]="Left",BACK[10]="Back",GO[10]="Go" ;

  if(msg->data >=0 && msg->data <=50){
    strcpy(direction,GO);
  }
  else if(msg->data <=100){
    strcpy(direction,RIGHT);
  }
  else if(msg->data <=150){
    strcpy(direction,LEFT);
  }
  else{
    strcpy(direction,BACK);
  }
    
  //次の行動の表示
ROS_INFO("Next Activity is Turn %s", direction);

}


// 購読者ノードのメイン関数
int main(int argc, char **argv)
{
// ノード名の初期化
ros::init(argc, argv, "ros_tutorial_msg_subscriber");
// ノードのハンドルを宣言
ros::NodeHandle nh;
// 購読者ノードの宣言
// irvs_ros_tutorialsパッケージのmsgTutorialメッセージファイルを
// 利用した購読者ros_tutorial_subを宣言
// トピック名をros_tutorial_msgとし、購読者キュー( queue )のサイズを100に。
// 購読者キューには、配信者から送信されてくるメッセージが蓄積
ros::Subscriber ros_tutorial_sub = nh.subscribe("ros_tutorial_msg",
100, msgCallback);
// メッセージが受信されるまで待機し、受信が行われた場合コールバック関数を実行
ros::spin();
return 0;
}
```





#実行結果
送信側
[ INFO] [1452489799.783031785]: sensor_value = 31
[ INFO] [1452489800.783673737]: sensor_value = 117
[ INFO] [1452489801.783218993]: sensor_value = 97
[ INFO] [1452489802.783206242]: sensor_value = 171
[ INFO] [1452489803.784123567]: sensor_value = 81
[ INFO] [1452489804.783630950]: sensor_value = 75
[ INFO] [1452489805.783732135]: sensor_value = 109



受信側
[ INFO] [1452489799.784065612]: Next Activity is Turn Go
[ INFO] [1452489800.784399460]: Next Activity is Turn Left
[ INFO] [1452489801.783684183]: Next Activity is Turn Right
[ INFO] [1452489802.784117211]: Next Activity is Turn Back
[ INFO] [1452489803.785018115]: Next Activity is Turn Right
[ INFO] [1452489804.784827967]: Next Activity is Turn Right
[ INFO] [1452489805.784411299]: Next Activity is Turn Left