main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include <ros.h>
00004 #include <std_msgs/String.h>
00005 #include <std_msgs/Float32.h>
00006 #include <std_msgs/Empty.h>
00007 
00008 ros::NodeHandle  nh;
00009 
00010 std_msgs::String str_msg;
00011 ros::Publisher chatter("chatter", &str_msg);
00012 
00013 std_msgs::String banana_msg;
00014 ros::Publisher banana("banana", &banana_msg);
00015 
00016 char hello[13] = "hello world!";
00017 char bababanana[11] = "bababanana";
00018 
00019 DigitalOut LEDs[4] = {
00020     DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
00021 };
00022 
00023 Serial serial1(PA_9, PA_10);
00024 
00025 void messageCb(const std_msgs::String& str_msg)
00026 {
00027     //myled = !myled;   // blink the led
00028     banana_msg.data = bababanana;
00029     banana.publish(&banana_msg);
00030     //str_msg.data = hello;
00031     //chatter.publish( &str_msg );
00032 }
00033 
00034 ros::Subscriber<std_msgs::String> sub("chatter", &messageCb);
00035 
00036 void blink(void const *n) {
00037     //int i = 1;
00038     //serial1.putc(i);
00039     str_msg.data = hello;
00040     chatter.publish( &str_msg );
00041 }
00042 
00043 int main()
00044 {
00045     nh.getHardware()->setBaud(1000000);
00046     nh.initNode();
00047     nh.advertise(chatter);
00048     nh.advertise(banana);
00049     nh.subscribe(sub);
00050     
00051     
00052     serial1.baud(115200);
00053     RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
00054     led_1_timer.start(1000);//1000ms
00055     //printf("\n\n*** RTOS basic example ***\n");
00056     //Thread thread(print_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
00057     //Thread::wait(osWaitForever);
00058     
00059     while (true) {
00060 
00061         
00062         //temp_msg.data = 1;
00063         //led_1_timer.start(100);
00064         //chatter.publish( &str_msg );
00065         nh.spinOnce();
00066     }
00067     
00068 }