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)
