modify manipulation states to add timeout and other parameters (#11)

* modify manipulation states

* add most basic state import tests

* add timeout outcome to constructor

Author: dcconner

flexbe_manipulation_states/src/flexbe_manipulation_states/get_joint_values_dyn_state.py

@@ -15,32 +15,45 @@
 class GetJointValuesDynState(EventState):
 	'''
 	Retrieves current values of specified joints.
+	In this version, specified joint names are passed by user data
+
+	-- timeout				double		Timeout value (optional)
+
+	-- joint_states_topic	string		Optional name of joint states topic
+										(default: /joint_states)
 
 	># joint_names	string[]	List of desired joint names.
 
 	#> joint_values float[] 	List of current joint values.
 
 	<= retrieved 				Joint values are available.
+	<= timeout	 				Joint values are not available.
 
 	'''
 
-	def __init__(self):
+	def __init__(self, timeout=None, joint_states_topic='/joint_states'):
 		'''
 		Constructor
 		'''
 		super(GetJointValuesDynState, self).__init__(
-			outcomes=['retrieved'],
+			outcomes=['retrieved', 'timeout'],
 			output_keys=['joint_values'],
 			input_keys=['joint_names'])
-		
-		self._topic = '/joint_states'
+
+		self._topic = joint_states_topic
 		self._sub = ProxySubscriberCached({self._topic: JointState})
 
 		self._joints = None
 		self._joint_values = list()
-			
-		
+		self._return_code = None
+		self._timeout = timeout
+
+
 	def execute(self, userdata):
+		if (self._return_code is not None):
+			# Handle blocked transition or error during on_enter
+			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)):
@@ -50,14 +63,22 @@ def execute(self, userdata):
 
 		if all(v is not None for v in self._joint_values):
 			userdata.joint_values = self._joint_values
+			self._return_code = 'retrieved'
 			return 'retrieved'
 
-			
+		if (self._timeout is not None and \
+			(Time.now()-self._start_time) > rospy.Duration(self._timeout) ):
+			self._return_code = 'timeout'
+			return 'timeout'
+
+
 	def on_enter(self, userdata):
 		self._sub.enable_buffer(self._topic)
 		self._joint_values = [None] * len(self._joints)
 		self._joints = userdata.joint_names
-
+		self._return_code = None
+		self._start_time = rospy.Time.now()
+		userdata.joint_values = None
 
 	def on_exit(self, userdata):
 		self._sub.disable_buffer(self._topic)

flexbe_manipulation_states/src/flexbe_manipulation_states/get_joint_values_state.py

@@ -15,30 +15,40 @@
 class GetJointValuesState(EventState):
 	'''
 	Retrieves current values of specified joints.
+	Joints are specified by parameter list.
 
-	-- joints		string[]	List of desired joint names.
+	-- joints				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_values float[] 	List of current joint values.
 
 	<= retrieved 				Joint values are available.
+	<= timeout	 				Joint values are not available.
 
 	'''
 
-	def __init__(self, joints):
+	def __init__(self, joints, timeout=None, joint_states_topic='/joint_states'):
 		'''
 		Constructor
 		'''
-		super(GetJointValuesState, self).__init__(outcomes=['retrieved'],
-												output_keys=['joint_values'])
-		
-		self._topic = '/joint_states'
+		super(GetJointValuesState, self).__init__(outcomes=['retrieved', 'timeout'],
+													output_keys=['joint_values'])
+
+		self._topic = joint_states_topic
 		self._sub = ProxySubscriberCached({self._topic: JointState})
 
 		self._joints = joints
 		self._joint_values = list()
-			
-		
+		self._timeout = timeout
+
+
 	def execute(self, userdata):
+		if (self._return_code is not None):
+			# Handle blocked transition or error during on_enter
+			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)):
@@ -48,12 +58,20 @@ def execute(self, userdata):
 
 		if all(v is not None for v in self._joint_values):
 			userdata.joint_values = self._joint_values
+			self._return_code = 'retrieved'
 			return 'retrieved'
 
-			
+		if (self._timeout is not None and \
+			(Time.now()-self._start_time) > rospy.Duration(self._timeout) ):
+			self._return_code = 'timeout'
+			return 'timeout'
+
 	def on_enter(self, userdata):
 		self._sub.enable_buffer(self._topic)
 		self._joint_values = [None] * len(self._joints)
+		self._return_code = None
+		self._start_time = rospy.Time.now()
+		userdata.joint_values = None
 
 	def on_exit(self, userdata):
 		self._sub.disable_buffer(self._topic)

flexbe_manipulation_states/src/flexbe_manipulation_states/get_joints_from_srdf_group.py

@@ -14,14 +14,17 @@
 
 class GetJointsFromSrdfGroup(EventState):
 	'''
-	Simple state to look up a pre-defined joint configuration from the given joint group in a SRDF file.
+	Simple state to look up a pre-defined list of joint names from a given
+	joint group defined in a SRDF file.
 	This state is recommended if you only need these values without any unnecessary overhead.
 
 	-- move_group 	string 		Name of the move group of interest.
 								e.g., "my_moveit_config/config/my_robot.srdf"
 	-- robot_name 	string 		Optional name of the robot to be used.
 								If left empty, the first one found will be used
 								(only required if multiple robots are specified in the same file).
+	-- srdf_name	string		Optional name of the srdf parameter
+								(default: "/robot_description_semantic")
 
 	#> joint_names string[] 	List of joint values for the requested group.
 
@@ -30,30 +33,32 @@ class GetJointsFromSrdfGroup(EventState):
 
 	'''
 
-	def __init__(self, move_group, robot_name=""):
+	def __init__(self, move_group, robot_name="", srdf_name = "/robot_description_semantic"):
 		'''
 		Constructor
 		'''
 		super(GetJointsFromSrdfGroup, self).__init__(outcomes=['retrieved', 'param_error'],
-												output_keys=['joint_names'])
+														output_keys=['joint_names'])
 
 		self._move_group = move_group
 		self._robot_name = robot_name
+		self._srdf_name  = srdf_name
 
-		# Check existence of SRDF parameter.
-		# Anyways, values will only be read during runtime to allow modifications.
 		self._srdf_param = None
-		if rospy.has_param("/robot_description_semantic"):
-			self._srdf_param = rospy.get_param("/robot_description_semantic")
+		if rospy.has_param(self._srdf_name):
+			self._srdf_param = rospy.get_param(self._srdf_name)
 		else:
-			Logger.logerr('Unable to get parameter: /robot_description_semantic')
+			Logger.logerr('Unable to get parameter: ' + self._srdf_name)
 
 		self._file_error = False
 		self._srdf = None
+		self._return_code = None
 
-		
 	def execute(self, userdata):
 
+		if (self._return_code is not None):
+			# Handle blocked transition or error during on_enter
+			return self._return_code
 
 		robot = None
 		for r in self._srdf.iter('robot'):
@@ -62,6 +67,7 @@ def execute(self, userdata):
 				break
 		if robot is None:
 			Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
+			self._return_code = 'param_error'
 			return 'param_error'
 
 		group = None
@@ -71,25 +77,33 @@ def execute(self, userdata):
 				break
 		if group is None:
 			Logger.logwarn('Did not find group name in SRDF: %s' % self._move_group)
+			self._return_code = 'param_error'
 			return 'param_error'
 
 		try:
 			userdata.joint_names = [str(j.attrib['name']) for j in group.iter('joint')]
 		except Exception as e:
 			Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
+			self._return_code = 'param_error'
 			return 'param_error'
 
+		self._return_code = 'retrieved'
 		return 'retrieved'
 
-			
+
 	def on_enter(self, userdata):
-		# Parameter check
-		if self._srdf_param is None:
-			self._param_error = True
+		# Check existence of SRDF parameter.
+		# Values are read during runtime to allow modifications.
+		if rospy.has_param(self._srdf_name):
+			self._srdf_param = rospy.get_param(self._srdf_name)
+		else:
+			Logger.logerror("Unable to get parameter: "+self._srdf_name)
+			self._return_code = 'param_error'
 			return
 
+		# Parse the SRDF string
 		try:
 			self._srdf = ET.fromstring(self._srdf_param)
 		except Exception as e:
 			Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
-			self._param_error = True
+			self._return_code = 'param_error'

flexbe_manipulation_states/tests/get_joint_values_dyn_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.get_joint_values_dyn_state
+class: GetJointValuesDynState
+
+import_only: True

flexbe_manipulation_states/tests/get_joint_values_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.get_joint_values_state
+class: GetJointValuesState
+
+import_only: True

flexbe_manipulation_states/tests/get_joints_from_srdf_group_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.get_joints_from_srdf_group
+class: GetJointsFromSrdfGroup
+
+import_only: True

flexbe_manipulation_states/tests/get_joints_from_srdf_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.get_joints_from_srdf_state
+class: GetJointsFromSrdfState
+
+import_only: True

flexbe_manipulation_states/tests/joint_state_to_moveit_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.joint_state_to_moveit
+class: JointStateToMoveit
+
+import_only: True

flexbe_manipulation_states/tests/moveit_to_joints_dyn_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.moveit_to_joints_dyn_state
+class: MoveitToJointsDynState
+
+import_only: True

flexbe_manipulation_states/tests/moveit_to_joints_state_import.test

@@ -0,0 +1,5 @@
+# Declare state to be tested
+path: flexbe_manipulation_states.moveit_to_joints_state
+class: MoveitToJointsState
+
+import_only: True