Diff: main.cpp

Revision:

1:8080f7a0c35d

Parent:

0:17fd7572aedb

--- a/main.cpp	Tue Apr 19 21:11:17 2016 +0000
+++ b/main.cpp	Sun Feb 09 04:07:51 2020 +0000
@@ -1,30 +1,45 @@
-/*
- * rosserial Publisher Example
- * Prints "hello world!"
- */
-
 #include"mbed.h"
 #include <ros.h>
 #include <std_msgs/String.h>
+Timer t;
 
 ros::NodeHandle  nh;
-
 std_msgs::String str_msg;
-ros::Publisher chatter("chatter", &str_msg);
-
-char hello[13] = "hello world!";
-
+ros::Publisher pub("LL2ROS", &str_msg);
+char msg_string[10];
+Serial debug(D1,D0); //UART2_TX and UART2_RX
 DigitalOut led = LED1;
 
-int main() {
+void matlab_msg( const std_msgs::String& msg)
+{
+    debug.printf("MATLAB response time: %lu \n", t.read_ms());
+}
+
+void python_msg( const std_msgs::String& msg)
+{
+    debug.printf("PYTHON response time: %lu \n", t.read_ms());
+}
+
+ros::Subscriber<std_msgs::String> sub1("MATLAB2LL", matlab_msg);
+ros::Subscriber<std_msgs::String> sub2("PYTHON2LL", python_msg);
+
+int main()
+{
     nh.initNode();
-    nh.advertise(chatter);
-
+    nh.advertise(pub);
+    nh.subscribe(sub1);
+    nh.subscribe(sub2);
+    debug.baud(115200);
+    debug.printf("ready \n");
+    t.start();
     while (1) {
-        led = !led;
-        str_msg.data = hello;
-        chatter.publish( &str_msg );
-        nh.spinOnce();
-        wait_ms(1000);
+        if (t.read_ms() >  1000) {
+            t.reset();
+            debug.printf("send datat to ROS %lu \n", t.read_ms());
+            str_msg.data="data to ROS";
+            pub.publish( &str_msg );
+            nh.spinOnce();
+            led = !led;
+        }
     }
 }
\ No newline at end of file