mbedでROSをやってみた

今回、mbedを使ってROSをやってみるということになったのでそのやり方の備忘録。
rosserialというのを用いて、マイコン同士の通信を行っています。

1 参考にしたページ
2 Publisherの作成
3 Subscriberの作成

参考にしたページ

ROSではトピックを通してデータのやり取りを行うので、まず、トピックにデータを送信するパブリッシャー(Publisher)、そのデータを受信するサブスクライバー(Subscriber)を作っていきたいと思います。

Publisherの作成

まず、ソースコード。

#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;
}

#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側で、

$roscore &amp;
$/rosout
$rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

$roscore &

$/rosout

$rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

これで、通信が行われるようになる。

送信の内容が見たいときは、別端末で

$rostopic echo chatter

1	$rostopic echo chatter

で送信内容を確認することができる。

注意する点としては、あまり通信速度が速いとエラーが頻発するのでwaitを入れておくこと。

Subscriberの作成

以下ソース。

#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;
}

#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);