Coffee shop rs

common/interfaces/coffee_shop_ui/robot-interface/components/screens/CreateOrder.tsx

@@ -8,19 +8,16 @@ type State = "edit" | "confirm";
 
 type Item =
   | "cup"
-  // | "banana"
-  | "bottle";
+  | "banana";
 
 const items: Item[] = [
   "cup",
-  // "banana",
-  "bottle",
+  "banana"
 ];
 
 const NAMES: { [key in Item]: string } = {
   cup: 'Coffee',
-  // banana: 'Banana',
-  bottle: 'Coca Cola'
+  banana: 'Banana'
 }
 
 export function CreateOrder({ finish }: { finish: (order: Item[]) => void }) {
@@ -110,8 +107,7 @@ function EditMode({
     <>
       <div className="flex flex-row gap-4 flex-[2] min-h-0">
         <Card item="cup" order={order} setOrder={setOrder} />
-        {/* <Card item="banana" order={order} setOrder={setOrder} /> */}
-        <Card item="bottle" order={order} setOrder={setOrder} />
+        <Card item="banana" order={order} setOrder={setOrder} />
       </div>
       <div className="flex flex-row gap-4 flex-1">
         <div

tasks/coffee_shop/config/motions.yaml

@@ -60,3 +60,8 @@ play_motion:
       points:
       - positions: [0.05, 0.0, -0.5]
         time_from_start: 0.0
+    tablet_no_head:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.35]
+        time_from_start: 0.0

tasks/coffee_shop/config/research_showcase.yaml

@@ -0,0 +1,152 @@
+counter:
+  cuboid:
+  - - 6.385480886114623
+    - 26.053033932057588
+  - - 6.512335358611623
+    - 25.466639879090895
+  - - 7.098741359648088
+    - 25.593552439607766
+  - - 6.971886887151087
+    - 26.17994649257446
+  last_updated: '2024-06-09 17:13:12.316195'
+  location:
+    orientation:
+      w: 0.9956514044661379
+      x: 0.0
+      y: 0.0
+      z: 0.09315729056068087
+    position:
+      x: 6.021648694375644
+      y: 25.69783072605752
+      z: 0.0
+tables:
+  table0:
+    last_updated: '2024-06-09 17:06:29.140552'
+    location:
+      orientation:
+        w: 0.13258267564351778
+        x: 0.0
+        y: 0.0
+        z: -0.9911719498246536
+      position:
+        x: 9.41866902109906
+        y: 26.351774527034294
+        z: 0.0
+      semantic: end
+    num_persons: 0
+    objects_cuboid:
+    - - 9.033581583294428
+      - 25.805967137066013
+    - - 8.852281808376198
+      - 26.939316643384483
+    - - 8.259815970776838
+      - 26.844531575478726
+    - - 8.441115745695068
+      - 25.711182069160255
+    order: []
+    persons_cuboid:
+    - - 9.94965761597872
+      - 25.143930179733303
+    - - 9.516114675956866
+      - 27.85411378179921
+    - - 7.343739938092545
+      - 27.506568532811436
+    - - 7.7772828781144
+      - 24.79638493074553
+    pre_location:
+      orientation:
+        w: 0.6981442535161398
+        x: 0.0
+        y: 0.0
+        z: -0.7159571225166993
+      position:
+        x: 3.4185893265341467
+        y: 1.5007719904983003
+        z: 0.0
+    status: unvisited
+  table1:
+    last_updated: '2024-06-09 17:08:30.928953'
+    location:
+      orientation:
+        w: 0.09526670117283224
+        x: 0.0
+        y: 0.0
+        z: -0.9954517846925818
+      position:
+        x: 5.712413766768246
+        y: 21.407430719219526
+        z: 0.0
+    objects_cuboid:
+    - - 5.407575647338826
+      - 20.96312899531904
+    - - 5.196037608221362
+      - 21.734394479759732
+    - - 4.424543687273396
+      - 21.52312444121242
+    - - 4.636081726390859
+      - 20.75185895677173
+    persons_cuboid:
+    - - 6.390607607404255
+      - 20.403133549425657
+    - - 5.755993490051864
+      - 22.716930002747734
+    - - 3.4415117272079665
+      - 22.083119887105806
+    - - 4.076125844560358
+      - 19.76932343378373
+    pre_location:
+      orientation:
+        w: 0.679755237535674
+        x: 0.0
+        y: 0.0
+        z: -0.7334390343053875
+      position:
+        x: 6.294876273276469
+        y: 2.3564053436919483
+        z: 0.0
+wait:
+  approach1:
+    position: 
+      x: 7.354957011662686
+      y: 23.54583447482398
+      z: 0.0
+    orientation: 
+      x: 0.0
+      y: 0.0
+      z: -0.9936295230126899
+      w: 0.1126959227193882
+  approach2:
+    position: 
+      x: 6.417241971006523
+      y: 23.293313284595392
+      z: 0.0
+    orientation: 
+      x: 0.0
+      y: 0.0
+      z: -0.9938574810686658
+      w: 0.11066755316643868
+  cuboid:
+  - - 5.530478314692662
+    - 22.707294950778433
+  - - 5.314449572896507
+    - 23.68361553552213
+  - - 4.340152013654879
+    - 23.46730109152614
+  - - 4.5561807554510345
+    - 22.490980506782442
+  last_updated: '2024-06-09 17:14:52.166498'
+  location:
+    orientation:
+      w: 0.09274159057330929
+      x: 0.0
+      y: 0.0
+      z: -0.9956902115507276
+    position:
+      x: 7.792123040557456
+      y: 23.654717441934658
+      z: 0.0
+objects: {"cup" : "coffee", "banana": "banana"}
+
+yolo_person_model: "yolov8x-seg.pt"
+yolo_objects_model: "yolov8x-seg.pt"
+yolo_counter_model: "yolov8x-seg.pt"

tasks/coffee_shop/scripts/test_make_order.py

@@ -17,7 +17,7 @@
     context = Context(sys.argv[1], sys.argv[2])
     context.current_table = "table0"
     context.tables[context.current_table]["status"] = "currently serving"
-    context.tables[context.current_table]["order"] = ["cup", "cup"]
+    context.tables[context.current_table]["order"] = ["banana"]
 
     with sm:
         sm.add(

tasks/coffee_shop/src/coffee_shop/context.py

@@ -19,7 +19,8 @@
 class Context:
     def __init__(self, config_path=None, tablet=False):
         self.tablet = tablet
-
+        self.tablet_on_head = False
+        rospy.loginfo(f"Tablet: {self.tablet}, Tablet on head: {self.tablet_on_head}")
         self.move_base_client = actionlib.SimpleActionClient(
             "/move_base", MoveBaseAction
         )

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"

tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/check_order.py

@@ -7,8 +7,9 @@
 import cv2_img
 from play_motion_msgs.msg import PlayMotionGoal
 from collections import Counter
-from geometry_msgs.msg import Point
+from geometry_msgs.msg import Point, Pose, Quaternion
 from shapely.geometry import Point as ShapelyPoint, Polygon
+from move_base_msgs.msg import MoveBaseGoal
 
 
 class CheckOrder(smach.State):
@@ -41,6 +42,15 @@ def execute(self, userdata):
 
         self.n_checks += 1
 
+        position = rospy.get_param("counter/location/position")
+        orientation = rospy.get_param("counter/location/orientation")
+        move_base_goal = MoveBaseGoal()
+        move_base_goal.target_pose.header.frame_id = "map"
+        move_base_goal.target_pose.pose = Pose(
+            position=Point(**position), orientation=Quaternion(**orientation)
+        )
+        self.context.move_base_client.send_goal_and_wait(move_base_goal)
+
         pm_goal = PlayMotionGoal(motion_name="check_table_low", skip_planning=True)
         self.context.play_motion_client.send_goal_and_wait(pm_goal)
 

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(

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(

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"

tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_order.py

@@ -5,9 +5,12 @@
 import difflib
 from play_motion_msgs.msg import PlayMotionGoal
 from control_msgs.msg import PointHeadGoal
-from geometry_msgs.msg import Point
+from geometry_msgs.msg import Point, Pose, Quaternion, PoseWithCovarianceStamped
 from coffee_shop_ui.msg import Order
 from std_msgs.msg import String
+from move_base_msgs.msg import MoveBaseGoal
+import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 
 class TakeOrder(smach.State):
@@ -90,8 +93,35 @@ def execute(self, userdata):
             self.context.voice_controller.sync_tts(
                 "Please use the tablet to make your order."
             )
-            pm_goal = PlayMotionGoal(motion_name="tablet", skip_planning=True)
-            self.context.play_motion_client.send_goal_and_wait(pm_goal)
+            if self.context.tablet_on_head:
+                pm_goal = PlayMotionGoal(motion_name="tablet", skip_planning=True)
+                self.context.play_motion_client.send_goal_and_wait(pm_goal)
+                rospy.loginfo("Tablet is on the head")
+            else:
+                rospy.loginfo("Tablet is not on the head")
+                robot_pose = rospy.wait_for_message(
+                    "/amcl_pose", PoseWithCovarianceStamped
+                ).pose.pose
+                target_orientation = R.from_quat(
+                    [
+                        robot_pose.orientation.x,
+                        robot_pose.orientation.y,
+                        robot_pose.orientation.z,
+                        robot_pose.orientation.w,
+                    ]
+                ) * 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(
+                    position=robot_pose.position,
+                    orientation=Quaternion(*target_orientation.as_quat()),
+                )
+                self.context.move_base_client.send_goal_and_wait(move_base_goal)
+                pm_goal = PlayMotionGoal(
+                    motion_name="tablet_no_head", skip_planning=True
+                )
+                self.context.play_motion_client.send_goal_and_wait(pm_goal)
+
             self.tablet_pub.publish(String("order"))
             order = rospy.wait_for_message("/tablet/order", Order).products
         else:

tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/wait_for_order.py

@@ -4,6 +4,10 @@
 from play_motion_msgs.msg import PlayMotionGoal
 import difflib
 from std_msgs.msg import Empty, String
+import numpy as np
+from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped
+from move_base_msgs.msg import MoveBaseGoal
+from scipy.spatial.transform import Rotation as R
 
 
 class WaitForOrder(smach.State):
@@ -37,8 +41,33 @@ def execute(self, userdata):
             self.context.voice_controller.sync_tts(
                 "Please press 'done' when you are ready for me to check the order."
             )
-            pm_goal = PlayMotionGoal(motion_name="tablet", skip_planning=True)
-            self.context.play_motion_client.send_goal_and_wait(pm_goal)
+            if self.context.tablet_on_head:
+                pm_goal = PlayMotionGoal(motion_name="tablet", skip_planning=True)
+                self.context.play_motion_client.send_goal_and_wait(pm_goal)
+            else:
+                robot_pose = rospy.wait_for_message(
+                    "/amcl_pose", PoseWithCovarianceStamped
+                ).pose.pose
+                target_orientation = R.from_quat(
+                    [
+                        robot_pose.orientation.x,
+                        robot_pose.orientation.y,
+                        robot_pose.orientation.z,
+                        robot_pose.orientation.w,
+                    ]
+                ) * 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(
+                    position=robot_pose.position,
+                    orientation=Quaternion(*target_orientation.as_quat()),
+                )
+                self.context.move_base_client.send_goal_and_wait(move_base_goal)
+                pm_goal = PlayMotionGoal(
+                    motion_name="tablet_no_head", skip_planning=True
+                )
+                self.context.play_motion_client.send_goal_and_wait(pm_goal)
+
             self.tablet_pub.publish(String("done"))
             rospy.wait_for_message("/tablet/done", Empty)
             return "done"