Added grasp detection after grab and retry grab

Author: P-ict0

robot_smach_states/src/robot_smach_states/manipulation/grab.py

@@ -17,6 +17,8 @@
 from ..manipulation.grasp_point_determination import GraspPointDeterminant
 from ..util.designators.arm import ArmDesignator
 from ..util.designators.core import Designator
+from ..manipulation.active_grasp_detector import ActiveGraspDetector
+from robot_smach_states.human_interaction import Say
 
 
 class PrepareEdGrasp(smach.State):
@@ -349,9 +351,28 @@ def __init__(self, robot, item, arm):
                                                 'failed': 'RESET_FAILURE'})
 
             smach.StateMachine.add('GRAB', PickUp(robot, arm, item),
-                                   transitions={'succeeded': 'done',
+                                   transitions={'succeeded': 'GRASP_DETECTOR',
                                                 'failed': 'RESET_FAILURE'})
 
+            smach.StateMachine.add('GRASP_DETECTOR', ActiveGraspDetector(robot, arm),
+                                   transitions={'true': 'done',
+                                                'false': 'RETRY_GRAB',
+                                                'failed': 'done',
+                                                'cannot determine': 'done'})
+
+            smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item),
+                                   transitions={'succeeded': 'RETRY_GRASP_DETECTOR',
+                                                'failed': 'RETRY_GRASP_DETECTOR'})
+
+            smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm),
+                                   transitions={'true': 'done',
+                                                'false': 'SAY_FAILED',
+                                                'failed': 'done',
+                                                'cannot determine': 'done'})
+
+            smach.StateMachine.add('SAY_FAILED', Say(robot, "I failed grabbing the object"),
+                                   transitions={'spoken': 'failed'})
+
             smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm),
                                    transitions={'done': 'failed'})