![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |