覚え書きブログ

読者です 読者をやめる 読者になる 読者になる

ROSの覚え書き(ArduinoのPublisher編)

ロボット

前回は、Arduino IDEでrosserialを使えるように設定し、LEDの点滅をするSubscriberを作ってみた。hirotaka-hachiya.hatenablog.com

今回は、ソナーセンサーで測定した距離情報に関するメッセージを送信するPublisherを作ってみる。
ソナーセンサーHC-SR04とArduino UNO間の配線方法は、前回紹介したものと同じである。hirotaka-hachiya.hatenablog.com

プログラムコードは、ros_libのサンプルコードのHelloWorldを参考にして、
以下のように作成した。

#include <ros.h>
#include <std_msgs/Float32.h>

//Trig、EchoをD4、D5ピンに設定
int Trig = 4;
int Echo = 5;
int Duration;
float Distance;

//ROSメッセージのインスタンス
ros::NodeHandle  nh;

//ROSメッセージの型
std_msgs::Float32 dist_msg;

//Publisherのインスタンスchatter
//トピック名:distance
//ROSメッセージのインスタンス:dist_msg
ros::Publisher chatter("distance", &dist_msg);

void setup()
{
  //TrigとEchoを出力モードに設定
  pinMode(Trig,OUTPUT);
  pinMode(Echo,INPUT);

  //ROSノードのインスタンス「nh」を初期化し、Publisher「chatter」を宣伝する
  nh.initNode();
  nh.advertise(chatter);
}

void loop()
{
  //TrigにLow, High, Lowを出力
  digitalWrite(Trig,LOW);
  delayMicroseconds(1);
  digitalWrite(Trig,HIGH);
  delayMicroseconds(1);
  digitalWrite(Trig,LOW);

  //EchoがLOWからHighになってから再びLOWに戻るまでの時間(us)を計測
  Duration = pulseIn(Echo,HIGH);
  
  if (Duration>0) {
    //行と帰りがあるので、2で割る    
    Distance = Duration/2;

    //超音波の速度は、340m/sなので、0.034cm/usを、Durationにかけて、距離(cm)に変換
    Distance = Distance*340*100/1000000;

    //ROSメッセージをDistance(距離)に設定
    dist_msg.data = Distance;

    //ROSメッセージをpublishする
    chatter.publish( &dist_msg );
  }

  nh.spinOnce();
  delay(1000);
}

該プログラムコードを、Arduino UNOに書き込んだ後、USBケーブルを繋いだまま、
3つのシェルでそれぞれ次のコマンドを実行する。

>roscore
>rosrun rosserial_python serial_node.py /dev/ttyACM0
>rostopic echo distance
data: 107.575996399
---
data: 104.652000427
---
data: 24.2420005798
---
data: 10.982000351
---
data: 105.536003113
---
data: 12.2060003281
---
data: 20.3999996185
---
data: 103.428001404
---
data: 113.38999939
---

最後のコマンドは、subscriberとして、トピック「distance」から、メッセージを受信するためのものである。
距離のメッセージが受信できていることがわかる。

最後にノード間のやりとりを可視化してみる。

> rosrun rqt_graph rqt_graph

f:id:hirotaka_hachiya:20150816220618p:plain