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


rosserial_mbed
Author(s): Gary Servin
autogenerated on Sat Oct 7 2017 03:08:46