control servo using sensor_msgs/JointState
I want to control some servos using sensor_msgs/JointState on Arduino UNO. I'm using Indigo. I write this code called move.cpp that i upload in the board.
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]);
}
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);
}
If i try to control two servos with the same code without one of then servo.write that works well, But when i try it with three servos they don't work and any error occurs. Someone know what can be the problem?
Thanks
You don't provide enough information to get a useful answer. What error do you get?, is the failing component written by you or a third party?, what is your ROS setup (distro, binaries/source build)?
Sorry i just edit the post ! when i upload the code in the board and run rostopic pub i don't get any error message so i think that all is ok but servos don't move to the position i indicate in the position array in the command.
Does any combination of two servos work?. This is to figure out if the problem is related to a specific servo, like
servo2
, or to _any_ third servo.Yes, I change the pins on the board but always work well using two servos.I think the problem is related to any third servo.When i run rostopic command in a terminal i don't get any error but servos don't work. I`m new user ROS so i don't know where can be the problem.
From what you say, it doesn't seem like the error is on the ROS side of things. Can you make three servos work in a simple, ROS-less example?
If i use geometry_msgs/Vector3 instead of sensor_msgs/JointState to send the position to three servos with the same code changing the struct to Vector3 type, they work well so i think servos are not the problem and i don't know why with one type of message they work and they don't with other type.
Vector3
is a fixed-size array. The memebers ofJointState
are dynamically-sized, and implemented withstd::vector
in C++. Maybe you're not properly resizing your JointState message. When populating it, you should eitherpush_back()
vector elements, orresize()
and then useoperator[]
.I write in void setup:
unsigned num=giro.name.size(); giro.position.resize(n); giro.velo.. giro.eff..
I get the following error
emilio_arduino/firmware/move.cpp:50:16: error: request for member ‘size’ in‘giro.sensor_msgs::JointState::name’, which is of non-class type ‘char**’ num=giro.name.size()