Diff: main.cpp

Revision:

8:57aa8a35d983

Parent:

7:2cf57f28255d

Child:

10:be9de79cf6b0

--- a/main.cpp	Wed Oct 23 14:00:26 2019 +0000
+++ b/main.cpp	Wed Oct 30 13:12:25 2019 +0000
@@ -2,6 +2,7 @@
 #include <ros.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
+#include <mbed_custom_msgs/lidar_msg.h>
 
 #include "Sensor.h"
 #include "Motor.h"
@@ -9,9 +10,6 @@
 // Set up serial to pc
 //Serial pc(SERIAL_TX, SERIAL_RX); 
 
-// Set up ROS
-ros::NodeHandle nh;
-
 // Set up I²C on the STM32 NUCLEO-401RE 
 #define addr1   (0x29) 
 #define addr2   (0x2A)  
@@ -37,6 +35,7 @@
 Motor motor_left(PC_6, PB_15, PB_13);
 Motor motor_right(PA_15, PC_7, PB_4);
     
+    /*
 void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
 {    
     // When obstacle ahead
@@ -71,14 +70,20 @@
         motor_right.moveForward();
     }
 }
+*/
 
-ros::Subscriber<std_msgs::Empty> avoid_obstacle_sub("avoid_obstacle", &avoidObstacle);
+//ros::Subscriber<std_msgs::Empty> avoid_obstacle_sub("avoid_obstacle", &avoidObstacle);
 
 int main() 
 {
     ros::NodeHandle  nh;
     nh.initNode();
+    
+    mbed_custom_msgs::lidar_msg int_lidar_msg;
+    ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
+    nh.advertise(lidar_pub);
   
+  /*
     std_msgs::Int32 int1_sensor_msg;
     ros::Publisher range_forward_pub("sensor_forward", &int1_sensor_msg);
     nh.advertise(range_forward_pub);
@@ -101,6 +106,7 @@
     int range2;
     int range3;
     int range4; 
+*/
 
     // load settings onto VL6180X sensors 
     sensor_forward.init();
@@ -123,13 +129,15 @@
   
     while (1)
     {            
-        range1 = sensor_forward.read();
-        range2 = sensor_right.read();
-        range3 = sensor_back.read();
-        range4 = sensor_left.read();         
+        int_lidar_msg.sensor_forward  = sensor_forward.read();
+        int_lidar_msg.sensor_right  = sensor_right.read();
+        int_lidar_msg.sensor_back  = sensor_back.read();
+        int_lidar_msg.sensor_left  = sensor_left.read();         
 
+        lidar_pub.publish(&int_lidar_msg);
         //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;
@@ -138,7 +146,8 @@
         range_back_pub.publish(&int2_sensor_msg);  
         range_left_pub.publish(&int3_sensor_msg);     
         range_right_pub.publish(&int4_sensor_msg);  
-
+        */
+        
         nh.spinOnce();
         
         //avoidObstacle();