pubsub.cpp

00001 //#define COMPILE_PUBSUB_CODE_ROSSERIAL
00002 #ifdef  COMPILE_PUBSUB_CODE_ROSSERIAL
00003 
00004 /*
00005  * rosserial PubSub Example
00006  * Prints "hello world!" and toggles led
00007  */
00008 #include "mbed.h"
00009 #include <ros.h>
00010 #include <std_msgs/String.h>
00011 #include <std_msgs/Empty.h>
00012 
00013 ros::NodeHandle  nh;
00014 DigitalOut myled(LED1);
00015 
00016 void messageCb( const std_msgs::Empty& toggle_msg) {
00017     myled = !myled;   // blink the led
00018 }
00019 
00020 ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
00021 
00022 
00023 
00024 std_msgs::String str_msg;
00025 ros::Publisher chatter("chatter", &str_msg);
00026 
00027 char hello[13] = "hello world!";
00028 
00029 int main() {
00030     nh.initNode();
00031     nh.advertise(chatter);
00032     nh.subscribe(sub);
00033 
00034     while (1) {
00035         str_msg.data = hello;
00036         chatter.publish( &str_msg );
00037         nh.spinOnce();
00038         wait_ms(500);
00039     }
00040 }
00041 #endif