File content as of revision 4:851261a9273b:

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

#include"mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <Gobeur.h>
#include <gobeur_lib.h>
#include <Servo.h>

Servo gobeur_right_aspire_servo(servo_aspire_1_pin);
Servo gobeur_left_aspire_servo(servo_aspire_2_pin);
Servo gobeur_right_block_servo(servo_block_1_pin);
Servo gobeur_left_block_servo(servo_block_2_pin);
 
ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

ros::Subscriber<gobeur_node::Gobeur> sub("gobeur", &gobeur_action);

char hello[13] = "hello!";

void gobeur_initialize()
{
    gobeur_right_aspire_servo = 0;
    gobeur_left_aspire_servo = 0;
    wait(2);
    gobeur_right_aspire_servo = 1.0;
    gobeur_left_aspire_servo = 1.0;
    wait(2);
    gobeur_right_aspire_servo = 0;
    gobeur_left_aspire_servo = 0;
    wait(2);
    nh.loginfo("Actuators initialised");
    }
    
int aspirateur_on(uint8_t gobeur_id) 
{
    if(gobeur_id == 0)
    {
        gobeur_right_aspire_servo = 0.15;
        nh.loginfo("Aspirateur right on");
        }

    if(gobeur_id == 1)
    {
        gobeur_left_aspire_servo = 0.15;
        nh.loginfo("Aspirateur left on");
        }
}

int aspirateur_off(uint8_t gobeur_id)
{
    if(gobeur_id == 0)
    {
        gobeur_right_aspire_servo = 0.0;
        nh.loginfo("Aspirateur right off");
        }

    if(gobeur_id == 1)
    {
        gobeur_left_aspire_servo = 0.0;
        nh.loginfo("Aspirateur left off");
        }
}

int block_gobelet(uint8_t gobeur_id) 
{
    if(gobeur_id == 0)
        {
        gobeur_right_block_servo = 0.15;
        nh.loginfo("Gobelet right blocked");
        }
    if(gobeur_id == 1)
        {
        gobeur_left_block_servo = 0.15;
        nh.loginfo("Gobelet left blocked");
        }
}
int unblock_gobelet(uint8_t gobeur_id)
{
        {
        gobeur_right_block_servo = 0.0;
        nh.loginfo("Gobelet right unblocked");
        }
    if(gobeur_id == 1)
        {
        gobeur_left_block_servo = 0.0;
        nh.loginfo("Gobelet left unblocked");
        }
}

void gobeur_action(const gobeur_node::Gobeur& gobeur_msg){
    gobeur_right_status   = gobeur_msg.gobeur_right_status ; 
    gobeur_left_status  = gobeur_msg.gobeur_left_status ; 
    gobeur_right_aspire = gobeur_msg.gobeur_right_aspire ; 
    gobeur_left_aspire = gobeur_msg.gobeur_left_aspire ; 
    game_status = gobeur_msg.game_status ; 
    
    if ( gobeur_msg.init_status == 1 && game_status == 1 ) {
        gobeur_initialize(); 
    }
}

int main() {
    nh.initNode();
    nh.advertise(chatter);
    nh.subscribe(sub);
    
        str_msg.data = hello;
        chatter.publish( &str_msg );
        
    while (1) {
        
        switch(gobeur_right_aspire) {
            case 2 : unblock_gobelet(GOBEUR_RIGHT); 
                     aspirateur_on(GOBEUR_RIGHT);
                     wait(2);
                     block_gobelet(GOBEUR_RIGHT);
                     aspirateur_off(GOBEUR_RIGHT);
                     gobeur_right_aspire = 0;
                     break;      
            case 1 : aspirateur_off(GOBEUR_RIGHT);
                     unblock_gobelet(GOBEUR_RIGHT); 
                     gobeur_right_aspire = 0;
                     break;      
            case 0 : break;
            }
            
        switch(gobeur_right_status) {
            case 2: aspirateur_on(GOBEUR_RIGHT);
                     break;      
            case 1 : aspirateur_off(GOBEUR_RIGHT);
                     break;      
            }
            gobeur_right_status = -1;
        nh.spinOnce();
    }
}