Revision 1:e3660c13da3c, committed 2019-12-11

Comitter:

msalmehmadi

Date:

Wed Dec 11 14:54:04 2019 +0000

Parent:

0:7107a8b171d1

Commit message:

ready to share

Changed in this revision

--- a/main.cpp	Tue Dec 03 11:43:08 2019 +0000
+++ b/main.cpp	Wed Dec 11 14:54:04 2019 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 //#include "VL6180.h"
-//#include <std_msgs/Empty.h>
+#include <std_msgs/Empty.h>
 #include <ros.h>
 #include <std_msgs/UInt8.h>
 ///////////////////////////////////////////////////////////////////
@@ -21,10 +21,12 @@
 VL6180 tof4(0x55);
 */
 
+DigitalOut myled(LED1);
+
 ros::NodeHandle  nh;
 
 std_msgs::UInt8 Tof1;
-ros::Publisher tof1("tof1", &Tof1);
+ros::Publisher ("tof1", &Tof1);
 
 
 std_msgs::UInt8 Tof2;
@@ -36,6 +38,20 @@
 std_msgs::UInt8 Tof4;
 ros::Publisher tof4("tof4", &Tof4);
 
+std_msgs::UInt8 received_speed;
+ros::Publisher speed_publisher("speed_publisher", &received_speed);
+
+std_msgs::UInt8 the_motor_speed;
+
+void messageCb(const std_msgs::UInt8& speed){
+    the_motor_speed = speed;
+}
+ros::Subscriber<std_msgs::UInt8> sub("motor_speed", &messageCb);
+
+//void messageCb(const std_msgs::Empty& toggle_msg){
+//    myled = !myled;   // blink the led
+//}
+//ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);
 
 int main() {
     nh.initNode();
@@ -43,6 +59,9 @@
     nh.advertise(tof2);
     nh.advertise(tof3);
     nh.advertise(tof4);
+    nh.advertise(speed_publisher);
+    
+    nh.subscribe(sub);
    
     while (1) {
 //        led = !led;
@@ -50,10 +69,11 @@
         tof1.publish( &Tof1 );
         Tof2.data = 6;
         tof2.publish( &Tof2 );
-        Tof2.data = 7;
-        tof2.publish( &Tof3);
-        Tof2.data = 8;
-        tof2.publish( &Tof4);
+        Tof3.data = 7;
+        tof3.publish( &Tof3);
+        Tof4.data = 8;
+        tof4.publish( &Tof4);
+        speed_publisher.publish(&the_motor_speed);
         nh.spinOnce();
         wait_ms(1000);
     }