Retrieving joint states from python script on demand without server

asked 2020-03-19 15:11:35 -0500

argargreg gravatar image

updated 2020-03-20 05:35:03 -0500


I'm trying to use the joint_states topic to extract the joint angles at specific times of a python script, but the variable that stores the joint angles, though locked by mutex, is not updated correctly. I adapted the code from an action server found here. Why doesn't it work? I have added two comments on print's to show when the value stops being as it should.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import time
import roslib; roslib.load_manifest('ur_driver')
import rospy
import actionlib
from control_msgs.msg import *
from trajectory_msgs.msg import *
from sensor_msgs.msg import JointState
import threading

JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
               'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Q1 = [2.2,0,-1.57,0,0,0]
Q2 = [1.5,0,-1.57,0,0,0]
Q3 = [1.5,-0.2,-1.57,0,0,0]

client = None #ROS driver client
jointsMaxSpeed = 0.2 #rad, default value (will be settable)
jointsAccel = 3 #rad/s², default value (will be settable)

#For now, the speed is sufficiently low and the accel is sufficiently high to assume max speed is reached immediately
def calcDur(orig, dest, maxSpeed, accel):
    return abs(dest-orig)/maxSpeed

class Robot:
    def __init__(self):
        #For joint states
        self.lock = threading.Lock() = []
        self.jointsAng = []
        self.jointsVel = []
        self.thread = threading.Thread(target=self.joint_states_listener)
        #end for joint states

    def joint_states_listener(self):
        rospy.Subscriber('joint_states', JointState, self.joint_states_callback)

    #callback function: when a joint_states message arrives, save the values
    def joint_states_callback(self, msg):
        self.lock.acquire() =
        self.jointsAng = msg.position
        #print self.jointsAng #This works
        self.jointsVel = msg.velocity

    #Joint values is a list of the angles of the joints, in the order set by JOINT_NAMES (in rad)
    def moveJ(self, jointReqs):
        jointsAngles = self.jointsAng
        print self.jointsAng #Does not work (yields 0)

        #Calculate minimum duration for the last joint to reach the specified location
        if(len(jointReqs) == len(JOINT_NAMES) and len(jointsAngles) == len(JOINT_NAMES)):
            #Then calculate the move duration
            minDur = 9999
            for i in range(len(jointReqs)):
                minDur = min(minDur, calcDur(jointsAngles[i], jointReqs[i], jointsMaxSpeed, jointsAccel))

            g = FollowJointTrajectoryGoal()
            g.trajectory = JointTrajectory()
            g.trajectory.joint_names = JOINT_NAMES
            g.trajectory.points = [JointTrajectoryPoint(positions=jointReqs, velocities=[0]*6, time_from_start=minDur)]
            print "Length mismatch between joint names, joint setpoints, and joint states" #Is triggered because length jointAngles is zero

#--- main loop

def main():
    global client
    client = actionlib.SimpleActionClient('/scaled_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    print "Waiting for server..."
    print "Connected to server"

    ur10 = Robot()

if __name__ == '__main__': main()

Thanks in advance.

edit retag flag offensive close merge delete


I'm not entirely sure how you are trying to retrieve JointStates here, but I believe the biggest issue with your script is the fact that rospy.wait_for_message(..) doesn't work the way you seem to think it does.

wait_for_message(..) requires arguments, which you are not passing, and it returns a message, it's not a sort of barrier that makes sure there are messages (somewhere).

If you want to retrieve a single message from a topic, without subscribing yourself, that is what rospy.wait_for_message(..) is for.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-20 05:26:42 -0500 )edit

Thanks for your comment. I had forgotten this line when trying out things, but wait_for_message() is not part of the problem unfortunately. Basically I am trying to get the same behaviour as the linked code (LatestJointStates), but without creating an additional node to offer the service. Just subscribe to the joint states topic and make sure the member variable self.jointsAng is the latest set of joint angles, to be used in the methods I call. I have no clue how to do that anymore - any idea?

argargreg gravatar image argargreg  ( 2020-03-20 05:38:36 -0500 )edit

Yes: use rospy.wait_for_message(..).

Your extra thread is unnecessary, rospy.spin() is unnecessary, the Subscriber is unnecessary, the locks are unnecessary and the callback is unnecessary.

See the documentation for how to use it.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-20 05:43:42 -0500 )edit

You're absolutely right, I don't know why the authors have chosen to do so! It works like a charm. Don't hesitate to answer and I'll accept it. Good day to you sir.

argargreg gravatar image argargreg  ( 2020-03-20 06:01:09 -0500 )edit