File content as of revision 4:62c92aa48842:

#include "mbed.h"
#include <ros.h>

#include <s_rov_msgs/s_rov_control_extra.h>
#include <s_rov_msgs/s_rov_control_esc.h>
#include <s_rov_msgs/s_rov_control_led.h>
#include <s_rov_msgs/s_rov_state.h>

#include <pca9685/pca9685.h>
#include <ads1015/ads1015.h>

#define CH_LED1        9
#define CH_LED2        8
#define CH_ESC1       10
#define CH_ESC2       11
#define CH_ESC3        7
#define CH_ESC4        6
#define CH_1           0
#define CH_2           1
#define CH_3           2
#define CH_4           3
#define CH_5           4
#define CH_6           5

#define ESC_SWITCH  PB_1


/**************************** System Configuration ***************************/

Timer t;
I2C i2c(PB_11, PB_10);        // sda, scl
PCA9685 pwm(0x40, &i2c, 50);  // Address, i2c pointer, frequency
ADS1015 adc(0x48, &i2c, ADS1015_GAIN_TWOTHRIDS); // Address, i2c pointer, gain
DigitalOut ESC_switch(ESC_SWITCH);

/**************************** System Configuration ***************************/

/******************************ROS Configuration *****************************/

class NewHardware : public MbedHardware{
    public:
    NewHardware():MbedHardware(PA_9, PA_10, 57600){};    
};
ros::NodeHandle_<NewHardware> nh;

void messageExtra(const s_rov_msgs::s_rov_control_extra& control_msg){
    pwm.set_servo_angle(CH_1, control_msg.extraPWM[0]);
    pwm.set_servo_angle(CH_2, control_msg.extraPWM[1]);
    pwm.set_servo_angle(CH_3, control_msg.extraPWM[2]);
    pwm.set_servo_angle(CH_4, control_msg.extraPWM[3]);
    pwm.set_servo_angle(CH_5, control_msg.extraPWM[4]);
    pwm.set_servo_angle(CH_6, control_msg.extraPWM[5]);
}

void messageESC(const s_rov_msgs::s_rov_control_esc& control_msg){
    pwm.set_servo_angle(CH_ESC1, control_msg.escPWM[0]);
    pwm.set_servo_angle(CH_ESC2, control_msg.escPWM[1]);
    pwm.set_servo_angle(CH_ESC3, control_msg.escPWM[2]);
    pwm.set_servo_angle(CH_ESC4, control_msg.escPWM[3]);
    ESC_switch = control_msg.escSwitch;
}

void messageLed(const s_rov_msgs::s_rov_control_led& control_msg){
    pwm.set_pwm_duty(CH_LED1, control_msg.ledPWM[0]);
    pwm.set_pwm_duty(CH_LED2, control_msg.ledPWM[1]);
}

ros::Subscriber<s_rov_msgs::s_rov_control_extra> sub_extra_control("/s_rov/control/extra", &messageExtra);
ros::Subscriber<s_rov_msgs::s_rov_control_esc> sub_esc_control("/s_rov/control/esc", &messageESC);
ros::Subscriber<s_rov_msgs::s_rov_control_led> sub_led_control("/s_rov/control/led", &messageLed);

s_rov_msgs::s_rov_state state_msg;
ros::Publisher pub_state("/s_rov/state", &state_msg);

/******************************ROS Configuration *****************************/

int main() {
    nh.initNode();
    // ros subscriber
    nh.subscribe(sub_extra_control);
    nh.subscribe(sub_esc_control);
    nh.subscribe(sub_led_control);
    
    // ros publisher
    nh.advertise(pub_state);
    
    ESC_switch = false;
    pwm.init();
    
    t.start();
    int prev_time, current_time;
    float voltage, current;
    
    prev_time = t.read_ms();
    while (1) {
        
        // 1 Hz : Publish state
        current_time = t.read_ms();
        if(current_time - prev_time >= 1000){
            voltage = (float)adc.read_single_channel(0)*0.003*4.0;
            current = (float)adc.read_single_channel(1)*0.003*16.7/5.0;
            state_msg.Voltage = voltage;
            state_msg.Current = current;
            pub_state.publish(&state_msg);
            
            prev_time = prev_time + 1000;
        }
        
        nh.spinOnce();
        wait_ms(1);     
    }
}