File content as of revision 0:be6abb948b16:

#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>

PwmOut servos[3] = {
    PwmOut(PA_5), 
    PwmOut(PA_6),
    PwmOut(PA_7),
    };

double offset_deg_servo[3] = {
    0.0,
    -10.0,
    20.0
};

ros::NodeHandle nh;
geometry_msgs::Twist degrees;
geometry_msgs::Twist pub_degrees;

//ros::Publisher deg_pub("mbed_degrees", &pub_degrees);
double setLimitDegree(double input){
    if(input <= -90.0) return -90.0;
    else if(input >= 90.0) return 90.0;
    else return input;
}

void messageCb( const geometry_msgs::Twist& degrees) {
    
    // servo position determined by a pulsewidth between 1-2ms : -90 ~ 90
    servos[0].pulsewidth((setLimitDegree(degrees.linear.x + offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015); 
    servos[1].pulsewidth((setLimitDegree(degrees.linear.y + offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015); 
    servos[2].pulsewidth((setLimitDegree(-degrees.linear.z + offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015); 
    
    pub_degrees = degrees;
    //deg_pub.publish(&pub_degrees);
}
 
ros::Subscriber<geometry_msgs::Twist> sub("degrees", &messageCb );

int main() {
    for(int i = 0; i < 3; ++i){
        servos[i].period(0.020);          // servo requires a 20ms period   
    }
    
    servos[0].pulsewidth((setLimitDegree(offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015); 
    servos[1].pulsewidth((setLimitDegree(offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015); 
    servos[2].pulsewidth((setLimitDegree(offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015); 
    
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(sub);
    //
    //nh.advertise(deg_pub);
    
     while(1){
        nh.spinOnce();
 
        wait_ms(30);
    }
}