Use handover detector to detect failed handover (#1327)

Merge after #1326 
Fixes one of the issues in #1325

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py

@@ -10,10 +10,13 @@
 
 from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER
 from robot_skills import get_robot
+from robot_skills.arm.arms import GripperTypes
 # ROS
 from pykdl_ros import VectorStamped
+from robot_smach_states.human_interaction import Say
+from robot_smach_states.manipulation.active_grasp_detector import ActiveGraspDetector
 from robot_smach_states.navigation import NavigateToSymbolic
-from robot_smach_states.util.designators import EdEntityDesignator
+from robot_smach_states.util.designators import EdEntityDesignator, ArmDesignator
 from smach import StateMachine, cb_interface, CBState
 
 item_img_dict = {
@@ -30,6 +33,7 @@ def __init__(self, robot):
         # noinspection PyProtectedMember
         arm = robot.get_arm()._arm
         picked_items = []
+        armdes = ArmDesignator(robot, {"required_gripper_types": [GripperTypes.GRASPING]})
 
         def send_joint_goal(position_array, wait_for_motion_done=True):
             # noinspection PyProtectedMember
@@ -76,8 +80,6 @@ def _ask_user(user_data):
 
             item_name = leftover_items[0]
 
-            picked_items.append(item_name)
-
             robot.speech.speak("Please put the {} in my gripper, like this".format(item_name), block=False)
             show_image("challenge_serve_breakfast", item_img_dict[item_name])
 
@@ -96,12 +98,27 @@ def _carrying_pose(user_data):
             robot.speech.speak("Thanks for that!", block=False)
             return "done"
 
+        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
+        def _remember_item(user_data):
+            picked_items.append(user_data["item_picked"])
+            return "done"
+
         with self:
             self.add("ROTATE", CBState(_rotate), transitions={"done": "HANDOVER_POSE"})
             self.add("HANDOVER_POSE", CBState(_handover_pose), transitions={"done": "ASK_USER"})
             self.add("ASK_USER", CBState(_ask_user),
-                     transitions={"succeeded": "CARRYING_POSE", "failed": "failed"})
-
+                     transitions={"succeeded": "CHECK_PICK_SUCCESSFUL", "failed": "failed"})
+            self.add("CHECK_PICK_SUCCESSFUL",
+                     ActiveGraspDetector(robot, armdes),
+                     transitions={'true': "ADD_ITEM_TO_LIST",
+                                  'false': "SAY_SOMETHING_WENT_WRONG",
+                                  'failed': "failed",
+                                  'cannot_determine': "SAY_SOMETHING_WENT_WRONG"}
+                     )
+            self.add("SAY_SOMETHING_WENT_WRONG", Say(robot, "Oops, it seems I missed it. Lets try again"),
+                     transitions={"spoken": "ASK_USER2"})
+            self.add("ASK_USER2", CBState(_ask_user), transitions={"succeeded": "ADD_ITEM_TO_LIST", "failed": "failed"})
+            self.add("ADD_ITEM_TO_LIST", CBState(_remember_item), transitions={"done": "CARRYING_POSE"})
             self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"})