前回は、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) { //行と帰りがあるので、２で割る 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ケーブルを繋いだまま、
３つのシェルでそれぞれ次のコマンドを実行する。
>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
