ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Rosserial Arduino Moveit "joint_states" subscriber callback never called

asked 2020-01-22 12:52:18 -0500

argargreg gravatar image

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

edit retag flag offensive close merge delete

Comments

JointState messages can be big, and the default buffer size for Arduinos with rosserial is not. That could lead to your "dummy message" getting across perfectly fine (as it's small) and a JointState message never making it.

I don't use Arduinos, so I would suggest to search this site for arduino buffer size or similar keywords. Use Google, and add site:answers.ros.org at the end of the query.

Additionally: JointState messages are intended to convey state, not setpoints. You seem to want to use them for the latter, which goes against their semantics.

Finally: I don't know how often loop() is called (ie: what it's period is), but I often see people here on the forum having difficulties with Arduinos and loop rates when they don't sleep or delay for at least a little while each iteration. You may want to look into ...(more)

gvdhoorn gravatar image gvdhoorn  ( 2020-01-22 13:50:02 -0500 )edit

Thank you for your answer. I'm only getting started with ROS and MoveIt, and the FollowJointTrajectory interface seems much more complex than simply subscribing to the joint_states topic published by the FakeRobotController - so I started with that. Actually I had already tried playing with the input buffer, and the terminal running rosserial displays an error message if the message is too big for the input buffer so it was not that (although it was indeed my first lead as well). I have tried adding delays in the loop as well without success. In the end, I tried subscribing to move_group/fake_controller_joint_states and it worked, but I still don't understand why subscribing to joint_states, which is also valid because I can see the messages using rostopic echo joint_states, does not work.

argargreg gravatar image argargreg  ( 2020-01-23 09:05:15 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-01-23 09:05:48 -0500

argargreg gravatar image

In the end, I tried subscribing to move_group/fake_controller_joint_states and it worked.. The mystery is still unsolved as to why subscribing to joint_states, which is also valid because I can see the messages using rostopic echo joint_states, does not work.

edit flag offensive delete link more

Comments

At the end were you able to use the FollowJointTrajectory with rosserial ?

jkoubs gravatar image jkoubs  ( 2022-12-08 21:42:44 -0500 )edit

Question Tools

Stats

Asked: 2020-01-22 12:52:18 -0500

Seen: 802 times

Last updated: Jan 23 '20