Nakarin Satthamnuwong / Mbed 2 deprecated ROS1-test-v1

test1

Dependencies:   ros_lib_indigo mbed

main.cpp@1:8080f7a0c35d, 2020-02-09 (annotated)

Committer:
fibokmutt
Date:
Sun Feb 09 04:07:51 2020 +0000
Revision:
1:8080f7a0c35d
Parent:
0:17fd7572aedb 
test1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garyservin 0:17fd7572aedb 1 #include"mbed.h"
garyservin 0:17fd7572aedb 2 #include <ros.h>
garyservin 0:17fd7572aedb 3 #include <std_msgs/String.h>
fibokmutt 1:8080f7a0c35d 4 Timer t;
garyservin 0:17fd7572aedb 5
garyservin 0:17fd7572aedb 6 ros::NodeHandle nh;
garyservin 0:17fd7572aedb 7 std_msgs::String str_msg;
fibokmutt 1:8080f7a0c35d 8 ros::Publisher pub("LL2ROS", &str_msg);
fibokmutt 1:8080f7a0c35d 9 char msg_string[10];
fibokmutt 1:8080f7a0c35d 10 Serial debug(D1,D0); //UART2_TX and UART2_RX
garyservin 0:17fd7572aedb 11 DigitalOut led = LED1;
garyservin 0:17fd7572aedb 12
fibokmutt 1:8080f7a0c35d 13 void matlab_msg( const std_msgs::String& msg)
fibokmutt 1:8080f7a0c35d 14 {
fibokmutt 1:8080f7a0c35d 15 debug.printf("MATLAB response time: %lu \n", t.read_ms());
fibokmutt 1:8080f7a0c35d 16 }
fibokmutt 1:8080f7a0c35d 17
fibokmutt 1:8080f7a0c35d 18 void python_msg( const std_msgs::String& msg)
fibokmutt 1:8080f7a0c35d 19 {
fibokmutt 1:8080f7a0c35d 20 debug.printf("PYTHON response time: %lu \n", t.read_ms());
fibokmutt 1:8080f7a0c35d 21 }
fibokmutt 1:8080f7a0c35d 22
fibokmutt 1:8080f7a0c35d 23 ros::Subscriber<std_msgs::String> sub1("MATLAB2LL", matlab_msg);
fibokmutt 1:8080f7a0c35d 24 ros::Subscriber<std_msgs::String> sub2("PYTHON2LL", python_msg);
fibokmutt 1:8080f7a0c35d 25
fibokmutt 1:8080f7a0c35d 26 int main()
fibokmutt 1:8080f7a0c35d 27 {
garyservin 0:17fd7572aedb 28 nh.initNode();
fibokmutt 1:8080f7a0c35d 29 nh.advertise(pub);
fibokmutt 1:8080f7a0c35d 30 nh.subscribe(sub1);
fibokmutt 1:8080f7a0c35d 31 nh.subscribe(sub2);
fibokmutt 1:8080f7a0c35d 32 debug.baud(115200);
fibokmutt 1:8080f7a0c35d 33 debug.printf("ready \n");
fibokmutt 1:8080f7a0c35d 34 t.start();
garyservin 0:17fd7572aedb 35 while (1) {
fibokmutt 1:8080f7a0c35d 36 if (t.read_ms() > 1000) {
fibokmutt 1:8080f7a0c35d 37 t.reset();
fibokmutt 1:8080f7a0c35d 38 debug.printf("send datat to ROS %lu \n", t.read_ms());
fibokmutt 1:8080f7a0c35d 39 str_msg.data="data to ROS";
fibokmutt 1:8080f7a0c35d 40 pub.publish( &str_msg );
fibokmutt 1:8080f7a0c35d 41 nh.spinOnce();
fibokmutt 1:8080f7a0c35d 42 led = !led;
fibokmutt 1:8080f7a0c35d 43 }
garyservin 0:17fd7572aedb 44 }
garyservin 0:17fd7572aedb 45 }