rosserial arduinoの使い方メモです．ソースべたべたの真っ茶色の記事です．
ArduinoはDuemilanoveを使用しました．Japaninoだとうまく受信できなかったりしたので，クロックが16Mhzのものじゃないと駄目なのかも．
まずは基本的なPublisher,Subscriberの使い方から．
#include <ros.h>
#include <std_msgs/Int32MultiArray.h>
void messageCb( const std_msgs::Int32MultiArray& msg);
ros::NodeHandle nh;
std_msgs::Int32MultiArray array_msg;
ros::Subscriber<std_msgs::Int32MultiArray> s("array_sub", &messageCb);
ros::Publisher p("array_pub", &array_msg);
void messageCb( const std_msgs::Int32MultiArray& msg){
array_msg = msg;
}
void setup()
{
array_msg.data_length = 3;
array_msg.data = (int32_t *)malloc(sizeof(int32_t)*3);
array_msg.data[0] = 0;
array_msg.data[1] = 0;
array_msg.data[2] = 0;
nh.initNode();
nh.advertise(p);
nh.subscribe(s);
}
void loop()
{
p.publish(&array_msg);
nh.spinOnce();
delay(10);
}
というスケッチをArduinoに書き込み，PCのコンソールから
$ rosrun rosserial_python serial_node.py /dev/ttyUSB0
としてクライアントを起動させた後
$ rostopic pub array_sub std_msgs/Int32MultiArray [[['a',3,1]],0] [123,456,789]
publishing and latching message. Press ctrl-C to terminate
$ rostopiecho array_publayout:
dim:
-
label: 4
size: 3
stride: 1
data_offset: 0
data: [123, 456, 789]
---
というようにして，Publishした値がちゃんと受け取られていることが確認できます．
2011/12/6追記
change set 167あたりのサンプルプログラムに使い方が載っていました。
この記事を書いていた時点ではまだちゃんと実装されてなかったんですね。。。
次にServiceServerの使い方．こちらは実はよくわかってません．
#include <ros.h>
#include <std_msgs/Empty.h>
void myservice(const std_msgs::Empty& req, std_msgs::Empty& res );
ros::NodeHandle nh;
void myservice(const std_msgs::Empty& req, std_msgs::Empty& res ) {
}
ros::ServiceServer<std_msgs::Empty,std_msgs::Empty> service("service", &myservice);
void setup()
{
nh.initNode();
nh.advertiseService(service);
}
void loop()
{
nh.spinOnce();
delay(10);
}
このスケッチを書き込み，クライアントを起動させると期待していたServiceServerではなくSubscriverとして認識されます．
そして
$rostopic pub service std_msgs/Empty
というようにPublishするとmyservice関数の中に入った後，resをPublishしてくれているようです．
どうやってresを受け取るのかはわかりません．．．
次に便利なパラメータの使い方に関して．
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
ros::NodeHandle nh;
std_msgs::Float32MultiArray data_msg;
ros::Publisher data_pub("data_pub", &data_msg);
void setup()
{
nh.initNode();
nh.advertise(data_pub);
data_msg.data_length = 3;
data_msg.data=(float*)malloc(sizeof(float)*3);
while(!nh.connected()) {
nh.spinOnce();
delay(10);
}
if (!nh.getParam("param_test", data_msg.data,3)){
//default values
data_msg.data[0]=0;
data_msg.data[1]=0;
data_msg.data[2]=0;
}
}
void loop()
{
data_pub.publish( &data_msg );
nh.spinOnce();
delay(10);
}
このスケッチを書き込んだら，クライアントを起動する前に，
$rosparam set param_test [1.23,4.56,7.89]
というようにあらかじめパラメータをセットしておきます．
試しているバージョン(changeset 143)の段階ではパラメータをセットしておかないとエラーでクライアントが落ちるので注意してください．
クライアントを起動したら
$ rostopic echo data_pub
layout:
dim: []
data_offset: 0
data: [1.2300000190734863, 4.559999942779541, 7.8899998664855957]
---
というようにパラメータがセットできていることが確認できます．
mbedに移植したものでも同じように使うことができます．
mbedの場合だと
main()
{
//setup()部分をここに
while(1){
//loop()部分をここに
}
}
みたいな感じでちょっとの修正(delay()をwait_ms()にするとか)でそのまま使えます．便利！
