Diff: main.cpp

Revision:

0:a66fe2119e59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 06 06:51:51 2016 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+#include "rtos.h"
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+std_msgs::String banana_msg;
+ros::Publisher banana("banana", &banana_msg);
+
+char hello[13] = "hello world!";
+char bababanana[11] = "bababanana";
+
+DigitalOut LEDs[4] = {
+    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
+};
+
+Serial serial1(PA_9, PA_10);
+
+void messageCb(const std_msgs::String& str_msg)
+{
+    //myled = !myled;   // blink the led
+    banana_msg.data = bababanana;
+    banana.publish(&banana_msg);
+    //str_msg.data = hello;
+    //chatter.publish( &str_msg );
+}
+
+ros::Subscriber<std_msgs::String> sub("chatter", &messageCb);
+
+void blink(void const *n) {
+    //int i = 1;
+    //serial1.putc(i);
+    str_msg.data = hello;
+    chatter.publish( &str_msg );
+}
+
+int main()
+{
+    nh.getHardware()->setBaud(1000000);
+    nh.initNode();
+    nh.advertise(chatter);
+    nh.advertise(banana);
+    nh.subscribe(sub);
+    
+    
+    serial1.baud(115200);
+    RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
+    led_1_timer.start(1000);//1000ms
+    //printf("\n\n*** RTOS basic example ***\n");
+    //Thread thread(print_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    //Thread::wait(osWaitForever);
+    
+    while (true) {
+
+        
+        //temp_msg.data = 1;
+        //led_1_timer.start(100);
+        //chatter.publish( &str_msg );
+        nh.spinOnce();
+    }
+    
+}