class GetJointValuesDynState(EventState): ''' Retrieves current values of specified joints. ># joint_names string[] List of desired joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. ''' def __init__(self): ''' Constructor ''' super(GetJointValuesDynState, self).__init__( outcomes=['retrieved'], output_keys=['joint_values'], input_keys=['joint_names']) self._topic = '/joint_states' self._sub = ProxySubscriberCached({self._topic: JointState}) self._joints = None self._joint_values = list() def execute(self, userdata): while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) for i in range(len(msg.name)): if msg.name[i] in self._joints \ and self._joint_values[self._joints.index(msg.name[i])] is None: self._joint_values[self._joints.index(msg.name[i])] = msg.position[i] if all(v is not None for v in self._joint_values): userdata.joint_values = self._joint_values return 'retrieved' def on_enter(self, userdata): self._sub.enable_buffer(self._topic) self._joint_values = [None] * len(self._joints) self._joints = userdata.joint_names def on_exit(self, userdata): self._sub.disable_buffer(self._topic)
class GetJointValuesDynState(EventState): ''' Retrieves current values of specified joints. ># joint_names string[] List of desired joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. ''' def __init__(self): ''' Constructor ''' super(GetJointValuesDynState, self).__init__(outcomes=['retrieved'], output_keys=['joint_values'], input_keys=['joint_names']) self._topic = '/joint_states' self._sub = ProxySubscriberCached({self._topic: JointState}) self._joints = None self._joint_values = list() def execute(self, userdata): while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) for i in range(len(msg.name)): if msg.name[i] in self._joints \ and self._joint_values[self._joints.index(msg.name[i])] is None: self._joint_values[self._joints.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self._joint_values): userdata.joint_values = self._joint_values return 'retrieved' def on_enter(self, userdata): self._sub.enable_buffer(self._topic) self._joint_values = [None] * len(self._joints) self._joints = userdata.joint_names def on_exit(self, userdata): self._sub.disable_buffer(self._topic)
class GetCurrentJointValuesListState(EventState): ''' Retrieves current values of specified joints. Joints are specified by parameter list. -- joint_names string[] List of desired joint names. -- timeout double Timeout value (optional) -- joint_states_topic string Optional name of joint states topic (default: /joint_states) #> joint_names string[] List of current joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. <= timeout Joint values are not available. ''' def __init__(self, joint_names, timeout=None, joint_states_topic='/joint_states'): ''' Constructor ''' super(GetCurrentJointValuesListState, self).__init__(outcomes=['retrieved', 'timeout'], output_keys=['joint_names', 'joint_values']) self.topic = joint_states_topic self.sub = ProxySubscriberCached({self.topic: JointState}) self.joint_names = joint_names self.joint_values = list() if (timeout is not None) and (timeout > 0.0): self.timeout_duration = rospy.Duration(timeout) else: self.timeout_duration = None def execute(self, userdata): if (self.return_code is not None): # Handle blocked transition or error during on_enter userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values return self.return_code while self.sub.has_buffered(self.topic): msg = self.sub.get_from_buffer(self.topic) for i in range(len(msg.name)): if msg.name[i] in self.joint_names \ and self.joint_values[self.joint_names.index(msg.name[i])] is None: self.joint_values[self.joint_names.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self.joint_values): userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values self.return_code = 'retrieved' return 'retrieved' if (self.timeout_duration is not None and \ (rospy.Time.now()-self.start_time) > self.timeout_duration ): Logger.logerr( 'Timeout %s - found %s of %s' % (self.name, str(self.joint_values), str(self.joint_names))) self.return_code = 'timeout' return 'timeout' def on_enter(self, userdata): self.sub.enable_buffer(self.topic) self.sub.remove_last_msg(self.topic, True) self.joint_values = [None] * len(self.joint_names) self.return_code = None self.start_time = rospy.Time.now() userdata.joint_names = self.joint_names # Clear user data until we have valid data userdata.joint_values = None def on_exit(self, userdata): self.sub.disable_buffer(self.topic)