rosserial RosServoControl arduino with two servos

edit

ReHi, good to know, that somebody can reuse my old code ;-) "rostopic pub /servo1 std_msgs/UInt16 $i$j" was just a stresstest for my servos from bash... I expect rostopic did not support multiple topic from a single line. For performance reasons use python, lua or other languages for rapid testing. Cheers Christian

hey, let me help you with this... I am also doing the same

  #include "Arduino.h"
#else
  #include <wprogram.h>  
#endif

#include <Servo.h> 
#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

Servo servo1;
Servo servo2;

void servo1_cb( const std_msgs::UInt16& cmd_msg)
{
  servo1.write(cmd_msg.data); //set servo angle, should be from 0-180  
}
void servo2_cb( const std_msgs::UInt16& cmd_msg)
{
  servo2.write(cmd_msg.data);
}

ros::Subscriber<std_msgs::UInt16> sub1("servo1", servo1_cb);
ros::Subscriber<std_msgs::UInt16> sub2("servo2", servo2_cb);

void setup()
{

  nh.initNode();
  nh.subscribe(sub1);
  nh.subscribe(sub2);

  servo1.attach(9); //attach it to pin 9
  servo2.attach(10); // attach it to pin 10
}

void loop()
{
  nh.spinOnce();
  delay(1);
}

in the terminal use different servo names as servo1 or servo2 and insert your angle.

I am having trouble in subscribing this topic into the keyboard keypress (when one key is pressed- the command need to move in 180 angles) any ideas???? please help. also, help me how to write a node to subscribe and publish outer message (wireless topic) topic comes from Ros (not Arduino) that can control the servo.