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

        * modify manipulation states
        * add most basic state import tests
        * add timeout outcome to constructor
      * Added velocity and acceleration scaling factors of 1 to moveit dyn goal to fix bug in TOTG which crashes with the default 0. (#13)

Author: David Conner

flexbe_manipulation_states/flexbe_manipulation_states/get_joint_values_dyn_state.py

@@ -36,6 +36,8 @@
 @author: Alberto Romay
 """
 
+from rclpy.duration import Duration
+
 from flexbe_core import EventState
 from flexbe_core.proxy import ProxySubscriberCached
 
@@ -45,31 +47,44 @@
 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_names          string[]    List of desired joint names.
 
-    #> joint_values float[]     List of current joint values.
+    #> 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().__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}, inst_id=id(self))
 
         self._joints = None
         self._joint_values = []
+        self._return_code = None
+        self._timeout = Duration(seconds=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, jnt_name in enumerate(msg.name):
@@ -78,12 +93,21 @@ 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 (self.get_clock().now() - self._start_time) > 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 = self.get_clock().now()
+        userdata.joint_values = None
 
     def on_exit(self, userdata):
         self._sub.disable_buffer(self._topic)

flexbe_manipulation_states/flexbe_manipulation_states/get_joint_values_state.py

@@ -36,6 +36,8 @@
 @author: Philipp Schillinger
 """
 
+from rclpy.duration import Duration
+
 from flexbe_core import EventState
 from flexbe_core.proxy import ProxySubscriberCached
 
@@ -46,28 +48,37 @@ class GetJointValuesState(EventState):
     """
     Retrieves current values of specified joints.
 
-    -- 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.
 
     """
 
-    def __init__(self, joints):
+    def __init__(self, joints, timeout=None, joint_states_topic='/joint_states'):
         """
         Constructor
         """
-        super().__init__(outcomes=['retrieved'],
+        super().__init__(outcomes=['retrieved', 'timeout'],
                          output_keys=['joint_values'])
 
-        self._topic = '/joint_states'
+        self._topic = joint_states_topic
         self._sub = ProxySubscriberCached({self._topic: JointState}, inst_id=id(self))
 
         self._joints = joints
         self._joint_values = []
+        self._return_code = None
+        self._timeout = Duration(seconds=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, jnt_name in enumerate(msg.name):
@@ -76,11 +87,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 (self.get_clock().now() - self._start_time) > 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 = self.get_clock().now()
+        userdata.joint_values = None
 
     def on_exit(self, userdata):
         self._sub.disable_buffer(self._topic)

flexbe_manipulation_states/flexbe_manipulation_states/get_joints_from_srdf_group.py

@@ -54,6 +54,8 @@ class GetJointsFromSrdfGroup(EventState):
     -- 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.
 
@@ -62,7 +64,7 @@ class GetJointsFromSrdfGroup(EventState):
 
     """
 
-    def __init__(self, move_group, robot_name=""):
+    def __init__(self, move_group, robot_name="", srdf_name="/robot_description_semantic"):
         """
         Constructor
         """
@@ -71,23 +73,31 @@ def __init__(self, move_group, robot_name=""):
 
         self._move_group = move_group
         self._robot_name = robot_name
+        self._srdf_name = srdf_name
 
         # Check existence of SRDF parameter.
         # Values will only be read during runtime to allow modifications.
         self._srdf_param = None
         try:
-            self._srdf_param = self._node.get_parameter('/robot_description_semantic')
+            self._srdf_param = self._node.get_parameter(self._srdf_name)
         except ParameterNotDeclaredException:
-            Logger.logerr('Unable to get parameter: /robot_description_semantic')
+            Logger.logerr(f"Unable to get srdf parameter: '{self._srdf_name}'")
 
         self._param_error = False
         self._file_error = False
         self._srdf = None
+        self._return_code = None
 
     def execute(self, userdata):
         """ execute the state """
+        if (self._return_code is not None):
+            # Handle blocked transition or error during on_enter
+            return self._return_code
+
         if self._param_error:
+            self._return_code = 'param_error'
             return 'param_error'
+
         robot = None
         for rbt in self._srdf.iter('robot'):
             if self._robot_name in ('', rbt.attrib['name']):
@@ -96,6 +106,7 @@ def execute(self, userdata):
         if robot is None:
             Logger.logwarn(f'Did not find robot name in SRDF: {self._robot_name}')
             self._param_error = True
+            self._return_code = 'param_error'
             return 'param_error'
 
         group = None
@@ -107,25 +118,39 @@ def execute(self, userdata):
         if group is None:
             Logger.logwarn(f'Did not find group name in SRDF: {self._move_group}')
             self._param_error = True
+            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 exc:  # pylint: disable=W0703
             Logger.logwarn(f'Unable to parse joint values from SRDF:\n{str(exc)}')
             self._param_error = True
+            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
+            self._return_code = 'param_error'
+            return
+
+        try:
+            self._srdf_param = self._node.get_parameter(self._srdf_name)
+        except ParameterNotDeclaredException:
+            Logger.logerr(f"Unable to get srdf parameter: '{self._srdf_name}'")
+            self._param_error = True
+            self._return_code = 'param_error'
             return
 
         try:
             self._srdf = ET.fromstring(self._srdf_param)
         except Exception as exc:  # pylint: disable=W0703
             Logger.logwarn(f'Unable to parse given SRDF parameter: /robot_description_semantic\n{exc}')
             self._param_error = True
+            self._return_code = 'param_error'
+            return

flexbe_manipulation_states/flexbe_manipulation_states/moveit_to_joints_dyn_state.py

@@ -123,6 +123,9 @@ def on_enter(self, userdata):
 
         action_goal = MoveGroup_Goal()
         action_goal.request.group_name = self._move_group
+        action_goal.request.max_velocity_scaling_factor = 1
+        action_goal.request.max_acceleration_scaling_factor = 1
+
         goal_constraints = Constraints()
         for i, jnt_name in enumerate(self._joint_names):
             goal_constraints.joint_constraints.append(JointConstraint(joint_name=jnt_name,

flexbe_manipulation_states/flexbe_manipulation_states/moveit_to_joints_state.py

@@ -120,6 +120,8 @@ def on_enter(self, userdata):
 
         action_goal = MoveGroup_Goal()
         action_goal.request.group_name = self._move_group
+        action_goal.request.max_velocity_scaling_factor = 1
+        action_goal.request.max_acceleration_scaling_factor = 1
         goal_constraints = Constraints()
         for i, jnt_name in enumerate(self._joint_names):
             goal_constraints.joint_constraints.append(JointConstraint(joint_name=jnt_name,

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