File content as of revision 11:0b9098ec11c7:

#include "mbed.h"
#include "General.hpp"
#include "ros.h"
#include "rtos.h"
#include "ROS_Handler.hpp"
#include <motordriver.h>
#include "math.h"
#include "Motors.hpp"
#include "Pins.h"
#include "VL6180.hpp"

extern Motor A; // pwm, fwd, rev, can brake
extern Motor B; // pwm, fwd, rev, can brake

/******************************************************************************/
/*  ROS serial hardware setup                                                 */
/******************************************************************************/
// Create the ROS node handle
class HaseHardware : public MbedHardware
{
public:
    HaseHardware(): MbedHardware(ROS_Tx, ROS_Rx, ROS_BaudRate) {};
};
/******************************************************************************/

ros::NodeHandle_<HaseHardware>   Node_;
void Control_CallBack(const std_msgs::Int32MultiArray& msg);
ros::Subscriber<std_msgs::Int32MultiArray> Control_Subscriber("/cmd_vel", &Control_CallBack);
std_msgs::String State_msg;
std_msgs::String TOF_msg;
ros::Publisher Status_PUB("/Nucleo/State", &State_msg);
ros::Publisher TOF_PUB("/Nucleo/TOF_data", &TOF_msg);

extern Mutex Global_Battery_Level_Mutex;
extern float Global_Battery_Level;
extern float Global_Battery_Value;
extern bool Battery_Update_Flag;

extern int Global_TOF_Ranges[num_VL6180];
extern bool TOF_Range_Flag;
extern Mutex Global_TOF_Range_Mutex;

// Main function for thread to handle ROS comms
void ROS_Handler(void){
    Node_.initNode();
    Node_.advertise(Status_PUB);
    Node_.advertise(TOF_PUB);
    Node_.subscribe(Control_Subscriber);

    while (1) {
        Global_Battery_Level_Mutex.lock();
        if(Battery_Update_Flag == true)
        {
            float Battery_Level = Global_Battery_Level;
            float Battery_Value = Global_Battery_Value;
            Battery_Update_Flag = false;
            Global_Battery_Level_Mutex.unlock();
            char buffer[50];
            sprintf(buffer, "Battery Level : %1.3fV     | Battery Voltage %1.3fV", Battery_Level, Battery_Value);
            State_msg.data = buffer;
            Status_PUB.publish(&State_msg);
        } else {
          Global_Battery_Level_Mutex.unlock();  
        }
        
        Global_TOF_Range_Mutex.lock();
        if(TOF_Range_Flag == true)
        {
            int TOF_Range[num_VL6180];
            for(int IDX = 0; IDX < num_VL6180; IDX++) {
                TOF_Range[IDX] = Global_TOF_Ranges[IDX];
            }
            TOF_Range_Flag = false;
            Global_TOF_Range_Mutex.unlock();
            char buffer[50];
            sprintf(buffer, "Range 1: %d   | Range 2: %d    | Range 3: %d   | Range 4: %d", TOF_Range[0], TOF_Range[1], TOF_Range[2], TOF_Range[3]);
            TOF_msg.data = buffer;
            TOF_PUB.publish(&State_msg);
        } else {
          Global_TOF_Range_Mutex.unlock();  
        }       
        
        Node_.spinOnce();
        wait_ms(20);     
    }
}

void Control_CallBack(const std_msgs::Int32MultiArray& msg)
{
    /**************************************************************************/
    //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
    //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
    float xlin = 0.01 * msg.data[1];
    float zrot = 0.01 * msg.data[6];
    //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller
    if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction)
        A.speed(-zrot);
        B.speed(zrot);
    } else if(xlin != 0) {
        A.speed(xlin);
        B.speed(xlin);
    } else {
        A.stop(1);
        B.stop(1);
    }
    /**************************************************************************/
}