File content as of revision 2:f3d47d1e19c3:

#include "mbed.h"
#include "PololuLedStrip.h"
#include <string>
#include <ros.h>
#include <std_msgs/String.h>
//#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

PololuLedStrip ledStripFront(PC_9);
PololuLedStrip ledStripBack(PC_8);

#define LED_COUNT 30
#define MaxVelocity 0.5f
rgb_color colors[LED_COUNT];


ros::NodeHandle nh;

//ROS Audio Status Callback
string audio_state = "NothingSpecial";

void AudioStatusCB(const std_msgs::String &status)
{
    audio_state = status.data;
}

float vel = 0.0f;
float ang = 0.0f;


/* Uncomment this For Testing With Controller */
void cmdVelCB(const geometry_msgs::Twist &twist) {
    vel = twist.linear.x;
    ang = twist.angular.z;
    nh.loginfo("received twist");
}

float Map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

std_msgs::String status_msg;
ros::Publisher status_pub("status", &status_msg); // Instance for ROS publisher (String Message)

ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB);

/* Uncomment this for testing with controller */
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB);

int main()
{

    int LEDBrightness = 128;
    float led_tmp = LED_COUNT/4;
    int led_num = floor(led_tmp);
    nh.getHardware()->setBaud(460800);      //set ROSSERIAL baud rate
    nh.initNode();
    nh.advertise(status_pub);                  
    nh.subscribe(AudioStatus);
//    nh.subscribe(Velocity);
    nh.subscribe(cmd_vel_sub);

    rgb_color led_colour = (rgb_color) {
        0, 0, 0
    };

    while(true) {

        if(!nh.connected()) {
            nh.spinOnce();
        } else {

            while(nh.connected()) {
                nh.spinOnce();
                
                /* Moving and Playing */
                if (((abs(vel) || abs(ang)) > 0.01f) && (audio_state == "Playing")) {
                    status_msg.data = "M & A";
                    
                    /* Take the bigger of the two (angular or linear) */
                    if(abs(vel) >= abs(ang)) {
                        LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
                    } else {
                        LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
                    }
                    
                    /* Set colour */
                    led_colour = (rgb_color) {
                        0, 0, 255
                    };
                    
                    /* Populate colours for each led */
                    for(int i = 0; i < led_num; i++) {
                        colors[i] = led_colour;
                    }
                    
                    led_colour = (rgb_color) {
                        0, LEDBrightness, 0
                    };
                    for(int i = led_num; i < ((3*led_num)+1); i++) {
                        colors[i] = led_colour;
                    }
                    led_colour = (rgb_color) {
                        0, 0, 255
                    };
                    for(int i = ((3*led_num)+1); i < LED_COUNT; i++) {
                        colors[i] = led_colour;
                    }
                } 
                
                /* Moing only */
                else if((abs(vel) || abs(ang)) > 0.01f) {     //If Not In DeadZone (Robot Is Moving)
                    status_msg.data = "M.";
                    
                    if(abs(vel) >= abs(ang)) {
                        LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
                    } else {
                        LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
                    }
                    led_colour = (rgb_color) {
                        0, LEDBrightness, 0
                    };
                    for(int i = 0; i < LED_COUNT; i++) {
                        colors[i] = led_colour;
                    }
                } 
                
                /* Playing only */
                else if (audio_state == "Playing") {           //Else If Audio Is Playing
                
                status_msg.data = "A.";
                    led_colour = (rgb_color) {
                        0, 0, 255
                    };
                    for(int i = 0; i < LED_COUNT; i++) {
                        colors[i] = led_colour;
                    }
                } 
                
                /* Not moing and not playing */
                else {   
                status_msg.data = "N.";                                     //Robot Is Neither Moving Nor Playing Audio
                    led_colour = (rgb_color) {
                        255, 0, 0
                    };
                    for(int i = 0; i < LED_COUNT; i++) {
                        colors[i] = led_colour;
                    }
                }


                /* Write to leds */
                ledStripFront.write(colors, LED_COUNT);
                ledStripBack.write(colors, LED_COUNT);
                status_pub.publish(&status_msg);
                wait_ms(200);
            }
        }
    }
}