2011/10/25(火)rosserial_arduino使い方メモ
2011/10/25 12:11
ArduinoはDuemilanoveを使用しました．Japaninoだとうまく受信できなかったりしたので，クロックが16Mhzのものじゃないと駄目なのかも．
Publisher,Subscriber
まずは基本的な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した値がちゃんと受け取られていることが確認できます．ServiceServer
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を受け取るのかはわかりません．．．
Parameter
次に便利なパラメータの使い方に関して．
#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()にするとか)でそのまま使えます．便利！2011/08/22(月)rosserial_arduinoをmbedへ移植
2011/08/23 02:40
そしてふらふら探してみるとRTnoのROS版みたいなものでrosserialというROSノード?があるんですね。
例によって反射的にmbedに移植してしまいましたので下に置いておきます。
まぁ自分以外にこれを求めている人がいるか甚だ疑問があるところですが……。
プログラム
rosserial_mbedrosserial_arduinoに付いてきてたexamplesごと移植したコードです。まず試したいときはこちらをインポートしてください。
ただし全てのexampleが完全に動作かどうかはチェックできてないのでご注意を。。。
なお、初期状態ではHelloWorldのexampleが実行されるようになってます。
rosserial_mbed_lib
examplesだけ取り除いたものです。
ライブラリとして使いたいときはこちらをご利用ください。
使い方
基本的にはrosserial_arduinoのチュートリアルを参考にすればオッケーだと思います。ここではとりあえず、一番基本的なところのHelloWorldの使い方だけ載っけておきます。
HelloWorld
コンソール上でroscoreとroscoreを実行して
rosrun rosserial_python serial_node.py /dev/ttyACM0とmbedの繋がっているポートを指定してserial_node.pyを実行します。
うまく繋がっていれば
rostopic echo chatterとすると、mbedからstringのデータが送られてきます。
その他は本家のチュートリアルとソースを照らし合わせながらやってみてください。
これを利用して自作ロボットをROSの枠組みで動かすとか楽しそうですよね。