add new outcome object_not-grasped to grab.py

Author: GustavoDCC

challenge_manipulation/src/manipulation.py

@@ -516,7 +516,8 @@ def lock(userdata=None):
             smach.StateMachine.add( "GRAB_ITEM",
                                     Grab(robot, self.current_item, self.empty_arm_designator),
                                     transitions={   'done'              :'STORE_ITEM',
-                                                    'failed'            :'SAY_GRAB_FAILED'})
+                                                    'failed'            :'SAY_GRAB_FAILED',
+                                                    'object_not_grasped': 'SAY_GRAB_FAILED'})
 
             smach.StateMachine.add( "SAY_GRAB_FAILED",
                                     states.Say(robot, ["I couldn't grab this thing"], mood="sad"),

challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py

@@ -116,7 +116,8 @@ def lock(userdata=None):
             smach.StateMachine.add("GRAB_ITEM",
                                    states.Grab(robot, self.grab_designator, self.empty_arm_designator),
                                    transitions={'done': 'UNLOCK_ITEM_SUCCEED',
-                                                'failed': 'UNLOCK_ITEM_FAIL'})
+                                                'failed': 'UNLOCK_ITEM_FAIL',
+                                                'object_not_detected': 'UNLOCK_ITEM_FAIL'})
 
             @smach.cb_interface(outcomes=["unlocked"])
             def lock(userdata=None):

robot_smach_states/src/robot_smach_states/clear.py

@@ -111,7 +111,8 @@ def __init__(self, robot, source_location, source_navArea, target_location, targ
             smach.StateMachine.add('GRAB',
                                    Grab(robot, selected_entity_designator, arm_des),
                                    transitions={'done': 'INSPECT_TARGET',
-                                                'failed': 'failed'}
+                                                'failed': 'failed',
+                                                'object_not_grasped': 'failed'}
                                    )
 
             smach.StateMachine.add('INSPECT_TARGET',

robot_smach_states/src/robot_smach_states/manipulation/grab.py

@@ -331,7 +331,7 @@ def __init__(self, robot, item, arm):
         :param item: Designator that resolves to the item to grab. E.g. EntityByIdDesignator
         :param arm: Designator that resolves to the arm to use for grabbing. Eg. UnoccupiedArmDesignator
         """
-        smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
+        smach.StateMachine.__init__(self, outcomes=['done', 'failed', 'object_not_grasped'])
 
         # Check types or designator resolve types
         check_type(item, Entity)
@@ -357,7 +357,7 @@ def __init__(self, robot, item, arm):
 
             smach.StateMachine.add('GRASP_DETECTOR', ActiveGraspDetector(robot, arm),
                                    transitions={'true': 'done',
-                                                'false': 'RETRY_GRAB',
+                                                'false': 'object_not_grasped',
                                                 'failed': 'done',
                                                 'cannot_determine': 'RETRY_GRAB'})
 
@@ -367,7 +367,7 @@ def __init__(self, robot, item, arm):
 
             smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm),
                                    transitions={'true': 'done',
-                                                'false': 'SAY_FAILED',
+                                                'false': 'object_not_grasped',
                                                 'failed': 'done',
                                                 'cannot_determine': 'SAY_FAILED'})