Diff: main.cpp

Revision:

7:2cf57f28255d

Parent:

6:858a5116688e

Child:

8:57aa8a35d983

--- a/main.cpp	Wed Oct 23 10:38:36 2019 +0000
+++ b/main.cpp	Wed Oct 23 14:00:26 2019 +0000
@@ -1,10 +1,16 @@
 #include "mbed.h"
+#include <ros.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/Int32.h>
 
 #include "Sensor.h"
 #include "Motor.h"
 
 // Set up serial to pc
-Serial pc(SERIAL_TX, SERIAL_RX); 
+//Serial pc(SERIAL_TX, SERIAL_RX); 
+
+// Set up ROS
+ros::NodeHandle nh;
 
 // Set up I²C on the STM32 NUCLEO-401RE 
 #define addr1   (0x29) 
@@ -22,16 +28,16 @@
 #define S8 PG_3 
 
 // VL6180x sensors
-Sensor sensor_forward(I2C_SDA, I2C_SCL, S1);
-Sensor sensor_right(I2C_SDA, I2C_SCL, S3);
-Sensor sensor_back(I2C_SDA, I2C_SCL, S5);
-Sensor sensor_left(I2C_SDA, I2C_SCL, S7);
+Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
+Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
+Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
+Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
 // Motors
 Motor motor_left(PC_6, PB_15, PB_13);
 Motor motor_right(PA_15, PC_7, PB_4);
-
-void CheckObstacle()
+    
+void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
 {    
     // When obstacle ahead
     if (sensor_forward.getIsObstacle())
@@ -66,8 +72,31 @@
     }
 }
 
+ros::Subscriber<std_msgs::Empty> avoid_obstacle_sub("avoid_obstacle", &avoidObstacle);
+
 int main() 
 {
+    ros::NodeHandle  nh;
+    nh.initNode();
+  
+    std_msgs::Int32 int1_sensor_msg;
+    ros::Publisher range_forward_pub("sensor_forward", &int1_sensor_msg);
+    nh.advertise(range_forward_pub);
+    
+    std_msgs::Int32 int2_sensor_msg;
+    ros::Publisher range_back_pub("sensor_back", &int2_sensor_msg);
+    nh.advertise(range_forward_pub);
+    
+    std_msgs::Int32 int3_sensor_msg;
+    ros::Publisher range_left_pub("sensor_left", &int3_sensor_msg);
+    nh.advertise(range_forward_pub);
+    
+    std_msgs::Int32 int4_sensor_msg;
+    ros::Publisher range_right_pub("sensor_right", &int4_sensor_msg);
+    nh.advertise(range_forward_pub);
+    
+    nh.subscribe(avoid_obstacle_sub);
+
     int range1; 
     int range2;
     int range3;
@@ -99,10 +128,20 @@
         range3 = sensor_back.read();
         range4 = sensor_left.read();         
 
-        pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4); 
+        //pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4); 
+               
+        int1_sensor_msg.data = range1;
+        int2_sensor_msg.data = range3;
+        int3_sensor_msg.data = range4;
+        int4_sensor_msg.data = range2;  
+        range_forward_pub.publish(&int1_sensor_msg);     
+        range_back_pub.publish(&int2_sensor_msg);  
+        range_left_pub.publish(&int3_sensor_msg);     
+        range_right_pub.publish(&int4_sensor_msg);  
+
+        nh.spinOnce();
         
-        // TODO : better name for this method ?? 
-        CheckObstacle();
+        //avoidObstacle();
               
         wait_ms(10); 
     }