File content as of revision 7:2cf57f28255d:

#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); 

// Set up ROS
ros::NodeHandle nh;

// Set up I²C on the STM32 NUCLEO-401RE 
#define addr1   (0x29) 
#define addr2   (0x2A)  
#define addr3   (0x2B)
#define addr4   (0x2C)

#define S1 PC_8
#define S2 PC_9
#define S3 PC_10
#define S4 PC_11
#define S5 PC_12
#define S6 PD_2
#define S7 PG_2
#define S8 PG_3 

// VL6180x sensors
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 avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
{    
    // When obstacle ahead
    if (sensor_forward.getIsObstacle())
    {
        if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
        {
            //Turn backward
            while(!sensor_back.getIsObstacle())
            {
                motor_left.moveForward();
                motor_right.moveBackward();  
            }
        }
        if (sensor_left.getIsObstacle())
        {
            //Turn to the right
            motor_left.moveForward();
            motor_right.moveBackward();
        }
        else 
        {
            // By default : turn to the left
            motor_left.moveBackward();
            motor_right.moveForward();      
        }
    }
    // No obstacle
    else
    {
        motor_left.moveForward();
        motor_right.moveForward();
    }
}

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;
    int range4; 

    // load settings onto VL6180X sensors 
    sensor_forward.init();
    // change default address of sensor 2
    sensor_forward.changeAddress(addr2);
    
    sensor_right.init();
    // change default address of sensor 3
    sensor_right.changeAddress(addr3);

    sensor_back.init();
    // change default address of sensor 4
    sensor_back.changeAddress(addr4);
    
    sensor_left.init();
    
    //Set Speeds
    motor_left.setSpeed(0.5f);
    motor_right.setSpeed(0.5f);
  
    while (1)
    {            
        range1 = sensor_forward.read();
        range2 = sensor_right.read();
        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); 
               
        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();
        
        //avoidObstacle();
              
        wait_ms(10); 
    } 
}