Rwc2023 challenge serve breakfast (#1326)

- Challenge serve breakfast adopted to impuls but not tuned for it.
- issue #1325 keeps track of the issues that are still in the code. 
- Additional PRs to follow that fix the issues

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py

@@ -10,6 +10,10 @@
 
 from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER, PICK_ROTATION
 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.navigation import NavigateToSymbolic
 from robot_smach_states.util.designators import EdEntityDesignator
 from smach import StateMachine, cb_interface, CBState
@@ -31,7 +35,7 @@ def __init__(self, robot):
 
         def send_joint_goal(position_array, wait_for_motion_done=True):
             # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            arm._send_joint_trajectory([position_array], timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
@@ -52,10 +56,17 @@ def show_image(package_name, path_to_image_in_package):
 
         @cb_interface(outcomes=["done"])
         def _rotate(_):
-            arm.gripper.send_goal("close", timeout=0.)
-            robot.head.look_up()
-            vyaw = 0.5
-            robot.base.force_drive(0, 0, vyaw, PICK_ROTATION / vyaw)
+            arm.gripper.send_goal("close", timeout=0.0)
+
+            # Look to the operator
+            robot.head.look_at_point(VectorStamped.from_xyz(0.2, -0.2, 1.75, stamp=rospy.Time.now(),
+                                                            frame_id=robot.base_link_frame))
+            robot.head.wait_for_motion_done()
+            return "done"
+
+        @cb_interface(outcomes=["done"])
+        def _handover_pose(user_data):
+            send_joint_goal(JOINTS_HANDOVER, wait_for_motion_done=False)
             return "done"
 
         @cb_interface(outcomes=["succeeded", "failed"], output_keys=["item_picked"])
@@ -67,29 +78,33 @@ def _ask_user(user_data):
 
             item_name = leftover_items[0]
 
-            send_joint_goal(JOINTS_HANDOVER, wait_for_motion_done=False)
-
             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])
 
             send_gripper_goal("open")
-            rospy.sleep(10.0)
-            robot.speech.speak("Thanks for that!", block=False)
+            rospy.sleep(7.0)
             send_gripper_goal("close", max_torque=0.6)
             robot.head.reset()
 
             # Set output data
             user_data["item_picked"] = item_name
+            return "succeeded"
 
+        @cb_interface(outcomes=["done"])
+        def _carrying_pose(user_data):
             arm.send_joint_goal("carrying_pose", timeout=0.)
-
-            return "succeeded"
+            robot.speech.speak("Thanks for that!", block=False)
+            return "done"
 
         with self:
-            self.add("ROTATE", CBState(_rotate), transitions={"done": "ASK_USER"})
-            self.add("ASK_USER", CBState(_ask_user), transitions={"succeeded": "succeeded", "failed": "failed"})
+            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"})
+
+            self.add("CARRYING_POSE", CBState(_carrying_pose), transitions={"done": "succeeded"})
 
 
 class NavigateToAndPickItem(StateMachine):
@@ -101,7 +116,7 @@ def __init__(self, robot, pick_spot_id, pick_spot_navigation_area):
         with self:
             StateMachine.add(
                 "NAVIGATE_TO_PICK_SPOT",
-                NavigateToSymbolic(robot, {pick_spot: pick_spot_navigation_area}, pick_spot),
+                NavigateToSymbolic(robot, {pick_spot: pick_spot_navigation_area},pick_spot, speak=False),
                 transitions={"arrived": "PICK_ITEM", "unreachable": "failed", "goal_not_defined": "failed"},
             )
 

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py

@@ -42,7 +42,7 @@ def __init__(self, robot, table_id):
 
         def send_joint_goal(position_array, wait_for_motion_done=True):
             # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            arm._send_joint_trajectory([position_array], timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
@@ -83,10 +83,8 @@ def _align_with_table(user_data):
         def _place_and_retract(user_data):
             rospy.loginfo("Placing...")
             item_name = user_data["item_picked"]
-            if item_name in ["milk_carton"]:
+            if item_name in ["milk_carton", "cereal_box"]:
                 send_joint_goal(JOINTS_PLACE_HORIZONTAL_MILK)
-            elif item_name in ["milk_carton", "cereal_box"]:
-                send_joint_goal(JOINTS_PLACE_HORIZONTAL)
             else:
                 send_joint_goal(JOINTS_PLACE_VERTICAL)
 
@@ -129,7 +127,7 @@ def __init__(self, robot, table_id, table_close_navigation_area):
             )
             StateMachine.add(
                 "NAVIGATE_TO_TABLE_CLOSE",
-                NavigateToSymbolic(robot, {table: table_close_navigation_area}, table),
+                NavigateToSymbolic(robot, {table: table_close_navigation_area}, table, speak=False),
                 transitions={
                     "arrived": "PLACE_ITEM_ON_TABLE",
                     "unreachable": "SAY_PICK_AWAY_THE_CHAIR",
@@ -164,7 +162,7 @@ def __init__(self, robot, table_id, table_close_navigation_area):
             )
 
 @cb_interface(outcomes=["done"])
-def _publish_item_poses(marker_array_pub, items):
+def _publish_item_poses(user_data, marker_array_pub, items):
     """
     Publishes item poses as a visualization marker array
 
@@ -184,7 +182,7 @@ def _publish_item_poses(marker_array_pub, items):
         marker_msg.type = visualization_msgs.msg.Marker.SPHERE
         marker_msg.action = 0
         marker_msg.pose = posestamped.pose
-        marker_msg.pose.position.z += 1.0
+        marker_msg.pose.position.z = 1.0
         marker_msg.scale = Vector3(0.05, 0.05, 0.05)
         marker_msg.color = COLOR_DICT[k]
         array_msg.markers.append(marker_msg)

challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py

@@ -25,7 +25,7 @@ def __init__(self, robot, table_id):
 
         def send_joint_goal(position_array, wait_for_motion_done=True):
             # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
+            arm._send_joint_trajectory([position_array], timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
@@ -54,6 +54,7 @@ def _align_pour(_):
         def _pour(_):
             robot.speech.speak("Hope this goes well", block=False)
             send_joint_goal(JOINTS_PRE_POUR)
+            rospy.sleep(0.5)
             send_joint_goal(JOINTS_POUR)
             send_joint_goal(JOINTS_PRE_POUR)
             send_joint_goal(JOINTS_POST_PICK)

challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py

@@ -9,9 +9,10 @@
 from robot_smach_states.navigation import NavigateToWaypoint
 from robot_smach_states.startup import StartChallengeRobust
 from robot_smach_states.util.designators import EdEntityDesignator
+from robot_smach_states.world_model import UpdateEntityPose
 from robot_smach_states.utility import WaitTime
 from smach import State, StateMachine
-from .navigate_to_and_pick_item import NavigateToAndPickItem
+from .navigate_to_and_pick_item import NavigateToAndPickItem, NavigateToSymbolic
 from .navigate_to_and_place_item_on_table import NavigateToAndPlaceItemOnTable
 from .pick_pour_place_cereal import PickPourPlaceCereal
 from .tuning import REQUIRED_ITEMS
@@ -51,34 +52,53 @@ def execute(self, userdata):
 def setup_statemachine(robot):
     state_machine = StateMachine(outcomes=["done"])
     state_machine.userdata["item_picked"] = None
-    pick_id = "dinner_table"
+    pick_id = "closet"
     pick_area_id = "in_front_of"
     place_id = "dinner_table"
     place_area_id = "in_front_of"
     exit_id = "starting_pose"
+    table_des = EdEntityDesignator(robot=robot, uuid=place_id)
 
     with state_machine:
         # Intro
 
         StateMachine.add(
             "START_CHALLENGE_ROBUST",
             StartChallengeRobust(robot, "initial_pose"),
-            transitions={"Done": "SAY_START", "Aborted": "done", "Failed": "SAY_START"},
+            transitions={"Done": "NAVIGATE_TO_TABLE", "Aborted": "GOODBYE", "Failed": "NAVIGATE_TO_TABLE"},
+        )
+        #Main loop
+        StateMachine.add(
+            "NAVIGATE_TO_TABLE",
+            NavigateToSymbolic(robot, {table_des:place_area_id}, table_des, speak=True),
+            transitions={"arrived": "UPDATE_TABLE_POSE", "unreachable": "SAY_OPERATOR_WHERE_TO_STAND", "goal_not_defined": "SAY_OPERATOR_WHERE_TO_STAND"},
+            )
+
+        StateMachine.add(
+            "UPDATE_TABLE_POSE",
+            UpdateEntityPose(robot=robot, entity_designator=table_des),
+            transitions={"done": "SAY_OPERATOR_WHERE_TO_STAND"},
+        )
+
+        StateMachine.add(
+            "SAY_OPERATOR_WHERE_TO_STAND",
+            Say(
+                robot,
+                "Operator, please stand at the RIGHT of the dishwasher",
+                block=False,
+            ),
+            transitions={"spoken": "SAY_START"},
         )
 
         StateMachine.add(
             "SAY_START",
             Say(
                 robot,
-                #f"Lets serve some breakfast baby! If there are any chairs near the {place_id}, please remove them",
                 "Lets serve some breakfast baby! I will be asking for some fast handovers.",
                 block=False,
             ),
             transitions={"spoken": "NAVIGATE_AND_PICK_ITEM"},
         )
-
-        # Main loop
-
         StateMachine.add(
             "NAVIGATE_AND_PICK_ITEM",
             NavigateToAndPickItem(robot, pick_id, pick_area_id),
@@ -87,7 +107,7 @@ def setup_statemachine(robot):
 
         StateMachine.add(
             "NAVIGATE_AND_PICK_ITEM_FAILED", WaitTime(robot, 2),
-            transitions={"waited": "NAVIGATE_AND_PICK_ITEM", "preempted": "done"}
+            transitions={"waited": "NAVIGATE_AND_PICK_ITEM", "preempted": "GOODBYE"}
         )
 
         StateMachine.add(
@@ -97,7 +117,7 @@ def setup_statemachine(robot):
         )
 
         StateMachine.add(
-            "WAIT", WaitTime(robot, 2), transitions={"waited": "CHECK_IF_WE_HAVE_IT_ALL", "preempted": "done"}
+            "WAIT", WaitTime(robot, 2), transitions={"waited": "CHECK_IF_WE_HAVE_IT_ALL", "preempted": "GOODBYE"}
         )
 
         StateMachine.add(

challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py

@@ -15,10 +15,10 @@
 REQUIRED_ITEMS = ["spoon", "bowl", "milk_carton", "cereal_box"]
 
 ITEM_VECTOR_DICT = {
-    "spoon": PyKDL.Vector(0.03, -0.15, 0),
-    "bowl": PyKDL.Vector(0.1, -0.04, 0),
-    "milk_carton": PyKDL.Vector(-0.1, 0.3, 0),
-    "cereal_box": PyKDL.Vector(-0.1, -0.3, 0),
+    "spoon": PyKDL.Vector(0.0, -0.15, 0),
+    "bowl": PyKDL.Vector(0.0, 0.0, 0),
+    "milk_carton": PyKDL.Vector(-0.05, 0.15, 0),
+    "cereal_box": PyKDL.Vector(-0.05, -0.2, 0),
 }
 
 COLOR_DICT = {
@@ -28,7 +28,7 @@
     "cereal_box": ColorRGBA(1, 1, 0, 1),
 }
 
-PICK_ROTATION = 2.
+PICK_ROTATION = 0
 
 JOINTS_HANDOVER = [0.4, -0.2, 0.0, -1.37, 0]
 
@@ -45,16 +45,16 @@
 
 JOINTS_POST_PICK = [0.7, -1.2, 0, 0, 0]
 
-JOINTS_PRE_POUR = [0.5, -1.2, -1.5, 0, 0]
+JOINTS_PRE_POUR = [0.35, -1.2, 0, 0, 0]
 
-JOINTS_POUR = [0.5, -1.2, -2.5, 0, 0]
+JOINTS_POUR = [0.4, -1.2, -2.5, 0, 0]
 
-POUR_OFFSET_X = 0.03
+POUR_OFFSET_X = -0.15
 POUR_OFFSET_Y = 0.15
 
 
 def item_vector_to_item_frame(item_vector):
-    frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, -math.pi / 2), PyKDL.Vector(-0.05, 0.75, 0))
+    frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.7, 0, 0))
 
     item_placement_vector = item_vector
     item_frame = frame