Can any one help me how to write a simple subscriber to subscrribe joint position of a given joint ?

asked 2019-07-29 06:01:51 -0500

microbot gravatar image

Hello,

I have followed the following tutorial link text to create a joint state listener. Although this process is not working for me. I have a robot with 24 joints and I want to write a listener so that I can get joint position of the mentioned joint. The above mentioned link creates a handy server to filter out the required joint positions. I followed the same but I was unable to subsrcribe to the required joint. In order to test where the bug is , I have tried to first create the object from the LatestJointStates class, when I do that I am having the following error,

 File "/home/microbot/catkin_ws/src/spider/spider_control/control.py", line 31, in __init__
 rospy.init_node('joint_states_listener')
 File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/client.py", line 274, in init_node
 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: 
 "+str(_init_node_args))
 rospy.exceptions.ROSException: rospy.init_node() has already been called with different arguments: 
 ('joint_state_publisher', ['/home/microbot/catkin_ws/src/spider/spider_control/control.py'], True, None, False, False)

For reference I am also posting the code that is present in the link,

class  LatestJointStates:


def __init__(self):
    rospy.init_node('joint_states_listener')
    self.lock = threading.Lock()
    self.name = []
    self.position = []
    self.velocity = []
    self.effort = []
    self.thread = threading.Thread(target=self.joint_States_listener)
    self.thread.start()

    s = rospy.Service('return_joint_states', JointState, self.return_joint_states)
    #thread the function to listen to joint state messages
def joint_states_listener(self):
    rospy.Subscriber('joint_states', JointState, self.joint_states_callback)
    rospy.spin()

    #callback function inorder to save the values when joint_states are available
def joint_States_callback(self, msg):
    self.lock.acquire()
    self.name = msg.name
    self.position = msg.position
    self.velocity = msg.velocity
    self.effort = msg.effort
    self.lock.release()
    #function that returns joint variables for joint names that are given
def return_joint_state(self, joint_name):

        #no messages
    if self.name==[]:
        rospy.logerr("no robot_State messages received \n")
        return 0, 0, 0, 0
        #return info for this joint
    self.lock.acquire()
    if joint_name in self.name:
        index = self.name.index(joint_name)
        position = self.position[index]
        velocity = self.velocity[index]
        effort = self.effort[index]

        #joint name not found
    else:
        rospy.logerr("Joint %s not found", (joint_name))
        self.lock.release()
        return 0, 0, 0, 0
    self.lock.release()
    return 1, position, velocity, effort
    #server callback to return array of position velocity and effort of the called joint names

def return_joint_states(self, req):
    joints_found = []
    positions = []
    velocities = []
    efforts = []
    for joint_name in req.name:
        (found, position, velocity, effort)=self.return_joint_state(joint_name)
        joints_found.append(found)
        positions.append(position)
        velocities.append(velocity)
        efforts.append(effort)
    return ReturnJointStatesResponse(joints_found, positions, velocities, efforts)

This was supposed to be the service node as mentioned in the provided link and its called back as follows

def call_return_joint_states(joint_names):
rospy.wait_for_service("return_joint_states")
try:
    print("bug testing call_return joint states")
    s = rospy.ServiceProxy("return_joint_States", JointState)
    resp = s(joint_names)
except rospy.ServiceException as e:
    print ('error when calling return_joint_states:' + s +'is' + e)
    sys.exit(1)
for (ind, joint_name) in enumerate(joint_names):
    if(not resp.found[ind]):
        print ('joint %s ...
(more)
edit retag flag offensive close merge delete