(hopefully) all ready.

Author: jws-1

common/interfaces/coffee_shop_ui/robot-interface/public/tiago.jpg

@@ -1,7 +1,6 @@
 <launch>
     <arg name="config" default="full"/>
     <arg name="tablet" default="true"/>
-    <arg name="tablet_on_head" default="true"/>
     <rosparam command="load" file="$(find coffee_shop)/config/$(arg config).yaml" />
-    <node pkg="coffee_shop" type="main.py" name="main" output="screen" args="$(find coffee_shop)/config/$(arg config).yaml $(arg tablet) $(arg tablet_on_head)"/>
+    <node pkg="coffee_shop" type="main.py" name="main" output="screen" args="$(find coffee_shop)/config/$(arg config).yaml $(arg tablet)"/>
 </launch>

tasks/coffee_shop/launch/coffee_shop.launch

@@ -7,10 +7,10 @@
 if __name__ == "__main__":
     rospy.init_node("coffee_shop")
 
-    if len(sys.argv) < 4:
+    if len(sys.argv) < 3:
         rospy.signal_shutdown("No configuration was provided")
         sys.exit()
-    context = Context(sys.argv[1], sys.argv[2], sys.argv[3])
+    context = Context(sys.argv[1], sys.argv[2])
 
     coffee_shop = CoffeeShop(context)
     outcome = coffee_shop.execute()

tasks/coffee_shop/nodes/main.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/scripts/test_make_order.py

@@ -17,10 +17,10 @@
 
 
 class Context:
-    def __init__(self, config_path=None, tablet=False, tablet_on_head=False):
+    def __init__(self, config_path=None, tablet=False):
         self.tablet = tablet
-        self.tablet_on_head = tablet_on_head
-
+        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/context.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/check_order.py

@@ -96,8 +96,9 @@ def execute(self, userdata):
             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

tasks/coffee_shop/src/coffee_shop/phases/phase_2/states/take_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", np.pi, degrees=False)
+                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"