Identify look at person bug and reformatting

skills/src/lasr_skills/look_at_person.py

@@ -20,7 +20,7 @@
 import actionlib
 from control_msgs.msg import PointHeadAction, PointHeadGoal
 from geometry_msgs.msg import Point
-
+from copy import copy
 PUBLIC_CONTAINER = False
 
 try:
@@ -47,7 +47,7 @@ def __init__(self, debug=True, filter=False):
                     "detections",
                     "deepface_detection",
                 ],
-                output_keys=["pointstamped"],
+                output_keys=["pointstamped", "bbox_eyes"],
             )
             self.DEBUG = debug
             self.img_pub = rospy.Publisher("/debug/image", Image, queue_size=1)
@@ -58,8 +58,9 @@ def __init__(self, debug=True, filter=False):
             self._filter = filter
 
         def execute(self, userdata):
-            rospy.sleep(1)
-            if len(userdata.bbox_eyes) < 1 and len(userdata.detections.detections) > 0:
+            rospy.sleep(0.1)
+            if len(userdata.bbox_eyes) == 0 and len(userdata.detections.detections) > 0:
+                userdata.pointstamped = PointStamped()
                 return "succeeded"
             elif (
                 len(userdata.bbox_eyes) < 1 and len(userdata.detections.detections) < 1
@@ -88,7 +89,9 @@ def execute(self, userdata):
                 else:
                     return "failed"
 
-            for det in userdata.bbox_eyes:
+            bbox_eyes = copy(userdata.bbox_eyes)
+
+            for det in bbox_eyes:
                 left_eye = det["left_eye"]
                 right_eye = det["right_eye"]
                 eye_point = (left_eye[0] + right_eye[0]) / 2, (
@@ -141,15 +144,15 @@ def execute(self, userdata):
                     look_at.point.y = 0.0
                     look_at.point.z = 0.0
 
-                goal = PointHeadGoal()
-                goal.pointing_frame = "head_2_link"
-                goal.pointing_axis = Point(1.0, 0.0, 0.0)
-                goal.max_velocity = 1.0
-                goal.target = look_at
-                rospy.loginfo(
-                    f"LOOKING AT POINT {look_at.point.x}, {look_at.point.y}, {look_at.point.z}"
-                )
-                self.look_at_pub.send_goal(goal)
+                # goal = PointHeadGoal()
+                # goal.pointing_frame = "head_2_link"
+                # goal.pointing_axis = Point(1.0, 0.0, 0.0)
+                # goal.max_velocity = 1.0
+                # goal.target = look_at
+                # rospy.loginfo(
+                #     f"LOOKING AT POINT {look_at.point.x}, {look_at.point.y}, {look_at.point.z}"
+                # )
+                # self.look_at_pub.send_goal(goal)
 
                 print(self.look_at_pub.get_state())
 
@@ -272,7 +275,7 @@ def __init__(self, filter=False):
                 "CHECK_EYES",
                 self.CheckEyes(self.DEBUG, filter=filter),
                 transitions={
-                    "succeeded": "LOOP",
+                    "succeeded": "LOOK_TO_POINT",
                     "failed": "failed",
                     "no_detection": "no_detection",
                 },

skills/src/lasr_skills/look_to_given_point.py

@@ -49,7 +49,7 @@ def execute(self, userdata):
         if finished_within_time:
             state = self.client.get_state()
             if state == GoalStatus.SUCCEEDED:
-                rospy.sleep(1)
+                rospy.sleep(0.1)
                 return "succeeded"
             else:
                 return "aborted"

tasks/receptionist/scripts/main.py

@@ -60,10 +60,10 @@
         },
     )
 
-    sis = smach_ros.IntrospectionServer("smach_server", receptionist, "/SM_ROOT")
-    sis.start()
+    # sis = smach_ros.IntrospectionServer("smach_server", receptionist, "/SM_ROOT")
+    # sis.start()
     outcome = receptionist.execute()
 
-    sis.stop()
+    # sis.stop()
     rospy.loginfo(f"Receptionist finished with outcome: {outcome}")
     rospy.spin()

tasks/receptionist/src/receptionist/states/introduce.py

@@ -10,32 +10,6 @@
 from lasr_skills import Say
 from typing import Dict, List, Any, Optional
 
-
-def find_most_confident_clothes(
-    relevant_guest_data: Dict[str, Any], clothes: List[List[Any]]
-) -> List[Any]:
-    """Find the clothes it's most confident of, after determining the clothes type
-    through confidence values.
-
-    Args:
-        relevant_guest_data (Dict[str, Any]): guest data dictionary.
-        clothes List[List[Any]]: List of the clothes type and their confidence
-
-    Returns:
-        List: Maximum confidence and the relevant clothes
-    """
-    max_clothes_type, max_confidence = max(clothes, key=lambda c: c[1])
-
-    if max_clothes_type == "dress":
-        max_clothes = relevant_guest_data["attributes"]["max_dress"]
-    elif max_clothes_type == "top":
-        max_clothes = relevant_guest_data["attributes"]["max_top"]
-    else:
-        max_clothes = relevant_guest_data["attributes"]["max_outwear"]
-
-    return [max_confidence, max_clothes]
-
-
 def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
     """Converts the guest data for a specified guest into a string that can be used
     for the robot to introduce the guest to the other guests/host.
@@ -162,9 +136,32 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
 
     return guest_str
 
+def find_most_confident_clothes(
+    relevant_guest_data: Dict[str, Any], clothes: List[List[Any]]
+) -> List[Any]:
+    """Find the clothes it's most confident of, after determining the clothes type
+    through confidence values.
+
+    Args:
+        relevant_guest_data (Dict[str, Any]): guest data dictionary.
+        clothes List[List[Any]]: List of the clothes type and their confidence
+
+    Returns:
+        List: Maximum confidence and the relevant clothes
+    """
+    max_clothes_type, max_confidence = max(clothes, key=lambda c: c[1])
+
+    if max_clothes_type == "dress":
+        max_clothes = relevant_guest_data["attributes"]["max_dress"]
+    elif max_clothes_type == "top":
+        max_clothes = relevant_guest_data["attributes"]["max_top"]
+    else:
+        max_clothes = relevant_guest_data["attributes"]["max_outwear"]
+
+    return [max_confidence, max_clothes]
 
 def isSingular(attribute: str):
-    """Checks is a word is singular or plural by checking the last letter
+    """Checks if a word is singular or plural by checking the last letter
 
     Args:
         attribute (str): The attribute to check for plurality