Rosserial Arduino Moveit "joint_states" subscriber callback never called

edit

Hello,

I have followed the tutorial here and successfully managed to follow all steps right to the end (with a custom URDF) - however nothing happens when I execute a Moveit plan.

I have an Arduino Mega interfacing Moveit with my simple 6 dof arm, running the code below. I have placed a nh.loginfo() line in the loop, and a line in the JointsCallback(). I can see in the terminal running rosserial the "Loop" info, but not the "Callback called", which indicates the callback never gets called. I have checked with rostopic echo joint_states that the topic gets published to just fine. Finally, when I subscribe to a dummy topic in the Arduino code, say following the Blink tutorial of rosserial, the callback gets called as soon as a message is published... Yet I do the same things when subscribing to joint_states ! What is wrong with my joint_states subscriber?

I do exactly this: - 1st terminal: roscore - 2nd terminal: source ~/Documents/catkin_ws/devel/setup.bash, then rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200 - 3rd terminal: source ~/Documents/catkin_ws/devel/setup.bash, then roslaunch simple6dofarm_config demo.launch rviz_tutorial:=true

Arduino code: #include "Arduino.h"

#include <Servo.h> 

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

#define BAUDRATE 115200
#define ATTACH_DELAY 10

enum {
    HIP = 0,
    SHOULDER, //Shoulder has to be before elbow in the joint states message
    ELBOW,
    FOREARM,
    WRIST,
    TOOL_GRIP,
        CLAW, //First one of the claw joints has to be the actuator, the other ones will be ignored.
    N_JOINTS
};

//Pins
unsigned char const jointPin[N_JOINTS] = {
    8,
    4,
    2,
    10,
    12,
    6,
        14
};

unsigned char const jointNeutral[N_JOINTS] = {
    95, //Centred
    37, //arm vertical
    45, //crank perpendicular to arm
    95, // hand movement in arm plane
    90, //tool grip axis concentric to forearm
    90, //A prendre en compte au montage de l'outil, pince dans le plan du bras
        90  //TBC
};

//1 if directions identical between angles sent to servos directly and coordinate system in MoveIt, -1 otherwise
char const jointDirSign[N_JOINTS] = {
    1, //TBC
    1, //TBC
    1, //TBC
    1, //TBC
    -1, //TBC
    1, //TBC
        1 //TBC
};

Servo joints[N_JOINTS];

ros::NodeHandle nh;

void JointsCallback(const sensor_msgs::JointState& cmd_msg){
    float jointAngles[N_JOINTS];

        nh.loginfo("Callback called");

    for(unsigned int i = 0 ; i < N_JOINTS ; ++i) {
        jointAngles[i] = RadiansToDegrees(cmd_msg.position[i]);

        //Special processing for elbow: parallel mechanism actuator angle from joint angle
        if(i == ELBOW) {
            float elbowShoulderOff = -0.0028*pow(jointAngles[ELBOW],2)-0.7927*jointAngles[ELBOW]+77.845; //Obtained from ADAMS
            float shoulderAngleFromNeutral = jointAngles[SHOULDER]-jointNeutral[SHOULDER];
            joints[i].write(jointNeutral[ELBOW]+jointDirSign[ELBOW]*(shoulderAngleFromNeutral+elbowShoulderOff));
        }
        else {
            joints[i].write(jointNeutral[i]+jointDirSign[i]*jointAngles[i]);
        }
    } 
}

ros::Subscriber<sensor_msgs::JointState> jointsCallback("joint_states", &JointsCallback);

void setup(){
    nh.getHardware()->setBaud(BAUDRATE);
    nh.initNode();

    for(unsigned int i = 0 ; i < N_JOINTS ; ++i) {
        joints[i].attach(jointPin[i]);
                delay(ATTACH_DELAY);
                joints[i].write(jointNeutral[i]);
    }

        nh.subscribe(jointsCallback);
        delay(1000);
}

void loop(){
  nh.loginfo("Loop");
  nh.spinOnce();
}

inline float RadiansToDegrees(float position_radians)
{
  return position_radians * 57.2958;
}

Thanks for your help, Regards