From 9f955600ef104ffdff3a957cb6b660987b085bf4 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 25 Jul 2023 18:50:42 +0200 Subject: [PATCH 1/4] add handoverdetector --- .../navigate_to_and_pick_item.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) 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 039f8940d..37e15137a 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 @@ -14,8 +14,9 @@ # 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 = { @@ -32,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 @@ -102,8 +104,14 @@ def _carrying_pose(user_data): 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': "CARRYING_POSE", + 'false': "SAY_SOMETHING_WENT_WRONG", + 'failed': "failed", + 'cannot_determine': "SAY_SOMETHING_WENT_WRONG"} + ) self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"}) From 0068a7bddbc94692a5236603b036db017b75afbc Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 25 Jul 2023 18:56:57 +0200 Subject: [PATCH 2/4] add fallback states --- .../src/challenge_serve_breakfast/navigate_to_and_pick_item.py | 3 +++ 1 file changed, 3 insertions(+) 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 37e15137a..ff1751172 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 @@ -112,6 +112,9 @@ def _carrying_pose(user_data): '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": "CARRYING_POSE", "failed": "failed"}) self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"}) From 646ef059c7450792376aa02cc5639552ee44edb8 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 25 Jul 2023 19:11:46 +0200 Subject: [PATCH 3/4] only list item as picked when this is confirmed --- .../navigate_to_and_pick_item.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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 ff1751172..a5aa205c8 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 @@ -80,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]) @@ -100,6 +98,11 @@ 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"}) @@ -107,14 +110,15 @@ def _carrying_pose(user_data): transitions={"succeeded": "CHECK_PICK_SUCCESSFUL", "failed": "failed"}) self.add("CHECK_PICK_SUCCESSFUL", ActiveGraspDetector(robot, armdes), - transitions={'true': "CARRYING_POSE", + 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": "CARRYING_POSE", "failed": "failed"}) + 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"}) From c2a5b2f1c41b26c9b9dfcb2bf6222272d8d28fb7 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 8 Aug 2023 19:46:34 +0200 Subject: [PATCH 4/4] add grippertypes import --- .../src/challenge_serve_breakfast/navigate_to_and_pick_item.py | 1 + 1 file changed, 1 insertion(+) 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 763395952..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,6 +10,7 @@ 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