diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py index 20238d067..791732300 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py +++ b/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"})