rosserial, Arduino and uArm [closed]

edit

I am also trying to get my uArm working with rosserial.

I experienced the same odd behaviour, that the chatter example works perfectly, the uArm Motion demo works perfectly, but when I try to combine both the connection to rosserial is not working anymore or the robot is moving very odd.

Any ideas someone?

I tried this without luck: http://answers.ros.org/question/16419...

Here my code

#include <EEPROM.h>
#include <UF_uArm.h>

#include <ros.h>
#include <sensor_msgs/JointState.h>

UF_uArm uarm; // initialize the uArm library

float positions[5] = {0.0};
char *joint_names[5] = {"SERVO_ROT", "SERVO_L", "SERVO_R", "SERVO_HAND_ROT", "SERVO_HAND"};

ros::NodeHandle nh;

sensor_msgs::JointState joint_state_msg;
ros::Publisher joint_state_pub("/joint_states", &joint_state_msg);

void setup()
{
 uarm.init();          // initialize the uArm position

 uarm.setServoSpeed(SERVO_R,    50);  // 0=full speed, 1-255 slower to faster
 uarm.setServoSpeed(SERVO_L,    50);  // 0=full speed, 1-255 slower to faster
 uarm.setServoSpeed(SERVO_ROT, 50);  // 0=full speed, 1-255 slower to faster

 nh.initNode();
 nh.advertise(joint_state_pub);

 joint_state_msg.position_length = 5;
 joint_state_msg.position = positions;
 joint_state_msg.name_length = 5;
 joint_state_msg.name = joint_names;
 delay(500);
}

void loop()
{
   uarm.setPosition(0, 0, 0, 0);        // original pose
   joint_state_msg.header.stamp = nh.now();
   joint_state_msg.position[0] = (float)uarm.readAngle(SERVO_ROT);
   joint_state_msg.position[1] = (float)uarm.readAngle(SERVO_L);
   joint_state_msg.position[2] = (float)uarm.readAngle(SERVO_R);
   joint_state_msg.position[3] = (float)uarm.readAngle(SERVO_HAND_ROT);
   joint_state_msg.position[4] = (float)uarm.readAngle(SERVO_HAND); 

   joint_state_pub.publish( &joint_state_msg );
   uarm.gripperDetach();
   delay(100);
   nh.spinOnce();
}