Merge pull request #996 from tue-robotics/reset-all-arms

Add robot.reset_all_arms function.

challenge_demo/src/demo.py

@@ -179,9 +179,7 @@ def main():
         robot.lights.set_color(0, 0, 1)  # be sure lights are blue
 
         robot.head.look_at_standing_person()
-        for arm in robot.arms.itervalues():
-            arm.reset()
-            arm.send_gripper_goal('close', 0.0)
+        robot.reset_all_arms(gripper_timeout=0.0)
         robot.torso.reset()
 
         rospy.loginfo("Driving back to the starting point")

robot_skills/src/robot_skills/robot.py

@@ -133,16 +133,31 @@ def reset(self):
                 bodypart.reset()
         return all(results.values())
 
+    def reset_all_arms(self, arm_timeout=0.0, close_gripper=True, gripper_timeout=None):
+        """
+        Reset all arms of the robot, and by default, close their grippers.
+
+        The arm is always reset, the gripper is closed if 'close_gripper' is True (which is the default).
+
+        :param arm_timeout: How long to wait for resetting the arm, by default 0.0
+        :param close_gripper: By default 'True', which closes the gripper. If False, the gripper is not activated,
+        :param gripper_timeout: If specified and 'close_gripper' holds, timeout for closing the gripper.
+            If not specified, gripper movement uses 'arm_timeout'.
+        """
+        for arm in self._arms.itervalues():
+            if close_gripper:
+                if gripper_timeout is None:
+                    gripper_timeout = arm_timeout
+                arm.send_gripper_goal('close', timeout=gripper_timeout)
+
+            arm.reset(timeout=arm_timeout)
+
     def standby(self):
         if not self.robot_name == 'amigo':
             rospy.logerr('Standby only works for amigo')
             return
 
-        for arm in self._arms.itervalues():
-            arm.reset()
-        for arm in self._arms.itervalues():
-            arm.send_gripper_goal('close')
-
+        self.reset_all_arms()
         self.head.look_down()
         self.torso.low()
         self.lights.set_color(0, 0, 0)

robot_smach_states/src/robot_smach_states/reset.py

@@ -49,10 +49,7 @@ def __init__(self, robot, timeout=0.0):
         smach.State.__init__(self, outcomes=["done"])
 
     def execute(self, userdata=None):
-        for arm in self.robot.arms.itervalues():
-            arm.reset(timeout=self.timeout)
-            arm.send_gripper_goal('close', timeout=self.timeout)
-
+        self.robot.reset_all_arms(timeout=self.timeout)
         return "done"
 
 
@@ -75,9 +72,7 @@ def __init__(self, robot, timeout=0.0):
         smach.State.__init__(self, outcomes=["done"])
 
     def execute(self, userdata=None):
-        for arm in self.robot.arms.itervalues():
-            arm.reset()
-            arm.send_gripper_goal('close', timeout=self.timeout)
+        self.robot.reset_all_arms(gripper_timeout=self.timeout)
         self.robot.torso.reset()
         return "done"
 
@@ -91,9 +86,7 @@ def __init__(self,robot, timeout=0.0):
         self.timeout = timeout
 
     def execute(self, userdata=None):
-        for arm in self.robot.arms.itervalues():
-            arm.reset()
-            arm.send_gripper_goal('close', timeout=self.timeout)
+        self.robot.reset_all_arms(gripper_timeout=self.timeout)
         self.robot.head.reset(timeout=self.timeout)
         self.robot.torso.reset()
         return "done"