Python subscribe JointState problem

[Steven Schell]

Hi community,

i want to read the end position at the "joint_states"-topic. "Plan and Execute" via MoveIt! is successful.

I subscribe the "joint_states"-topic to read it, but if i want to read the end position, the subscribe will be in an endless loop.

How can i read the end position after "Plan and Execute" one time ??

Here is my code:

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
from control_msgs.msg import FollowJointTrajectoryActionResult
#from control_msgs.msg import FollowJointTrajectoryActionGoal

def callback(data):
    print "I hear: ", data.status.status
    if data.status.status == 3:
        rospy.Subscriber("joint_states", JointState, callback_pos)

def callback_pos(data):
    print "Position: ", data.position

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("joint_trajectory_action/result", FollowJointTrajectoryActionResult, callback)   
    rospy.spin()

if __name__ == '__main__':
    listener()

Thanks 4 help

[Philippe Capdepuy]

Hi Steven,

If you want to get the cartesian end_point position the topic you should listen to is:
Endpoint State
/robot/limb/<side>/endpoint_state (baxter_core_msgs/EndpointState)

--

Regards,

Dr. Philippe Capdepuy

Research Engineer

Génération Robots / HumaRobotics

Tel : +33 5 56 39 37 05

www.humarobotics.com

www.generationrobots.com

 

Découvrez Baxter, votre nouveau compagnon

pour la recherche et l’éducation

Baxter Research Robot

[Patrick Goebel]

Hi Philippe,

Is that topic Baxter specific? Because I don't see it being published when running a stock MoveIt installation with my own Dynamixel-based arm.

--patrick

[Evan Sonderegger]

Hi Steven,

What your code appears to be doing is creating a subscriber to the joint_states topic on the event of the joint_trajectory_action/result topic receiving a message. That joint_states subscriber then prints a message every time the joint_states topic is published to, which should be quite frequently.

If what you want to be doing is only printing the joint_states when joint_trajectory_action/result is published to, you could create two subscribers and a global variable. something like:

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import JointState

from control_msgs.msg import FollowJointTrajectoryActionResult

#from control_msgs.msg import FollowJointTrajectoryActionGoal

printJointStates = False

def joint_trajectory_callback(data):

    global printJointStates

    print "I hear: ", data.status.status

    printJointStates = True

def joint_state_callback(data):

    global printJointStates

    if printJointStates:

        print "Position: ", data.position

        printJointStates = False

def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("joint_trajectory_action/result",

                     FollowJointTrajectoryActionResult,

                     joint_trajectory_callback)

    rospy.Subscriber("joint_states", JointState, joint_state_callback)

    rospy.spin()

if __name__ == '__main__':

    listener()

Hope this helps!

Evan

[Philippe Capdepuy]

You can also use: rospy.wait_for_message to get just one message from joint_states (instead of subscribing etc...)

[Steven Schell]

Thank you Evan,
i got only one message!
Now how can i say that i store only the last position?
I try and compare with die "move_group/goal" topic, but my positions are a little different.
I use rospy.sleep(5) befor printJointStates is changed to True. But it's still different.

How can i fix this?

[Evan Sonderegger]

Unfortunately, these problems sound like they may be specific to your particular robot instead of being general in nature.

Are you sure that your robot is actually reaching the goal state as specified by move_group/goal? It might be a good idea to run "rostopic echo /joint_states" in separate tab of your terminal just to make sure joint_states is reflecting reality. Is waiting 5 seconds after joint_trajectory_action/result is published yielding a different set of joint states?

As for storing the current position of your robot, if you're using moveit_commander, you could call get_current_pose on your active group and store that result to a variable.

Also... Thank you to Philippe for pointing out rospy.wait_for_message! I didn't know that function existed and I agree it would be a much more elegant solution here.

[Steven Schell]

At the moment i use the industrial robot simulation. My robot (Igus Robolink) is at the moment not ready.
Tomorrow i test my program with the wait_for_message.

Thanks at all for help!!