Control 3 servos using sensor_msgs/JointState

edit

Hi !!

I try to control three servos in an Arduino board using sensor_msgs/JointState to send the position. I write this code for two servos and they work well, but when i include one more servo.write into the void function i don't get any error and the compilation is right but they don't work and i don't know what is wrong.

ros::NodeHandle nh;

sensor_msgs::JointState giro;

Servo servo,servo1,servo2;

void servo_cb( const sensor_msgs::JointState &giro)

{

servo.write((int)giro.position[0]);

servo1.write((int)giro.position[1]);

servo2.write((int)giro.position[2]);

}

Please replay if you know, i need help with the final project of my career. THANKS!! ;)

ros::Subscriber<sensor_msgs::jointstate> sub("servo", servo_cb);

void setup()

{

pinMode(13, OUTPUT);

nh.initNode();

nh.subscribe(sub);

servo2.attach(11);

servo.attach(9);

servo1.attach(8);

}

void loop() {

nh.spinOnce();

delay(1);

}