Final changes.

LASR-at-Home · May 28, 2024 · a204111 · a204111
1 parent 6cfce10
commit a204111
Show file tree

Hide file tree

Showing 8 changed files with 338 additions and 10 deletions.
diff --git a/common/interfaces/coffee_shop_ui/robot-interface/components/screens/CreateOrder.tsx b/common/interfaces/coffee_shop_ui/robot-interface/components/screens/CreateOrder.tsx
@@ -8,18 +8,18 @@ type State = "edit" | "confirm";
 
 type Item =
   | "cup"
-  | "banana"
+  // | "banana"
   | "bottle";
 
 const items: Item[] = [
   "cup",
-  "banana",
+  // "banana",
   "bottle",
 ];
 
 const NAMES: { [key in Item]: string } = {
   cup: 'Coffee',
-  banana: 'Banana',
+  // banana: 'Banana',
   bottle: 'Coca Cola'
 }
 
@@ -110,7 +110,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="banana" order={order} setOrder={setOrder} /> */}
         <Card item="bottle" order={order} setOrder={setOrder} />
       </div>
       <div className="flex flex-row gap-4 flex-1">

diff --git a/tasks/coffee_shop/config/ai_festival.yaml b/tasks/coffee_shop/config/ai_festival.yaml
@@ -0,0 +1,152 @@
+counter:
+  cuboid:
+  - - -4.664883613586426
+    - 0.5069149136543274
+  - - -4.66
+    - 1.18
+  - - -3.78360652923584
+    - 1.1981416940689087
+  - - -3.700509548187256
+    - 0.4764649569988251
+  last_updated: '2024-05-21 12:43:44.778366'
+  location:
+    position: 
+      x: -4.296226348137036
+      y: 0.08967264124909166
+      z: 0.0
+    orientation: 
+      x: 0.0
+      y: 0.0
+      z: 0.7401058378508781
+      w: 0.6724904079457562
+tables:
+  table0:
+    last_updated: '2024-05-21 11:05:19.105609'
+    location:
+      orientation:
+        w: 0.026904741251259426
+        x: 0.0
+        y: 0.0
+        z: -0.9996380019277993
+      position:
+        x: -4.855822287964941
+        y: -2.7264784408971066
+        z: 0.0
+      semantic: end
+    num_persons: 0
+    objects_cuboid:
+    - - -7.309726603688448
+      - -3.1535004587235225
+    - - -5.49823308072611
+      - -3.274925312500865
+    - - -5.448570318314367
+      - -2.4770380084237065
+    - - -7.260063841276704
+      - -2.3556131546463646
+    order: []
+    persons_cuboid:
+    - - -8.15129910291433
+      - -3.898305859510039
+    - - -4.755986106323717
+      - -4.125894519868664
+    - - -4.606997819088486
+      - -1.7322326076371901
+    - - -8.002310815679097
+      - -1.5046439472785653
+    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-05-21 11:06:56.162367'
+    location:
+      orientation:
+        w: 0.6940979898029392
+        x: 0.0
+        y: 0.0
+        z: -0.719880532138159
+      position:
+        x: -2.4651278785547643
+        y: -1.2458226745654628
+        z: 0.0
+    objects_cuboid:
+    - - -2.8254424871901778
+      - -1.9543831504533626
+    - - -2.859456062960578
+      - -3.7829678107717317
+    - - -2.059597477265037
+      - -3.79774950535365
+    - - -2.0255839014946373
+      - -1.9691648450352808
+    persons_cuboid:
+    - - -3.61043175014456
+      - -1.140220183601119
+    - - -3.6741839713972766
+      - -4.567567388460139
+    - - -1.2746082143106547
+      - -4.611912472205893
+    - - -1.210855993057938
+      - -1.1845652673468736
+    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:
+    orientation:
+      w: 0.03122925045972576
+      x: 0.0
+      y: 0.0
+      z: 0.9995122480068585
+    position:
+      x: -2.8701601223027664
+      y: -0.3562256053497737
+      z: 0.0
+  approach2:
+    orientation:
+      w: 0.005185091989047549
+      x: 0.0
+      y: 0.0
+      z: -0.9999865573201797
+    position:
+      x: -3.973111012447058
+      y: -0.35096261264440465
+      z: 0.0
+  cuboid:
+  - - -4.7717108537934765
+    - -1.0936771098678093
+  - - -4.826165578352385
+    - 0.5047626855067713
+  - - -5.923852265904756
+    - 0.4656883095154718
+  - - -5.8693975413458475
+    - -1.1327514858591088
+  last_updated: '2024-05-21 11:11:40.469766'
+  location:
+    orientation:
+      w: 0.0101037455354343
+      x: 0.0
+      y: 0.0
+      z: -0.9999489558603255
+    position:
+      x: -1.2641920084155993
+      y: -0.30165943641891735
+      z: 0.0
+objects: {"cup" : "coffee", "bottle": "cocacola"}
+
+yolo_person_model: "yolov8n-seg.pt"
+yolo_objects_model: "yolov8n-seg.pt"
+yolo_counter_model: "yolov8n-seg.pt"
diff --git a/tasks/coffee_shop/config/ai_festival_2.yaml b/tasks/coffee_shop/config/ai_festival_2.yaml
@@ -0,0 +1,147 @@
+counter:
+  cuboid:
+  - - -4.589782504695045
+    - 0.5566271401427713
+  - - -4.66
+    - 1.18
+  - - -3.5521452061834102
+    - 0.861161335790206
+  - - -4.411487855334991
+    - 1.127419091956325
+  last_updated: '2024-05-21 12:43:44.778366'
+  location:
+    orientation:
+      w: 0.7771925436938631
+      x: 0.0
+      y: 0.0
+      z: 0.62926286242449
+    position:
+      x: -4.334041429088822
+      y: -0.009764382985697966
+      z: 0.0
+tables:
+  table0:
+    last_updated: '2024-05-21 12:43:37.890911'
+    location:
+      orientation:
+        w: 0.7591775571212552
+        x: 0.0
+        y: 0.0
+        z: 0.6508835815746187
+      position:
+        x: -4.568242683559272
+        y: 0.06684751379553569
+        z: 0.0
+      semantic: end
+    num_persons: 0
+    objects_cuboid:
+    - - -4.5898689635799474
+      - 0.5569712154888609
+    - - -3.73085110930462 
+      - 0.28950763771438615
+    - - -3.5518578665543012
+      - 0.8602260634594268
+    - - -4.410875720829629
+      - 1.1276896412339015
+    order: []
+    persons_cuboid:
+    - - -5.84265531735862
+      - -0.09704440767012379
+    - - -3.0747088980270094
+      - -0.9588714916100978
+    - - -2.2990715127756283
+      - 1.5142416866184116
+    - - -5.067017932107239
+      - 2.3760687705583856
+    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-05-21 11:06:56.162367'
+    location:
+      orientation:
+        w: 0.6940979898029392
+        x: 0.0
+        y: 0.0
+        z: -0.719880532138159
+      position:
+        x: -2.4651278785547643
+        y: -1.2458226745654628
+        z: 0.0
+    objects_cuboid:
+    - - -2.8254424871901778
+      - -1.9543831504533626
+    - - -2.859456062960578
+      - -3.7829678107717317
+    - - -2.059597477265037
+      - -3.79774950535365
+    - - -2.0255839014946373
+      - -1.9691648450352808
+    persons_cuboid:
+    - - -3.61043175014456
+      - -1.140220183601119
+    - - -3.6741839713972766
+      - -4.567567388460139
+    - - -1.2746082143106547
+      - -4.611912472205893
+    - - -1.210855993057938
+      - -1.1845652673468736
+    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:
+    orientation:
+      w: 0.03122925045972576
+      x: 0.0
+      y: 0.0
+      z: 0.9995122480068585
+    position:
+      x: -2.8701601223027664
+      y: -0.3562256053497737
+      z: 0.0
+  approach2:
+    orientation:
+      w: 0.005185091989047549
+      x: 0.0
+      y: 0.0
+      z: -0.9999865573201797
+    position:
+      x: -3.973111012447058
+      y: -0.35096261264440465
+      z: 0.0
+  cuboid:
+  - - -4.684506316772146
+    - 0.6076050756113518
+  - - -3.6937263427363503
+    - 0.47421678736600054
+  - - -3.5589995422023004
+    - 1.4627103631531602
+  - - -4.549779516238097
+    - 1.5960986513985114
+  last_updated: '2024-05-21 12:44:20.229716'
+  location:
+    orientation:
+      w: 0.4204985527347494
+      x: 0.0
+      y: 0.0
+      z: 0.9072932090278099
+    position:
+      x: -4.408414332163996
+      y: 0.11043913610712916
+      z: 0.0
diff --git a/tasks/coffee_shop/scripts/test_check_table.py b/tasks/coffee_shop/scripts/test_check_table.py
@@ -9,9 +9,18 @@
     rospy.init_node("test_check_table")
     sm = smach.StateMachine(outcomes=["end", "not_finished"])
     context = Context(sys.argv[1], sys.argv[2])
-    context.current_table = "table0"
+    context.current_table = "table1"
 
     with sm:
-        sm.add("CHECK_TABLE", CheckTable(context), transitions={"finished": "end"})
+        sm.add(
+            "CHECK_TABLE",
+            CheckTable(context),
+            transitions={
+                "has_needs_serving_tables": "end",
+                "not_finished": "end",
+                "idle": "end",
+                "has_free_tables": "end",
+            },
+        )
     sm.execute()
     rospy.signal_shutdown("down")
diff --git a/tasks/coffee_shop/scripts/test_make_order.py b/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"] = ["coffee", "coffee"]
+    context.tables[context.current_table]["order"] = ["cup", "cup"]
 
     with sm:
         sm.add(

diff --git a/tasks/coffee_shop/src/coffee_shop/add_coffee_shop_vo.py b/tasks/coffee_shop/src/coffee_shop/add_coffee_shop_vo.py
@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+import rospy
+import rosparam
+
+rospy.init_node("add_coffee_shop_vo")
+
+x1, y1 = -5.8, -2.0
+x2, y2 = -5.8, -0.5
+x3, y3 = -4.27, -0.63
+x4, y4 = -4.27, -2.18
+vo_dict = {
+    "vo_0096": ["submap_0", "pillar", x1, y1, 0.0],
+    "vo_0097": ["submap_0", "pillar", x2, y2, 0.0],
+    "vo_0098": ["submap_0", "pillar", x3, y3, 0.0],
+    "vo_0099": ["submap_0", "pillar", x4, y4, 0.0],
+}
+
+
+for vo, params in vo_dict.items():
+    rosparam.upload_params(f"mmap/vo/submap_0/{vo}", params)
diff --git a/tasks/coffee_shop/src/coffee_shop/phases/phase_1/phase_1.py b/tasks/coffee_shop/src/coffee_shop/phases/phase_1/phase_1.py
@@ -46,7 +46,7 @@ def __init__(self):
                     input_keys=[], output_keys=[], outcomes=["succeeded"]
                 )
                 def idle_cb(_):
-                    rospy.sleep(rospy.Duration(30.0))
+                    rospy.sleep(rospy.Duration(15.0))
                     return "succeeded"
 
                 smach.StateMachine.add(