From 4ae891040b38329672bb69426f477169eccb6ca7 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Sun, 9 Jun 2024 20:15:30 +0100 Subject: [PATCH] Done. --- tasks/coffee_shop/config/motions.yaml | 2 +- .../src/coffee_shop/phases/phase_1/states/check_table.py | 6 +++--- .../src/coffee_shop/phases/phase_2/states/deliver_order.py | 2 +- .../src/coffee_shop/phases/phase_2/states/load_order.py | 2 +- .../src/coffee_shop/phases/phase_2/states/make_order.py | 4 +--- .../src/coffee_shop/phases/phase_2/states/take_order.py | 2 +- .../src/coffee_shop/phases/phase_2/states/wait_for_order.py | 2 +- 7 files changed, 9 insertions(+), 11 deletions(-) diff --git a/tasks/coffee_shop/config/motions.yaml b/tasks/coffee_shop/config/motions.yaml index 088d62c2b..02cb070fa 100644 --- a/tasks/coffee_shop/config/motions.yaml +++ b/tasks/coffee_shop/config/motions.yaml @@ -63,5 +63,5 @@ play_motion: tablet_no_head: joints: [torso_lift_joint] points: - - positions: [0.05] + - positions: [0.35] time_from_start: 0.0 diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_1/states/check_table.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_1/states/check_table.py index 88803c76e..f3baf0d87 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_1/states/check_table.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_1/states/check_table.py @@ -186,11 +186,11 @@ def execute(self, userdata): if table["status"] == "needs serving" ] - if len(unvisited_tables) > 0: - return "not_finished" - elif len(needs_serving_tables) > 0: + if len(needs_serving_tables) > 0: return "has_needs_serving_tables" elif len(free_tables) > 0: return "has_free_tables" + elif len(unvisited_tables) > 0: + return "not_finished" else: return "idle" diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/deliver_order.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/deliver_order.py index b2a198aff..245d2bb05 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/deliver_order.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/deliver_order.py @@ -20,7 +20,7 @@ def execute(self, userdata): target_orientation = R.from_quat( [orientation["x"], orientation["y"], orientation["z"], orientation["w"]] ) - target_orientation *= R.from_euler("z", np.pi, degrees=False) + target_orientation *= R.from_euler("z", 180.0, degrees=True) move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "map" move_base_goal.target_pose.pose = Pose( diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/load_order.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/load_order.py index a5595bf2f..8200c78fa 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/load_order.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/load_order.py @@ -26,7 +26,7 @@ def execute(self, userdata): robot_pose.orientation.z, robot_pose.orientation.w, ] - ) * R.from_euler("z", np.pi, degrees=False) + ) * R.from_euler("z", 180.0, degrees=True) move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "map" move_base_goal.target_pose.pose = Pose( diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/make_order.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/make_order.py index 95f57fca1..283496ee1 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/make_order.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/make_order.py @@ -20,7 +20,5 @@ def execute(self, userdata): for item, count in Counter(order).items() ] ).replace(", ", ", and ", len(order) - 2) - self.context.voice_controller.sync_tts( - f"Please get me {order_string}. Make sure you place the items in clear view of the robot." - ) + self.context.voice_controller.sync_tts(f"Please get me {order_string}.") return "done" diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_order.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_order.py index 401f44924..aaf116d08 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_order.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_order.py @@ -109,7 +109,7 @@ def execute(self, userdata): robot_pose.orientation.z, robot_pose.orientation.w, ] - ) * R.from_euler("z", np.pi, degrees=False) + ) * R.from_euler("z", 180.0, degrees=True) move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "map" move_base_goal.target_pose.pose = Pose( diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/wait_for_order.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/wait_for_order.py index 610e33b59..e47ff5915 100644 --- a/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/wait_for_order.py +++ b/tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/wait_for_order.py @@ -55,7 +55,7 @@ def execute(self, userdata): robot_pose.orientation.z, robot_pose.orientation.w, ] - ) * R.from_euler("z", np.pi, degrees=False) + ) * R.from_euler("z", 180.0, degrees=True) move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "map" move_base_goal.target_pose.pose = Pose(