File content as of revision 1:6a10e58b3d43:

#include "General.hpp"

DigitalOut myLED(LED1);

/******************************************************************************/
/*  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 CallBack(const std_msgs::Int32MultiArray& msg);
ros::Subscriber<std_msgs::Int32MultiArray> Sub_("/cmd_vel", &CallBack);
std_msgs::Int32 PUB_MSG;
ros::Publisher Pub_("/TOF_Data", &PUB_MSG);

// Main function for thread to handle ROS comms
void ROS_Handler(void){
    myLED = 1;
    Node_.initNode();
    Node_.advertise(Pub_);
    Node_.subscribe(Sub_);
    
    PUB_MSG.data = 10;

    while (1) {
        Pub_.publish(&PUB_MSG);
        Node_.spinOnce();
        wait_ms(20);
    }
}

void 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);
    }
    /**************************************************************************/
}