今回、mbedを使ってROSをやってみるということになったのでそのやり方の備忘録。
rosserialというのを用いて、マイコン同士の通信を行っています。
参考にしたページ
ROSではトピックを通してデータのやり取りを行うので、まず、トピックにデータを送信するパブリッシャー(Publisher)、そのデータを受信するサブスクライバー(Subscriber)を作っていきたいと思います。
Publisherの作成
まず、ソースコード。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 | #include <ros.h>; #include <std_msgs/String.h>; #include "mbed.h" ros::NodeHandle nh; std_msgs::String msg; ros::Publisher pub("chatter", &msg); int main(){ nh.getHardware()->setBaud(115200); nh.initNode(); nh.advertise(pub); while(1){ msg.data = "Hello World!"; pub.publish(&msg); nh.spinOnce(); wait_ms(1000); } //return 0; } |
これをmbedでコンパイルし、マイコンに書き込んでおく。
ROS側で、
1 2 3 | $roscore & $/rosout $rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200 |
これで、通信が行われるようになる。
送信の内容が見たいときは、別端末で
1 | $rostopic echo chatter |
で送信内容を確認することができる。
注意する点としては、あまり通信速度が速いとエラーが頻発するのでwaitを入れておくこと。
Subscriberの作成
以下ソース。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 | #include <ros.h> #include <std_msgs/Empty.h> #include "mbed.h" ros::NodeHandle nh; std_msgs::String msg; DigitalOut led(LED1); void messageCb( const std_msgs::Empty& toggle_msg) { led.write(0); wait(1.0); // 1 sec led.write( 1); wait(1.0); // 1 sec } ros::Subscriber sub("toggle_led", &messageCb ); int main(){ nh.getHardware()->setBaud(115200); nh.initNode();exit nh.subscribe(sub); while(1){ nh.spinOnce(); wait_ms(1000); } //return 0; } |
以下のコードを入力すると、LEDが反転するのが確認できると思います。
1 | $ rostopic pub toggle_led std_msgs/Empty --once |