william chen / Mbed 2 deprecated rtos_base_control

rosserial test

Dependencies:   CANnucleo mbed-rtos mbed ros_lib_indigo

main.cpp@0:a66fe2119e59, 2016-10-06 (annotated)

Committer:
piliwilliam
Date:
Thu Oct 06 06:51:51 2016 +0000
Revision:
0:a66fe2119e59
rosserial_test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piliwilliam 0:a66fe2119e59 1 #include "mbed.h"
piliwilliam 0:a66fe2119e59 2 #include "rtos.h"
piliwilliam 0:a66fe2119e59 3 #include <ros.h>
piliwilliam 0:a66fe2119e59 4 #include <std_msgs/String.h>
piliwilliam 0:a66fe2119e59 5 #include <std_msgs/Float32.h>
piliwilliam 0:a66fe2119e59 6 #include <std_msgs/Empty.h>
piliwilliam 0:a66fe2119e59 7
piliwilliam 0:a66fe2119e59 8 ros::NodeHandle nh;
piliwilliam 0:a66fe2119e59 9
piliwilliam 0:a66fe2119e59 10 std_msgs::String str_msg;
piliwilliam 0:a66fe2119e59 11 ros::Publisher chatter("chatter", &str_msg);
piliwilliam 0:a66fe2119e59 12
piliwilliam 0:a66fe2119e59 13 std_msgs::String banana_msg;
piliwilliam 0:a66fe2119e59 14 ros::Publisher banana("banana", &banana_msg);
piliwilliam 0:a66fe2119e59 15
piliwilliam 0:a66fe2119e59 16 char hello[13] = "hello world!";
piliwilliam 0:a66fe2119e59 17 char bababanana[11] = "bababanana";
piliwilliam 0:a66fe2119e59 18
piliwilliam 0:a66fe2119e59 19 DigitalOut LEDs[4] = {
piliwilliam 0:a66fe2119e59 20 DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
piliwilliam 0:a66fe2119e59 21 };
piliwilliam 0:a66fe2119e59 22
piliwilliam 0:a66fe2119e59 23 Serial serial1(PA_9, PA_10);
piliwilliam 0:a66fe2119e59 24
piliwilliam 0:a66fe2119e59 25 void messageCb(const std_msgs::String& str_msg)
piliwilliam 0:a66fe2119e59 26 {
piliwilliam 0:a66fe2119e59 27 //myled = !myled; // blink the led
piliwilliam 0:a66fe2119e59 28 banana_msg.data = bababanana;
piliwilliam 0:a66fe2119e59 29 banana.publish(&banana_msg);
piliwilliam 0:a66fe2119e59 30 //str_msg.data = hello;
piliwilliam 0:a66fe2119e59 31 //chatter.publish( &str_msg );
piliwilliam 0:a66fe2119e59 32 }
piliwilliam 0:a66fe2119e59 33
piliwilliam 0:a66fe2119e59 34 ros::Subscriber<std_msgs::String> sub("chatter", &messageCb);
piliwilliam 0:a66fe2119e59 35
piliwilliam 0:a66fe2119e59 36 void blink(void const *n) {
piliwilliam 0:a66fe2119e59 37 //int i = 1;
piliwilliam 0:a66fe2119e59 38 //serial1.putc(i);
piliwilliam 0:a66fe2119e59 39 str_msg.data = hello;
piliwilliam 0:a66fe2119e59 40 chatter.publish( &str_msg );
piliwilliam 0:a66fe2119e59 41 }
piliwilliam 0:a66fe2119e59 42
piliwilliam 0:a66fe2119e59 43 int main()
piliwilliam 0:a66fe2119e59 44 {
piliwilliam 0:a66fe2119e59 45 nh.getHardware()->setBaud(1000000);
piliwilliam 0:a66fe2119e59 46 nh.initNode();
piliwilliam 0:a66fe2119e59 47 nh.advertise(chatter);
piliwilliam 0:a66fe2119e59 48 nh.advertise(banana);
piliwilliam 0:a66fe2119e59 49 nh.subscribe(sub);
piliwilliam 0:a66fe2119e59 50
piliwilliam 0:a66fe2119e59 51
piliwilliam 0:a66fe2119e59 52 serial1.baud(115200);
piliwilliam 0:a66fe2119e59 53 RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
piliwilliam 0:a66fe2119e59 54 led_1_timer.start(1000);//1000ms
piliwilliam 0:a66fe2119e59 55 //printf("\n\n*** RTOS basic example ***\n");
piliwilliam 0:a66fe2119e59 56 //Thread thread(print_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
piliwilliam 0:a66fe2119e59 57 //Thread::wait(osWaitForever);
piliwilliam 0:a66fe2119e59 58
piliwilliam 0:a66fe2119e59 59 while (true) {
piliwilliam 0:a66fe2119e59 60
piliwilliam 0:a66fe2119e59 61
piliwilliam 0:a66fe2119e59 62 //temp_msg.data = 1;
piliwilliam 0:a66fe2119e59 63 //led_1_timer.start(100);
piliwilliam 0:a66fe2119e59 64 //chatter.publish( &str_msg );
piliwilliam 0:a66fe2119e59 65 nh.spinOnce();
piliwilliam 0:a66fe2119e59 66 }
piliwilliam 0:a66fe2119e59 67
piliwilliam 0:a66fe2119e59 68 }