Diff: main.cpp

Revision:

1:fb8f963047b0

Parent:

0:2595562b0c61

Child:

2:8b3e4bfa29e5

--- a/main.cpp	Fri Aug 10 21:02:43 2018 +0000
+++ b/main.cpp	Mon Aug 13 19:25:13 2018 +0000
@@ -4,35 +4,33 @@
  */
 #include "mbed.h"
 #include <ros.h>
-#include <std_msgs/String.h>
+#include <geometry_msgs/Twist.h>
 
 ros::NodeHandle nh;
 DigitalOut myled(LED1);
 
-std_msgs::String commandRead;
+geometry_msgs::Twist commandRead;
 ros::Publisher chatter("chatter", &commandRead);
 
 // /*
-void messageCb(const std_msgs::String& command){
-    myled = 1;   // turn on the led
-    commandRead.data = command.data;
+void handlerFunction(const geometry_msgs::Twist& command){
+    commandRead = command;
 }
 // */
 
-ros::Subscriber<std_msgs::String> sub("toggle_led", &messageCb);
-
-//char message[13] = "012345678910";
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
 
 int main() {
     nh.initNode();
     nh.subscribe(sub);
     nh.advertise(chatter);
+    
+    commandRead.linear.x = 6.9;
 
     while (1) {
-        myled = 0; // turn off the led
-        // commandRead.data = message ;
+        //commandRead = command;
         chatter.publish( &commandRead);
         nh.spinOnce();
-        wait_ms(1);
+        wait_ms(10);
     }
 }