2011/10/25(火)rosserial_arduino使い方メモ

2011/10/25 12:11 ROSnucho
rosserial arduinoの使い方メモです.ソースべたべたの真っ茶色の記事です.
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 ROSnucho
こないだまでちょっとRTMを触ってたんですが、世界的にはROSの方が流行ってそうということで浮気してます。ちなみに勉強する上でこちらが参考になりました。
そしてふらふら探してみるとRTnoのROS版みたいなものでrosserialというROSノード?があるんですね。

例によって反射的にmbedに移植してしまいましたので下に置いておきます。
まぁ自分以外にこれを求めている人がいるか甚だ疑問があるところですが……。

プログラム

rosserial_mbed
rosserial_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の枠組みで動かすとか楽しそうですよね。