[RESOLVED] Arduino subscribe to joint_states

edit

Hello,

I have connected my custom built robot arm to my arduino using adafruit PWM. I have tested subscribing to a simple UInt16 topic and manually publishing a message to move a servo. It works.

Now I want to link the arm to rviz. My goal is to use the sliders to move the joints on the screen, and have the real arm move as well.

This does not work at all.

The problem seems to be my callback on the arduino is never called. I have no idea why. It could be because no messages are getting there, but I do echo the joint_states topic in another window and I see the numbers changing as I slide the sliders. I'd expect these to arrive at the arduino as well?

It could be a problem with my arduino code setup, even though it worked with the simple subscription, it may not work with the more complex one. It is below.

Or maybe it is something else entirely.

(I have some problems with joint orientations in my model, with the intention of recalibrating the model properly at some point. So please ignore the dodgy code to convert radians to degrees)

Also I am only attempting to move one servo at this point, which is plugged into the adafruit at position 0, but this relates to joint 5 in the arm.

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

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

ros::NodeHandle nh;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  120 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  560 // this is the 'maximum' pulse length count (out of 4096)

#define BASE_SERVO 0
#define SHOULDER_SERVO 1
#define ELBOW_SERVO 2
#define FOREARM_SERVO 3
#define WRIST_SERVO 4
#define WRIST_ROLL_SERVO 5
#define GRIPPER_SERVO 6

#define VELOCITY 0.1

double targets[6] = {90,90,90,90,90,90};
double positions[6] = {90,90,90,90,90};

void servo_cb(const sensor_msgs::JointState& cmd_msg)
{
  digitalWrite(13, HIGH);
  targets[0] = radiansToDegrees(cmd_msg.position[5]);
  digitalWrite(13, LOW);

}

ros::Subscriber<sensor_msgs::JointState> sub("joint_states", servo_cb);

void setup()
{
  pinMode(13, OUTPUT);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  //nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.subscribe(sub);

  MoveServo(0,90);

}

void loop()
{

  // move towards target at speed velocity
  MoveServo(0, positions[0]);

  if (positions[0] < targets[0])
    positions[0] += VELOCITY;

  if (positions[0] > targets[0])
    positions[0] -= VELOCITY;


  nh.spinOnce();

}

void MoveServo(int servoNumber, double angleInDegrees)
{
  uint16_t pulselength = (uint16_t)map(angleInDegrees, 0, 180, SERVOMIN, SERVOMAX);

  pwm.setPWM(servoNumber, 0, pulselength);
}

double radiansToDegrees(float position_radians)
{

  position_radians = position_radians + 1.6;

  return position_radians * 57.2958;

}