diff --git a/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py b/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py
index 2642267d8..2d98a2169 100644
--- a/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py
+++ b/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py
@@ -41,6 +41,7 @@ def run_clip(
txt = model.encode(labels)
img = model.encode(img)
with torch.no_grad():
+ torch
cos_scores = util.cos_sim(img, txt)
return cos_scores
diff --git a/common/vision/lasr_vision_deepface/src/lasr_vision_deepface/deepface.py b/common/vision/lasr_vision_deepface/src/lasr_vision_deepface/deepface.py
index a563aff7d..d96fe664e 100644
--- a/common/vision/lasr_vision_deepface/src/lasr_vision_deepface/deepface.py
+++ b/common/vision/lasr_vision_deepface/src/lasr_vision_deepface/deepface.py
@@ -62,7 +62,6 @@ def _extract_face(cv_im: Mat) -> Union[Mat, None]:
try:
faces = DeepFace.extract_faces(
cv_im,
- target_size=(224, 244),
detector_backend="mtcnn",
enforce_detection=True,
)
diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py
index 06893720d..c0f07947c 100644
--- a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py
+++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py
@@ -279,7 +279,6 @@ def describe(self) -> dict:
max_attribute = "sleeveless top"
result["attributes"][max_attribute] = True
-
if outwear:
max_prob = 0.0
max_attribute = "short sleeve outwear"
diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py
index 86ce90ccb..b9f7080c5 100755
--- a/skills/src/lasr_skills/__init__.py
+++ b/skills/src/lasr_skills/__init__.py
@@ -22,4 +22,6 @@
from .detect_gesture import DetectGesture
from .learn_face import LearnFace
from .look_at_person import LookAtPerson
+from .wait import Wait
+from .look_to_given_point import LookToGivenPoint
from .find_gesture_person import FindGesturePerson
diff --git a/skills/src/lasr_skills/check_known_people.py b/skills/src/lasr_skills/check_known_people.py
new file mode 100755
index 000000000..134e8497e
--- /dev/null
+++ b/skills/src/lasr_skills/check_known_people.py
@@ -0,0 +1,35 @@
+import smach
+import rospy
+import os
+import rospkg
+
+DATASET_ROOT = os.path.join(
+ rospkg.RosPack().get_path("lasr_vision_deepface"), "datasets"
+)
+
+
+class CheckKnownPeople(smach.State):
+ def __init__(self):
+ smach.State.__init__(
+ self,
+ outcomes=["succeeded", "failed"],
+ input_keys=["task_name"],
+ output_keys=["known_people"],
+ )
+
+ def execute(self, userdata):
+ try:
+ dataset_path = os.path.join(DATASET_ROOT, userdata.task_name)
+ print(dataset_path)
+ known_people_names = [
+ f
+ for f in os.listdir(dataset_path)
+ if os.path.isdir(os.path.join(dataset_path, f))
+ ]
+ rospy.set_param("/known_people", known_people_names)
+ userdata.known_people = known_people_names
+ return "succeeded"
+ except Exception as e:
+ print(f"Face detection using dataset {dataset_path}")
+ rospy.logerr(f"Failed to get known people: {str(e)}")
+ return "failed"
diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py
index 1f1b6b93f..4f452b1aa 100755
--- a/skills/src/lasr_skills/describe_people.py
+++ b/skills/src/lasr_skills/describe_people.py
@@ -5,6 +5,7 @@
import smach
import cv2_img
import numpy as np
+from lasr_skills import Say
from lasr_vision_msgs.msg import BodyPixMaskRequest
from lasr_vision_msgs.srv import (
YoloDetection,
@@ -37,11 +38,39 @@ def __init__(self):
)
smach.StateMachine.add(
"GET_IMAGE",
- GetCroppedImage(
- object_name="person", crop_method=crop_method, rgb_topic=rgb_topic
+ GetCroppedImage(object_name="person", crop_method="closest"),
+ transitions={
+ "succeeded": "CONVERT_IMAGE",
+ "failed": "SAY_GET_IMAGE_AGAIN",
+ },
+ )
+ smach.StateMachine.add(
+ "SAY_GET_IMAGE_AGAIN",
+ Say(
+ text="Make sure you're looking into my eyes, I can't seem to see you."
),
- transitions={"succeeded": "CONVERT_IMAGE"},
+ transitions={
+ "succeeded": "GET_IMAGE_AGAIN",
+ "preempted": "GET_IMAGE_AGAIN",
+ "aborted": "GET_IMAGE_AGAIN",
+ },
+ )
+ smach.StateMachine.add(
+ "GET_IMAGE_AGAIN",
+ GetCroppedImage(object_name="person", crop_method="closest"),
+ transitions={"succeeded": "CONVERT_IMAGE", "failed": "SAY_CONTINUE"},
)
+
+ smach.StateMachine.add(
+ "SAY_CONTINUE",
+ Say(text="I can't see anyone, I will continue"),
+ transitions={
+ "succeeded": "failed",
+ "preempted": "failed",
+ "aborted": "failed",
+ },
+ )
+
smach.StateMachine.add(
"CONVERT_IMAGE", ImageMsgToCv2(), transitions={"succeeded": "SEGMENT"}
)
diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py
index d0ca7c280..0fac82a2a 100644
--- a/skills/src/lasr_skills/detect_3d_in_area.py
+++ b/skills/src/lasr_skills/detect_3d_in_area.py
@@ -29,6 +29,7 @@ def __init__(
def execute(self, userdata):
detected_objects = userdata["detections_3d"].detected_objects
# publish polygon for debugging
+
polygon_msg = Polygon()
polygon_msg.points = [
Point32(x=point[0], y=point[1], z=0.0)
@@ -50,7 +51,7 @@ def execute(self, userdata):
def __init__(
self,
- area_polygon: Polygon,
+ area_polygon: ShapelyPolygon,
depth_topic: str = "/xtion/depth_registered/points",
model: str = "yolov8x-seg.pt",
filter: Union[List[str], None] = None,
diff --git a/skills/src/lasr_skills/look_at_person.py b/skills/src/lasr_skills/look_at_person.py
index 11275023e..e41bcef48 100755
--- a/skills/src/lasr_skills/look_at_person.py
+++ b/skills/src/lasr_skills/look_at_person.py
@@ -15,7 +15,6 @@
import ros_numpy as rnp
import rosservice
from smach import CBState
-from std_msgs.msg import String
import actionlib
from control_msgs.msg import PointHeadAction, PointHeadGoal
from geometry_msgs.msg import Point
@@ -64,6 +63,18 @@ def execute(self, userdata):
len(userdata.bbox_eyes) < 1 and len(userdata.detections.detections) < 1
):
return "no_detection"
+ print("THE DEEPFACE")
+ print(userdata.deepface_detection)
+
+ if self._filter:
+ if userdata.deepface_detection:
+ deepface = userdata.deepface_detection[0]
+ for bbox in userdata.bbox_eyes:
+ if bbox["bbox"] == deepface:
+ userdata.bbox_eyes = [bbox]
+ break
+ else:
+ return "failed"
if self._filter:
if userdata.deepface_detection:
@@ -92,7 +103,15 @@ def execute(self, userdata):
pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(
userdata.pcl_msg, remove_nans=False
)
- eye_point_pcl = pcl_xyz[int(eye_point[1]), int(eye_point[0])]
+ try:
+ eye_point_pcl = pcl_xyz[int(eye_point[1]), int(eye_point[0])]
+ except IndexError:
+ eye_point_pcl = pcl_xyz[
+ int(eye_point[1]) - 1, int(eye_point[0]) - 1
+ ]
+ except Exception as e:
+ rospy.logerr(f"Unexpected Exception: {e}")
+ return "failed"
if any([True for i in eye_point_pcl if i != i]):
eye_point_pcl = pcl_xyz[int(right_eye[1]), int(right_eye[0])]
@@ -130,6 +149,8 @@ def execute(self, userdata):
)
self.look_at_pub.send_goal(goal)
+ print(self.look_at_pub.get_state())
+
return "succeeded"
return "failed"
@@ -270,7 +291,7 @@ def __init__(self, filter=False):
transitions={
"succeeded": "LOOP",
"aborted": "failed",
- "preempted": "failed",
+ "timed_out": "LOOP",
},
remapping={"pointstamped": "pointstamped"},
)
diff --git a/skills/src/lasr_skills/look_to_given_point.py b/skills/src/lasr_skills/look_to_given_point.py
new file mode 100755
index 000000000..819906744
--- /dev/null
+++ b/skills/src/lasr_skills/look_to_given_point.py
@@ -0,0 +1,58 @@
+"""Look to a specific point passed in as a parameter
+Similar to look_to_point but the input key is replaced with a passed argument for the point to look at
+"""
+
+import smach_ros
+import smach
+import actionlib
+import rospy
+from control_msgs.msg import PointHeadGoal, PointHeadAction
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import Header
+from actionlib_msgs.msg import GoalStatus
+from typing import List
+
+
+class LookToGivenPoint(smach.State):
+ def __init__(self, look_position: List[float]):
+ """
+ Args:
+ look_position (List[float]): Position to look to
+ """
+ smach.State.__init__(
+ self,
+ outcomes=["succeeded", "aborted", "timed_out"],
+ )
+ self.goal_pointstamped = PointStamped(
+ point=Point(x=look_position[0], y=look_position[1], z=1.0)
+ )
+ self.client = actionlib.SimpleActionClient(
+ "/head_controller/point_head_action", PointHeadAction
+ )
+ self.client.wait_for_server()
+
+ def execute(self, userdata):
+ goal = PointHeadGoal(
+ pointing_frame="head_2_link",
+ pointing_axis=Point(1.0, 0.0, 0.0),
+ max_velocity=1.0,
+ target=PointStamped(
+ header=Header(frame_id="map"),
+ point=self.goal_pointstamped.point,
+ ),
+ )
+ self.client.send_goal(goal)
+
+ # Wait for the result with a timeout of 7 seconds
+ finished_within_time = self.client.wait_for_result(rospy.Duration(7.0))
+
+ if finished_within_time:
+ state = self.client.get_state()
+ if state == GoalStatus.SUCCEEDED:
+ rospy.sleep(1)
+ return "succeeded"
+ else:
+ return "aborted"
+ else:
+ self.client.cancel_goal()
+ return "timed_out"
diff --git a/skills/src/lasr_skills/look_to_point.py b/skills/src/lasr_skills/look_to_point.py
index 3d5e41816..74cc28e22 100755
--- a/skills/src/lasr_skills/look_to_point.py
+++ b/skills/src/lasr_skills/look_to_point.py
@@ -1,28 +1,49 @@
+import smach_ros
import smach
-import rospy
import actionlib
-from control_msgs.msg import PointHeadAction, PointHeadGoal
-from geometry_msgs.msg import Point
+import rospy
+from control_msgs.msg import PointHeadGoal, PointHeadAction
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import Header
+from actionlib_msgs.msg import GoalStatus
class LookToPoint(smach.State):
def __init__(self):
smach.State.__init__(
self,
- outcomes=["succeeded", "preempted", "aborted"],
+ outcomes=["succeeded", "aborted", "timed_out"],
input_keys=["pointstamped"],
)
- self.look_at_pub = actionlib.SimpleActionClient(
+ self.client = actionlib.SimpleActionClient(
"/head_controller/point_head_action", PointHeadAction
)
+ self.client.wait_for_server()
def execute(self, userdata):
- 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 = userdata.pointstamped
- self.look_at_pub.send_goal(goal)
- self.look_at_pub.wait_for_result()
- rospy.sleep(3.0)
- return "succeeded"
+ # Define the goal
+ goal = PointHeadGoal(
+ pointing_frame="head_2_link",
+ pointing_axis=Point(1.0, 0.0, 0.0),
+ max_velocity=1.0,
+ target=PointStamped(
+ header=Header(frame_id="map"),
+ point=userdata.pointstamped.point,
+ ),
+ )
+
+ # Send the goal
+ self.client.send_goal(goal)
+
+ # Wait for the result with a timeout of 7 seconds
+ finished_within_time = self.client.wait_for_result(rospy.Duration(7.0))
+
+ if finished_within_time:
+ state = self.client.get_state()
+ if state == GoalStatus.SUCCEEDED:
+ return "succeeded"
+ else:
+ return "aborted"
+ else:
+ self.client.cancel_goal()
+ return "timed_out"
diff --git a/skills/src/lasr_skills/wait.py b/skills/src/lasr_skills/wait.py
new file mode 100644
index 000000000..a08fc866e
--- /dev/null
+++ b/skills/src/lasr_skills/wait.py
@@ -0,0 +1,23 @@
+"""Generic wait state for waiting a desired number of seconds"""
+
+import smach
+import rospy
+
+
+class Wait(smach.State):
+ def __init__(self, wait_time: int):
+ """
+ Args:
+ wait_time (int): Number of seconds to wait for and remain idle
+ """
+ smach.State.__init__(self, outcomes=["succeeded", "failed"])
+ self._wait_time = wait_time
+
+ def execute(self, userdata) -> str:
+ try:
+ print(f"Waiting for {self._wait_time} seconds.")
+ rospy.sleep(self._wait_time)
+ return "succeeded"
+ except:
+ print("Waiting failed")
+ return "failed"
diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml
index a3adc5c93..2bd6dceae 100644
--- a/tasks/receptionist/config/lab.yaml
+++ b/tasks/receptionist/config/lab.yaml
@@ -18,17 +18,32 @@ priors:
- orange juice
- red wine
- tropical juice
+# wait_pose:
+# position:
+# x: 2.4307581363168773
+# y: -1.661594410669659
+# z: 0.0
+# orientation:
+# x: 0.0
+# y: 0.0
+# z: 0.012769969339563213
+# w: 0.9999184606171978
+
wait_pose:
position:
- x: 2.4307581363168773
- y: -1.661594410669659
+ x: 4.637585370589175
+ y: 6.715591164127531
z: 0.0
orientation:
x: 0.0
y: 0.0
- z: 0.012769969339563213
- w: 0.9999184606171978
-wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
+ z: 0.478893309417269
+ w: 0.4
+#0.478893309417269
+#0.8778731105321406
+
+# wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
+wait_area: [[3.60, 8.56], [4.69, 8.90], [4.98, 7.59], [3.95, 7.34]]
seat_pose:
position:
x: 1.1034954065916212
diff --git a/tasks/receptionist/doc/PREREQUISITES.md b/tasks/receptionist/doc/PREREQUISITES.md
index a7365b161..007a9d29c 100644
--- a/tasks/receptionist/doc/PREREQUISITES.md
+++ b/tasks/receptionist/doc/PREREQUISITES.md
@@ -1 +1,8 @@
If you would like to view the documentation in the browser, ensure you have at [Node.js v16.x](https://nodejs.org/en) installed.
+
+
+Please make sure the following models are installed in order to run this code:
+
+- lasr_vision_yolov8 requires yolov8n-seg.pt in lasr_vision_yolov8/models/
+- lasr_vision_feature_extraction requires model.pth in lasr_vision_feature_extraction/models/
+- lasr_vision_clip requires a model from hugging face, this can be downloaded by running 'rosrun lasr_vision_clip vqa'
\ No newline at end of file
diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch
index 145b552f7..530dda7d7 100644
--- a/tasks/receptionist/launch/setup.launch
+++ b/tasks/receptionist/launch/setup.launch
@@ -15,7 +15,10 @@
-
+
+
+
+
diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py
index c6d6d2652..4c4cbeeea 100755
--- a/tasks/receptionist/scripts/main.py
+++ b/tasks/receptionist/scripts/main.py
@@ -1,6 +1,8 @@
#!/usr/bin/env python3
import rospy
from receptionist.state_machine import Receptionist
+import smach
+import smach_ros
from geometry_msgs.msg import Pose, Point, Quaternion
from shapely.geometry import Polygon
@@ -8,21 +10,27 @@
if __name__ == "__main__":
rospy.init_node("receptionist_robocup")
wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+ # wait_pose_param = rospy.get_param("/wait_pose/")
+
wait_pose = Pose(
position=Point(**wait_pose_param["position"]),
orientation=Quaternion(**wait_pose_param["orientation"]),
)
+ # wait_area_param = rospy.get_param("/wait_area")
wait_area_param = rospy.get_param("/receptionist/wait_area")
wait_area = Polygon(wait_area_param)
+ # seat_pose_param = rospy.get_param("/seat_pose")
seat_pose_param = rospy.get_param("/receptionist/seat_pose")
seat_pose = Pose(
position=Point(**seat_pose_param["position"]),
orientation=Quaternion(**seat_pose_param["orientation"]),
)
+ # seat_area_param = rospy.get_param("/seat_area")
seat_area_param = rospy.get_param("/receptionist/seat_area")
+
seat_area = Polygon(seat_area_param)
receptionist = Receptionist(
@@ -31,12 +39,17 @@
seat_pose,
seat_area,
{
- "name": "nicole",
- "drink": "beer",
+ "name": "charlie",
+ "drink": "wine",
"dataset": "receptionist",
"confidence": 0.5,
},
)
+
+ sis = smach_ros.IntrospectionServer("smach_server", receptionist, "/SM_ROOT")
+ sis.start()
outcome = receptionist.execute()
+
+ sis.stop()
rospy.loginfo(f"Receptionist finished with outcome: {outcome}")
rospy.spin()
diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py
index 2dacd7461..d35a76c8b 100755
--- a/tasks/receptionist/src/receptionist/state_machine.py
+++ b/tasks/receptionist/src/receptionist/state_machine.py
@@ -2,13 +2,21 @@
from geometry_msgs.msg import Pose
from shapely.geometry import Polygon
-from lasr_skills import GoToLocation, WaitForPersonInArea, Say, AskAndListen
+from lasr_skills import (
+ GoToLocation,
+ WaitForPersonInArea,
+ Say,
+ AskAndListen,
+ LookToGivenPoint,
+)
from receptionist.states import (
ParseNameAndDrink,
GetGuestAttributes,
Introduce,
SeatGuest,
FindAndLookAt,
+ ReceptionistLearnFaces,
+ ParseTranscribedInfo,
)
@@ -29,13 +37,30 @@ def __init__(
"guest1": {"name": ""},
"guest2": {"name": ""},
}
+ self.userdata.guest_name = "zoe"
+ self.userdata.dataset = "receptionist"
+ self.userdata.confidence = 0.2
+
+ smach.StateMachine.add(
+ "SAY_START",
+ Say(text="Start of receptionist task. Going to waiting area."),
+ transitions={
+ "succeeded": "GO_TO_WAIT_LOCATION_GUEST_1",
+ "aborted": "GO_TO_WAIT_LOCATION_GUEST_1",
+ "preempted": "GO_TO_WAIT_LOCATION_GUEST_1",
+ },
+ )
+
+ """
+ First guest
+ """
smach.StateMachine.add(
"GO_TO_WAIT_LOCATION_GUEST_1",
GoToLocation(wait_pose),
transitions={
"succeeded": "SAY_WAITING_GUEST_1",
- "failed": "failed",
+ "failed": "SAY_WAITING_GUEST_1",
},
)
@@ -44,8 +69,8 @@ def __init__(
Say(text="I am waiting for a guest."),
transitions={
"succeeded": "WAIT_FOR_PERSON_GUEST_1",
- "aborted": "failed",
- "preempted": "failed",
+ "aborted": "WAIT_FOR_PERSON_GUEST_1",
+ "preempted": "WAIT_FOR_PERSON_GUEST_1",
},
)
@@ -54,16 +79,20 @@ def __init__(
WaitForPersonInArea(wait_area),
transitions={
"succeeded": "GET_NAME_AND_DRINK_GUEST_1",
- "failed": "failed",
+ "failed": "GET_NAME_AND_DRINK_GUEST_1",
},
)
+ """
+ Asking first Guest for Drink and Name
+ """
+
smach.StateMachine.add(
"GET_NAME_AND_DRINK_GUEST_1",
AskAndListen("What is your name and favourite drink?"),
transitions={
"succeeded": "PARSE_NAME_AND_DRINK_GUEST_1",
- "failed": "failed",
+ "failed": "PARSE_NAME_AND_DRINK_GUEST_1",
},
)
@@ -71,18 +100,139 @@ def __init__(
"PARSE_NAME_AND_DRINK_GUEST_1",
ParseNameAndDrink("guest1"),
transitions={
- "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_1",
- "failed": "failed",
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "failed": "REPEAT_GET_NAME_AND_DRINK_GUEST_1",
+ "failed_name": "REPEAT_GET_NAME_GUEST_1",
+ "failed_drink": "REPEAT_GET_DRINK_GUEST_1",
},
remapping={"guest_transcription": "transcribed_speech"},
)
+ smach.StateMachine.add(
+ "REPEAT_GET_NAME_AND_DRINK_GUEST_1",
+ AskAndListen(
+ "Sorry, I didn't get that. What is your name and favourite drink?"
+ ),
+ transitions={
+ "succeeded": "REPEAT_PARSE_NAME_AND_DRINK_GUEST_1",
+ "failed": "SAY_CONTINUE",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_NAME_AND_DRINK_GUEST_1",
+ ParseNameAndDrink("guest1"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "failed": "SAY_CONTINUE",
+ "failed_name": "SAY_CONTINUE",
+ "failed_drink": "SAY_CONTINUE",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ """
+ Recovery for only name not recognised
+ """
+
+ smach.StateMachine.add(
+ "REPEAT_GET_NAME_GUEST_1",
+ AskAndListen("Sorry, I didn't get your name. What is your name?"),
+ transitions={
+ "succeeded": "REPEAT_PARSE_NAME_GUEST_1",
+ "failed": "SAY_CONTINUE",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_NAME_GUEST_1",
+ ParseTranscribedInfo("guest1", "name"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "failed": "SAY_CONTINUE",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ """
+ Recovery for only drink not recognised
+ """
+
+ smach.StateMachine.add(
+ "REPEAT_GET_DRINK_GUEST_1",
+ AskAndListen(
+ "Sorry, I didn't get your favourite drink. What is your favourite drink?"
+ ),
+ transitions={
+ "succeeded": "REPEAT_PARSE_DRINK_GUEST_1",
+ "failed": "SAY_CONTINUE",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_DRINK_GUEST_1",
+ ParseTranscribedInfo("guest1", "drink"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "failed": "SAY_CONTINUE",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ """
+ Recovery if nothing was recognised (twice)
+ """
+ smach.StateMachine.add(
+ "SAY_CONTINUE",
+ Say(text="Sorry, I didn't get that. I will carry on."),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "aborted": "SAY_GET_GUEST_ATTRIBUTE_1",
+ "preempted": "SAY_GET_GUEST_ATTRIBUTE_1",
+ },
+ )
+
+ """
+ GET GUEST ATTRIBUTES
+ """
+
+ smach.StateMachine.add(
+ "SAY_GET_GUEST_ATTRIBUTE_1",
+ Say(
+ text="Please look into my eyes, I am about to detect your attributes."
+ ),
+ transitions={
+ "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_1",
+ "aborted": "GET_GUEST_ATTRIBUTES_GUEST_1",
+ "preempted": "GET_GUEST_ATTRIBUTES_GUEST_1",
+ },
+ )
+
smach.StateMachine.add(
"GET_GUEST_ATTRIBUTES_GUEST_1",
GetGuestAttributes("guest1"),
+ transitions={
+ "succeeded": "SAY_LEARN_FACES",
+ "failed": "SAY_LEARN_FACES",
+ },
+ )
+
+ smach.StateMachine.add(
+ "SAY_LEARN_FACES",
+ Say(text="Continue to look into my eyes, I'm about to learn your face"),
+ transitions={
+ "succeeded": "LEARN_FACES",
+ "preempted": "LEARN_FACES",
+ "aborted": "LEARN_FACES",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LEARN_FACES",
+ ReceptionistLearnFaces("guest1"),
transitions={
"succeeded": "SAY_FOLLOW_GUEST_1",
- "failed": "failed",
+ "failed": "SAY_FOLLOW_GUEST_1",
},
)
@@ -101,7 +251,7 @@ def __init__(
GoToLocation(seat_pose),
transitions={
"succeeded": "SAY_WAIT_GUEST_1",
- "failed": "failed",
+ "failed": "SAY_WAIT_GUEST_1",
},
)
@@ -114,28 +264,63 @@ def __init__(
"aborted": "failed",
},
)
+
smach.StateMachine.add(
"FIND_AND_LOOK_AT",
FindAndLookAt(
"host",
[
[0.0, 0.0],
- [-1.0, 0.0],
- [1.0, 0.0],
+ [-0.8, 0.0],
+ [0.8, 0.0],
],
),
transitions={
"succeeded": "INTRODUCE_GUEST_1_TO_HOST",
- "failed": "failed",
+ "failed": "SAY_NO_HOST_1",
+ },
+ )
+
+ smach.StateMachine.add(
+ "SAY_NO_HOST_1",
+ Say(text="Wow, I can't find the host, I'm sure they're here"),
+ transitions={
+ "succeeded": "LOOK_AT_WAITING_GUEST_1_1",
+ "preempted": "failed",
+ "aborted": "failed",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_1_1",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
+ transitions={
+ "succeeded": "INTRODUCE_GUEST_1_TO_HOST",
+ "timed_out": "INTRODUCE_GUEST_1_TO_HOST",
+ "aborted": "INTRODUCE_GUEST_1_TO_HOST",
},
)
smach.StateMachine.add(
"INTRODUCE_GUEST_1_TO_HOST",
Introduce(guest_to_introduce="guest1", guest_to_introduce_to="host"),
+ transitions={
+ "succeeded": "LOOK_AT_WAITING_GUEST_1_2",
+ "failed": "LOOK_AT_WAITING_GUEST_1_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_1_2",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
transitions={
"succeeded": "INTRODUCE_HOST_TO_GUEST_1",
- "failed": "failed",
+ "timed_out": "INTRODUCE_HOST_TO_GUEST_1",
+ "aborted": "INTRODUCE_HOST_TO_GUEST_1",
},
)
@@ -144,7 +329,7 @@ def __init__(
Introduce(guest_to_introduce="host", guest_to_introduce_to="guest1"),
transitions={
"succeeded": "SEAT_GUEST_1",
- "failed": "failed",
+ "failed": "SEAT_GUEST_1",
},
)
@@ -152,8 +337,8 @@ def __init__(
"SEAT_GUEST_1",
SeatGuest(seat_area),
transitions={
- "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2",
- "failed": "failed",
+ "succeeded": "SAY_RETURN_WAITING_AREA",
+ "failed": "SAY_RETURN_WAITING_AREA",
},
)
@@ -161,12 +346,22 @@ def __init__(
Guest 2
"""
+ smach.StateMachine.add(
+ "SAY_RETURN_WAITING_AREA",
+ Say(text="Let me go back to the waiting area."),
+ transitions={
+ "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2",
+ "aborted": "GO_TO_WAIT_LOCATION_GUEST_2",
+ "preempted": "GO_TO_WAIT_LOCATION_GUEST_2",
+ },
+ )
+
smach.StateMachine.add(
"GO_TO_WAIT_LOCATION_GUEST_2",
GoToLocation(wait_pose),
transitions={
"succeeded": "SAY_WAITING_GUEST_2",
- "failed": "failed",
+ "failed": "SAY_WAITING_GUEST_2",
},
)
@@ -175,8 +370,8 @@ def __init__(
Say(text="I am waiting for a guest."),
transitions={
"succeeded": "WAIT_FOR_PERSON_GUEST_2",
- "aborted": "failed",
- "preempted": "failed",
+ "aborted": "WAIT_FOR_PERSON_GUEST_2",
+ "preempted": "WAIT_FOR_PERSON_GUEST_2",
},
)
@@ -185,16 +380,20 @@ def __init__(
WaitForPersonInArea(wait_area),
transitions={
"succeeded": "GET_NAME_AND_DRINK_GUEST_2",
- "failed": "failed",
+ "failed": "GET_NAME_AND_DRINK_GUEST_2",
},
)
+ """
+ Asking second guest for drink and name
+ """
+
smach.StateMachine.add(
"GET_NAME_AND_DRINK_GUEST_2",
AskAndListen("What is your name and favourite drink?"),
transitions={
"succeeded": "PARSE_NAME_AND_DRINK_GUEST_2",
- "failed": "failed",
+ "failed": "PARSE_NAME_AND_DRINK_GUEST_2",
},
)
@@ -202,18 +401,132 @@ def __init__(
"PARSE_NAME_AND_DRINK_GUEST_2",
ParseNameAndDrink("guest2"),
transitions={
- "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_2",
- "failed": "failed",
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "failed": "REPEAT_GET_NAME_AND_DRINK_GUEST_2",
+ "failed_name": "REPEAT_GET_NAME_GUEST_2",
+ "failed_drink": "REPEAT_GET_DRINK_GUEST_2",
},
remapping={"guest_transcription": "transcribed_speech"},
)
+ smach.StateMachine.add(
+ "REPEAT_GET_NAME_AND_DRINK_GUEST_2",
+ AskAndListen(
+ "Sorry, I didn't get that. What is your name and favourite drink?"
+ ),
+ transitions={
+ "succeeded": "REPEAT_PARSE_NAME_AND_DRINK_GUEST_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_NAME_AND_DRINK_GUEST_2",
+ ParseNameAndDrink("guest2"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ "failed_name": "SAY_CONTINUE_GUEST_2",
+ "failed_drink": "SAY_CONTINUE_GUEST_2",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ """
+ Recovery for only name not recognised
+ """
+
+ smach.StateMachine.add(
+ "REPEAT_GET_NAME_GUEST_2",
+ AskAndListen("Sorry, I didn't get your name. What is your name?"),
+ transitions={
+ "succeeded": "REPEAT_PARSE_NAME_GUEST_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_NAME_GUEST_2",
+ ParseTranscribedInfo("guest2", "name"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ """
+ Recovery for only drink not recognised
+ """
+
+ smach.StateMachine.add(
+ "REPEAT_GET_DRINK_GUEST_2",
+ AskAndListen(
+ "Sorry, I didn't get your favourite drink. What is your favourite drink?"
+ ),
+ transitions={
+ "succeeded": "REPEAT_PARSE_DRINK_GUEST_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "REPEAT_PARSE_DRINK_GUEST_2",
+ ParseTranscribedInfo("guest2", "drink"),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "failed": "SAY_CONTINUE_GUEST_2",
+ },
+ remapping={"guest_transcription": "transcribed_speech"},
+ )
+
+ smach.StateMachine.add(
+ "SAY_CONTINUE_GUEST_2",
+ Say(text="Sorry, I didn't get that. I will carry on."),
+ transitions={
+ "succeeded": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "aborted": "SAY_GET_GUEST_ATTRIBUTE_2",
+ "preempted": "SAY_GET_GUEST_ATTRIBUTE_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "SAY_GET_GUEST_ATTRIBUTE_2",
+ Say(
+ text="Please look into my eyes, I am about to detect your attributes."
+ ),
+ transitions={
+ "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_2",
+ "aborted": "GET_GUEST_ATTRIBUTES_GUEST_2",
+ "preempted": "GET_GUEST_ATTRIBUTES_GUEST_2",
+ },
+ )
+
smach.StateMachine.add(
"GET_GUEST_ATTRIBUTES_GUEST_2",
GetGuestAttributes("guest2"),
+ transitions={
+ "succeeded": "SAY_LEARN_FACES_GUEST_2",
+ "failed": "SAY_LEARN_FACES_GUEST_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "SAY_LEARN_FACES_GUEST_2",
+ Say(text="Continue looking into my eyes, I'm about to learn your face"),
+ transitions={
+ "succeeded": "LEARN_FACES_GUEST_2",
+ "preempted": "LEARN_FACES_GUEST_2",
+ "aborted": "LEARN_FACES_GUEST_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LEARN_FACES_GUEST_2",
+ ReceptionistLearnFaces("guest2"),
transitions={
"succeeded": "SAY_FOLLOW_GUEST_2",
- "failed": "failed",
+ "failed": "SAY_FOLLOW_GUEST_2",
},
)
@@ -232,7 +545,7 @@ def __init__(
GoToLocation(seat_pose),
transitions={
"succeeded": "SAY_WAIT_GUEST_2",
- "failed": "failed",
+ "failed": "SAY_WAIT_GUEST_2",
},
)
@@ -240,57 +553,112 @@ def __init__(
"SAY_WAIT_GUEST_2",
Say(text="Please wait here on my left"),
transitions={
- "succeeded": "INTRODUCE_GUEST_2_TO_EVERYONE",
+ "succeeded": "FIND_AND_LOOK_AT_HOST_2",
"preempted": "failed",
"aborted": "failed",
},
)
+ # INTRODUCE GUEST 2 TO HOST
+
smach.StateMachine.add(
- "INTRODUCE_GUEST_2_TO_EVERYONE",
- Introduce(guest_to_introduce="guest2", everyone=True),
- transitions={
- "succeeded": "FIND_AND_LOOK_AT_2",
- "failed": "failed",
- },
- )
- smach.StateMachine.add(
- "FIND_AND_LOOK_AT_2",
+ "FIND_AND_LOOK_AT_HOST_2",
FindAndLookAt(
"host",
[
[0.0, 0.0],
- [-1.0, 0.0],
- [1.0, 0.0],
+ [-0.8, 0.0],
+ [0.8, 0.0],
],
),
transitions={
- "succeeded": "INTRODUCE_HOST_TO_GUEST_2",
- "failed": "failed",
+ "succeeded": "INTRODUCE_GUEST_2_TO_HOST",
+ "failed": "LOOK_AT_WAITING_GUEST_2_1",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_2_1",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
+ transitions={
+ "succeeded": "INTRODUCE_GUEST_2_TO_HOST",
+ "timed_out": "INTRODUCE_GUEST_2_TO_HOST",
+ "aborted": "INTRODUCE_GUEST_2_TO_HOST",
},
)
+ # Check if host is sat where they are sat
+ # Look at the host
+
smach.StateMachine.add(
- "INTRODUCE_HOST_TO_GUEST_2",
- Introduce(guest_to_introduce="host", guest_to_introduce_to="guest2"),
+ "INTRODUCE_GUEST_2_TO_HOST",
+ Introduce(guest_to_introduce="guest2", guest_to_introduce_to="host"),
+ transitions={
+ "succeeded": "LOOK_AT_WAITING_GUEST_2_2",
+ "failed": "LOOK_AT_WAITING_GUEST_2_2",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_2_2",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
transitions={
- "succeeded": "FIND_AND_LOOK_AT_3",
- "failed": "failed",
+ "succeeded": "FIND_AND_LOOK_AT_GUEST_1",
+ "timed_out": "FIND_AND_LOOK_AT_GUEST_1",
+ "aborted": "FIND_AND_LOOK_AT_GUEST_1",
},
)
+
smach.StateMachine.add(
- "FIND_AND_LOOK_AT_3",
+ "FIND_AND_LOOK_AT_GUEST_1",
FindAndLookAt(
"guest1",
[
[0.0, 0.0],
- [-1.0, 0.0],
- [1.0, 0.0],
+ [-0.8, 0.0],
+ [0.8, 0.0],
],
),
+ transitions={
+ "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1",
+ "failed": "LOOK_AT_WAITING_GUEST_2_3",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_2_3",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
+ transitions={
+ "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1",
+ "timed_out": "INTRODUCE_GUEST_2_TO_GUEST_1",
+ "aborted": "INTRODUCE_GUEST_2_TO_GUEST_1",
+ },
+ )
+
+ smach.StateMachine.add(
+ "INTRODUCE_GUEST_2_TO_GUEST_1",
+ Introduce(guest_to_introduce="guest2", guest_to_introduce_to="guest1"),
+ transitions={
+ "succeeded": "LOOK_AT_WAITING_GUEST_2_4",
+ "failed": "LOOK_AT_WAITING_GUEST_2_4",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_AT_WAITING_GUEST_2_4",
+ LookToGivenPoint(
+ [-1.5, 0.0],
+ ),
transitions={
"succeeded": "INTRODUCE_GUEST_1_TO_GUEST_2",
- "failed": "failed",
+ "timed_out": "INTRODUCE_GUEST_1_TO_GUEST_2",
+ "aborted": "INTRODUCE_GUEST_1_TO_GUEST_2",
},
)
@@ -299,25 +667,40 @@ def __init__(
Introduce(guest_to_introduce="guest1", guest_to_introduce_to="guest2"),
transitions={
"succeeded": "SEAT_GUEST_2",
- "failed": "failed",
+ "failed": "SEAT_GUEST_2",
},
)
smach.StateMachine.add(
"SEAT_GUEST_2",
SeatGuest(seat_area),
- transitions={"succeeded": "GO_TO_FINISH_LOCATION", "failed": "failed"},
+ transitions={
+ "succeeded": "SAY_GOODBYE",
+ "failed": "SAY_GOODBYE",
+ },
)
"""
Finish
"""
+ smach.StateMachine.add(
+ "SAY_GOODBYE",
+ Say(
+ text="Goodbye fellow humans, I shall be going back where I came from"
+ ),
+ transitions={
+ "succeeded": "GO_TO_FINISH_LOCATION",
+ "aborted": "failed",
+ "preempted": "GO_TO_FINISH_LOCATION",
+ },
+ )
+
smach.StateMachine.add(
"GO_TO_FINISH_LOCATION",
GoToLocation(wait_pose),
transitions={
"succeeded": "SAY_FINISHED",
- "failed": "failed",
+ "failed": "SAY_FINISHED",
},
)
smach.StateMachine.add(
@@ -326,6 +709,6 @@ def __init__(
transitions={
"succeeded": "succeeded",
"aborted": "failed",
- "preempted": "failed",
+ "preempted": "succeeded",
},
)
diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py
index 41c25b53d..d47897a9e 100644
--- a/tasks/receptionist/src/receptionist/states/__init__.py
+++ b/tasks/receptionist/src/receptionist/states/__init__.py
@@ -3,3 +3,6 @@
from .introduce import Introduce
from .seat_guest import SeatGuest
from .find_and_look_at import FindAndLookAt
+from .receptionist_learn_face import ReceptionistLearnFaces
+from .detect_faces import DetectFaces
+from .get_name_or_drink import ParseTranscribedInfo
diff --git a/tasks/receptionist/src/receptionist/states/detect_faces.py b/tasks/receptionist/src/receptionist/states/detect_faces.py
new file mode 100644
index 000000000..d6b84e435
--- /dev/null
+++ b/tasks/receptionist/src/receptionist/states/detect_faces.py
@@ -0,0 +1,106 @@
+"""This is no longer used"""
+
+import sys
+import smach
+import rospy
+
+from sensor_msgs.msg import Image
+
+from lasr_vision_msgs.srv import Recognise, RecogniseRequest
+
+from lasr_skills import Say
+
+
+class DetectFaces(smach.State):
+ def __init__(self):
+ smach.State.__init__(self, outcomes=["recognised", "unrecognised"])
+ # self.dataset = sys.argv[2]
+ self.people_in_frame = (
+ {}
+ ) # dict of people in frame and the time they were detected
+ self.listen_topic = "/xtion/rgb/image_raw"
+ self.detected_people = False
+
+ def execute(self, userdata):
+ print("I'm about to guess who you are")
+ self.listener()
+ if self.subscriber:
+ self.subscriber.unregister()
+ if not self.detected_people:
+ self.say_nobody_detected()
+ rospy.sleep(2)
+ return "unrecognised"
+ else:
+ self.greet()
+ rospy.sleep(3)
+ return "recognised"
+
+ def listener(self):
+ # rospy.init_node("image_listener", anonymous=True)
+ rospy.wait_for_service("/recognise")
+ self.subscriber = rospy.Subscriber(
+ self.listen_topic, Image, self.image_callback, queue_size=1
+ )
+ rospy.sleep(7)
+
+ def detect(self, image):
+ rospy.loginfo("Received image message")
+ try:
+ detect_service = rospy.ServiceProxy("/recognise", Recognise)
+ req = RecogniseRequest()
+ req.image_raw = image
+ req.dataset = "receptionist"
+ req.confidence = 0.4
+ resp = detect_service(req)
+ for detection in resp.detections:
+ self.people_in_frame[detection.name] = rospy.Time.now()
+ if len(resp.detections) == 0:
+ return "unrecognised"
+ except rospy.ServiceException as e:
+ rospy.logerr("Service call failed: %s" % e)
+
+ def greet(self):
+ sm = smach.StateMachine(outcomes=["succeeded", "aborted", "preempted"])
+ with sm:
+ smach.StateMachine.add(
+ "SAY_DETECTIONS_IN_SEATING_AREA",
+ Say(
+ text=f"I have detected people. Hello, {' '.join(self.people_in_frame.keys())}"
+ ),
+ transitions={
+ "succeeded": "succeeded",
+ "aborted": "aborted",
+ "preempted": "preempted",
+ },
+ )
+
+ outcome = sm.execute()
+
+ def say_nobody_detected(self):
+ sm = smach.StateMachine(outcomes=["succeeded", "aborted", "preempted"])
+ with sm:
+ smach.StateMachine.add(
+ "SAY_NOBODY_DETECTED_IN_SEATING_AREA",
+ Say(text="I didn't detect anyone that I know."),
+ transitions={
+ "succeeded": "succeeded",
+ "aborted": "aborted",
+ "preempted": "preempted",
+ },
+ )
+
+ outcome = sm.execute()
+
+ def image_callback(self, image):
+
+ prev_people_in_frame = list(self.people_in_frame.keys())
+ # remove detections from people_in_frame that are older than 5 seconds long
+ self.detect(image)
+ for person in list(self.people_in_frame.keys()):
+ if rospy.Time.now() - self.people_in_frame[person] > rospy.Duration(10):
+ del self.people_in_frame[person]
+ if (
+ list(self.people_in_frame.keys()) != prev_people_in_frame
+ and len(self.people_in_frame) > 0
+ ) or (len(prev_people_in_frame) == 0 and len(self.people_in_frame) > 0):
+ self.detected_people = True
diff --git a/tasks/receptionist/src/receptionist/states/find_and_look_at.py b/tasks/receptionist/src/receptionist/states/find_and_look_at.py
index 2734b7f7c..442d5ba42 100755
--- a/tasks/receptionist/src/receptionist/states/find_and_look_at.py
+++ b/tasks/receptionist/src/receptionist/states/find_and_look_at.py
@@ -55,22 +55,6 @@ def execute(self, userdata):
userdata.look_positions = self.look_positions
return "succeeded"
- class GetGuestName(smach.State):
- def __init__(self, guest_name_in: str):
- super().__init__(
- outcomes=["succeeded"],
- input_keys=["guest_data"],
- output_keys=["guest_name"],
- )
- self._guest_name_in = guest_name_in
-
- def execute(self, userdata) -> str:
- # print("Guest data", self.userdata.guest_data)
- # self.userdata.guest_name = self.userdata.guest_data[guest_name_in]["name"]
- guest_name = userdata.guest_data[self._guest_name_in]["name"]
- userdata.guest_name = guest_name
- return "succeeded"
-
class GetPoint(smach.State):
def __init__(self):
smach.State.__init__(
@@ -97,12 +81,15 @@ def execute(self, userdata):
def check_name(self, ud):
rospy.logwarn(
- f"Checking name {ud.guest_name} in detections {ud.deepface_detection}"
+ f"Checking name {ud.guest_data[self.guest_name_in]['name']} in detections {ud.deepface_detection}"
)
if len(ud.deepface_detection) == 0:
return "no_detection"
for detection in ud.deepface_detection:
- if detection.name == ud.guest_name and detection.confidence > ud.confidence:
+ if (
+ detection.name == ud.guest_data[self.guest_name_in]["name"]
+ and detection.confidence > ud.confidence
+ ):
return "succeeded"
return "failed"
@@ -111,14 +98,13 @@ def __init__(
guest_name_in: str,
look_positions: Union[List[List[float]], None] = None,
):
+
+ self.guest_name_in = guest_name_in
+
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "failed"],
- input_keys=[
- "dataset",
- "confidence",
- "guest_data",
- ],
+ input_keys=["dataset", "confidence", "guest_data"],
output_keys=[],
)
@@ -176,31 +162,26 @@ def __init__(
request=StartupStopRequest("head_manager"),
),
transitions={
- "succeeded": "GET_GUEST_NAME",
+ "succeeded": "GET_POINT",
"aborted": "failed",
"preempted": "failed",
},
)
- smach.StateMachine.add(
- "GET_GUEST_NAME",
- self.GetGuestName(guest_name_in=guest_name_in),
- transitions={"succeeded": "GET_POINT"},
- )
smach.StateMachine.add(
"GET_POINT",
self.GetPoint(),
transitions={"succeeded": "GET_IMAGE", "failed": "failed"},
remapping={"pointstamped": "pointstamped"},
)
- smach.StateMachine.add(
- "LOOK_TO_POINT",
- LookToPoint(),
- transitions={
- "succeeded": "GET_IMAGE",
- "aborted": "failed",
- "preempted": "failed",
- },
- )
+ # smach.StateMachine.add(
+ # "LOOK_TO_POINT",
+ # LookToPoint(),
+ # transitions={
+ # "succeeded": "GET_IMAGE",
+ # "aborted": "failed",
+ # "preempted": "failed",
+ # },
+ # )
smach.StateMachine.add(
"GET_IMAGE",
GetPointCloud("/xtion/depth_registered/points"),
@@ -240,13 +221,13 @@ def __init__(
outcomes=["succeeded", "failed", "no_detection"],
input_keys=[
"deepface_detection",
- "guest_name",
"confidence",
+ "guest_data",
],
),
transitions={
"succeeded": "LOOK_AT_PERSON",
- "failed": "GET_IMAGE",
+ "failed": "continue",
"no_detection": "continue",
},
)
diff --git a/tasks/receptionist/src/receptionist/states/get_attributes.py b/tasks/receptionist/src/receptionist/states/get_attributes.py
index d3390d373..ee8a864a3 100644
--- a/tasks/receptionist/src/receptionist/states/get_attributes.py
+++ b/tasks/receptionist/src/receptionist/states/get_attributes.py
@@ -31,7 +31,7 @@ def __init__(
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "failed"],
- input_keys=["guest_id", "guest_data"],
+ input_keys=["guest_data"],
output_keys=["guest_data"],
)
self._guest_id: str = guest_id
diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
index a6203793f..1ef8c91a4 100644
--- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
+++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
@@ -23,7 +23,7 @@ def __init__(
"""
smach.State.__init__(
self,
- outcomes=["succeeded", "failed"],
+ outcomes=["succeeded", "failed", "failed_name", "failed_drink"],
input_keys=["guest_transcription", "guest_data"],
output_keys=["guest data", "guest_transcription"],
)
@@ -44,7 +44,6 @@ def execute(self, userdata: UserData) -> str:
str: state outcome. Updates the userdata with the parsed name and drink, under
the parameter "guest data".
"""
-
outcome = "succeeded"
name_found = False
drink_found = False
@@ -68,9 +67,35 @@ def execute(self, userdata: UserData) -> str:
if not name_found:
rospy.loginfo("Name not found in transcription")
+ userdata.guest_data[self._guest_id]["name"] = "unknown"
outcome = "failed"
if not drink_found:
rospy.loginfo("Drink not found in transcription")
+ userdata.guest_data[self._guest_id]["drink"] = "unknown"
outcome = "failed"
+ {
+ "name": name,
+ "drink": drink,
+ },
+
+ if outcome == "failed":
+ if not self._recovery_name_and_drink_required(userdata):
+ if userdata.guest_data[self._guest_id]["name"] == "unknown":
+ outcome = "failed_name"
+ else:
+ outcome = "failed_drink"
+
return outcome
+
+ def _recovery_name_and_drink_required(self, userdata: UserData) -> bool:
+ """Determine whether both the name and drink requires recovery.
+
+ Returns:
+ bool: True if both attributes require recovery.
+ """
+ if userdata.guest_data[self._guest_id]["name"] == "unknown":
+ if userdata.guest_data[self._guest_id]["drink"] == "unknown":
+ return True
+ else:
+ return False
diff --git a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py
new file mode 100644
index 000000000..88cbe432a
--- /dev/null
+++ b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py
@@ -0,0 +1,74 @@
+"""
+State for parsing the transcription of the guests' information (favourite drink or name), and adding this
+to the guest data userdata
+"""
+
+import rospy
+import smach
+from smach import UserData
+from typing import List, Dict, Any
+
+
+class ParseTranscribedInfo(smach.State):
+ def __init__(
+ self,
+ guest_id: str,
+ info_type: str,
+ # param_key: str = "/priors",
+ param_key: str = "/receptionist/priors",
+ ):
+ """Parses the transcription of the guests' information.
+
+ Args:
+ guest_id (str): ID of the guest (identifying the guest)
+ info_type (str): The type of information to try and extract useful information
+ (drink or name)
+ param_key (str, optional): Name of the parameter that contains the list of
+ possible . Defaults to "receptionist/priors".
+ """
+ smach.State.__init__(
+ self,
+ outcomes=["succeeded", "failed"],
+ input_keys=["guest_transcription", "guest_data"],
+ output_keys=["guest data", "guest_transcription"],
+ )
+ self._guest_id = guest_id
+ self._type = info_type
+ prior_data: Dict[str, List[str]] = rospy.get_param(param_key)
+ possible_drinks = [drink.lower() for drink in prior_data["drinks"]]
+ possible_names = [name.lower() for name in prior_data["names"]]
+ self._possible_information = {"drink": possible_drinks, "name": possible_names}[
+ self._type
+ ]
+
+ def execute(self, userdata: UserData) -> str:
+ """Parse the guest's information.
+
+ Args:
+ userdata (UserData): State machine userdata assumed to contain a key
+ called "guest transcription" with the transcription of the guest's name or
+ favourite drink or both.
+
+ Returns:
+ str: state outcome. Updates the userdata with the parsed information (drink or name), under
+ the parameter "guest data".
+ """
+ outcome = "succeeded"
+ information_found = False
+ transcription = userdata.guest_transcription.lower()
+
+ transcription = userdata["guest_transcription"].lower()
+
+ for key_phrase in self._possible_information:
+ print(self._possible_information)
+ if key_phrase in transcription:
+ userdata.guest_data[self._guest_id][self._type] = key_phrase
+ rospy.loginfo(f"Guest {self._type} identified as: {key_phrase}")
+ information_found = True
+ break
+ if not information_found:
+ rospy.loginfo(f"{self._type} not found in transcription")
+ userdata.guest_data[self._guest_id][self._type] = "unknown"
+ outcome = "failed"
+
+ return outcome
diff --git a/tasks/receptionist/src/receptionist/states/introduce.py b/tasks/receptionist/src/receptionist/states/introduce.py
index 3a5ab5a4d..7650737ff 100644
--- a/tasks/receptionist/src/receptionist/states/introduce.py
+++ b/tasks/receptionist/src/receptionist/states/introduce.py
@@ -24,20 +24,116 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
"""
relevant_guest_data = guest_data[guest_id]
+ relevant_guest_data.setdefault(
+ "attributes",
+ {
+ "hair_shape": "unknown",
+ "hair_colour": "unknown",
+ "facial_hair": "No_Beard",
+ "earrings": "unknown",
+ "necklace": "unknown",
+ "necktie": "unknown",
+ "height": "unknown",
+ "glasses": False,
+ "hat": False,
+ },
+ )
+
guest_str = ""
- if "attributes" in relevant_guest_data.keys():
- guest_str += f"They have {relevant_guest_data['attributes']['hair_shape']} {relevant_guest_data['attributes']['hair_colour']} hair, and they "
- guest_str += f"{'have facial hair' if relevant_guest_data['attributes']['facial_hair'] else 'do not have facial hair'}. "
- guest_str += f"They are {'wearing glasses' if relevant_guest_data['attributes']['glasses'] else 'not wearing glasses'}, "
- guest_str += f"{'wearing earrings' if relevant_guest_data['attributes']['earrings'] else 'not wearing earrings'}, and "
- guest_str += f"{'wearing a hat' if relevant_guest_data['attributes']['hat'] else 'not wearing a hat'}. "
- guest_str += f"They are {'wearing a necklace' if relevant_guest_data['attributes']['necklace'] else 'not wearing a necklace'}, and "
- guest_str += f"{'wearing a necktie' if relevant_guest_data['attributes']['necktie'] else 'not wearing a necktie'}. "
+ guest_str += f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. "
+
+ known_attributes = {}
+
+ for attribute, value in relevant_guest_data["attributes"].items():
+ if value != "unknown" and value != False and value != "No_Beard":
+ known_attributes[attribute] = value
+ print("These are the known attributes")
+ print(known_attributes)
+
+ has_hair = False
+ detection = False # Whenever the an attribute is detected in the for loop, the detection flag is set to true
+ # so that multiple attributes are not checked at the same time
+
+ for attribute, value in known_attributes.items():
+ if attribute == "has_hair":
+ has_hair = True
+ break
+
+ ignored_attributes = [
+ "top",
+ "down",
+ "outwear",
+ "dress",
+ "short sleeve top",
+ "long sleeve top",
+ "short sleeve outwear",
+ "long sleeve outwear",
+ "vest",
+ "sling",
+ "shorts",
+ "trousers",
+ "skirt",
+ "short sleeve dress",
+ "long sleeve dress",
+ "vest dress",
+ "sling dress",
+ ]
+
+ ignored_attributes.append("male")
+ ignored_attributes.append("has_hair")
+
+ for attribute, value in known_attributes.items():
+ if attribute in ignored_attributes:
+ detection = True
+ if has_hair:
+ if attribute == "hair_shape":
+ guest_str += (
+ f"Their hair is {relevant_guest_data['attributes'][attribute]}."
+ )
+ detection = True
+ elif attribute == "hair_colour":
+ guest_str += (
+ f"They have {relevant_guest_data['attributes'][attribute]} hair."
+ )
+ detection = True
+ if attribute == "facial_hair":
+ guest_str += f"They have facial hair."
+ detection = True
+ if not detection:
+ if isSingular(attribute):
+ guest_str += f"They are a wearing {attribute}."
+ else:
+ guest_str += f"They are wearing {attribute}."
+
+ # if "attributes" in relevant_guest_data.keys():
+
+ # guest_str += f"They have {relevant_guest_data['attributes']['hair_shape']} {relevant_guest_data['attributes']['hair_colour']} hair, and they "
+ # guest_str += f"{'have facial hair' if relevant_guest_data['attributes']['facial_hair'] else 'do not have facial hair'}. "
+ # guest_str += f"They are {'wearing glasses' if relevant_guest_data['attributes']['glasses'] else 'not wearing glasses'}, "
+ # guest_str += f"{'wearing earrings' if relevant_guest_data['attributes']['earrings'] else 'not wearing earrings'}, and "
+ # guest_str += f"{'wearing a hat' if relevant_guest_data['attributes']['hat'] else 'not wearing a hat'}. "
+ # guest_str += f"They are {'wearing a necklace' if relevant_guest_data['attributes']['necklace'] else 'not wearing a necklace'}, and "
+ # guest_str += f"{'wearing a necktie' if relevant_guest_data['attributes']['necktie'] else 'not wearing a necktie'}. "
return guest_str
+def isSingular(attribute: str):
+ """Checks is a word is singular or plural by checking the last letter
+
+ Args:
+ attribute (str): The attribute to check for plurality
+
+ Returns:
+ (bool): Boolean identifying whether the word is plural
+ """
+ if attribute[len(attribute) - 1] == "s":
+ return False
+ else:
+ return True
+
+
class GetStrGuestData(smach.State):
def __init__(self, guest_id: str):
super().__init__(
diff --git a/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py
new file mode 100644
index 000000000..1607a7754
--- /dev/null
+++ b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py
@@ -0,0 +1,40 @@
+"""The receptionist version of learn faces uses userdata for the name of the guest instead"""
+
+import smach
+import rospy
+import sys
+
+
+from lasr_vision_msgs.srv import (
+ LearnFace,
+ LearnFaceRequest,
+ LearnFaceResponse,
+)
+
+
+class ReceptionistLearnFaces(smach.State):
+ def __init__(self, guest_id: str):
+ smach.State.__init__(
+ self, outcomes=["succeeded", "failed"], input_keys=["guest_data"]
+ )
+ self._guest_id = guest_id
+
+ def execute(self, userdata):
+ print("here we will learn faces")
+ try:
+ learn_service = rospy.ServiceProxy("/learn_face", LearnFace)
+ guest_name = userdata.guest_data[self._guest_id]["name"]
+ print(guest_name)
+ req = LearnFaceRequest()
+ req.name = guest_name
+ req.dataset = "receptionist"
+ req.n_images = 10
+ resp = learn_service(req)
+ except ValueError as e:
+ print("No face detected. Error:" + str(e))
+ return "failed"
+ except rospy.ServiceException as e:
+ rospy.logerr("Service call failed: %s" % e)
+ return "failed"
+
+ return "succeeded"
diff --git a/tasks/receptionist/src/receptionist/states/seat_guest.py b/tasks/receptionist/src/receptionist/states/seat_guest.py
index 1957cfe37..f26360c3a 100755
--- a/tasks/receptionist/src/receptionist/states/seat_guest.py
+++ b/tasks/receptionist/src/receptionist/states/seat_guest.py
@@ -1,12 +1,20 @@
import smach
+import rospy
from typing import List
from shapely.geometry import Polygon
import numpy as np
-from lasr_skills import PlayMotion, Detect3DInArea, LookToPoint, Say
from geometry_msgs.msg import Point, PointStamped
+from lasr_skills import (
+ PlayMotion,
+ Detect3DInArea,
+ LookToPoint,
+ Say,
+ WaitForPerson,
+ Wait,
+)
class SeatGuest(smach.StateMachine):
@@ -119,7 +127,7 @@ def __init__(
transitions={
"succeeded": "SAY_SIT",
"aborted": "failed",
- "preempted": "failed",
+ "timed_out": "SAY_SIT",
},
remapping={"pointstamped": "seat_position"},
)
@@ -127,11 +135,21 @@ def __init__(
"SAY_SIT",
Say("Please sit in the seat that I am looking at."),
transitions={
- "succeeded": "RESET_HEAD",
+ "succeeded": "WAIT_FOR_GUEST_SEAT",
"aborted": "failed",
"preempted": "failed",
},
- ) # TODO: sleep after this.
+ )
+
+ smach.StateMachine.add(
+ "WAIT_FOR_GUEST_SEAT",
+ # Number of seconds to wait for passed in as argument
+ Wait(5),
+ transitions={
+ "succeeded": "RESET_HEAD",
+ "failed": "RESET_HEAD",
+ },
+ )
smach.StateMachine.add(
"RESET_HEAD",