diff --git a/common/navigation/lasr_person_following/nodes/person_following.py b/common/navigation/lasr_person_following/nodes/person_following.py
index 4124045e6..456f1eaa4 100644
--- a/common/navigation/lasr_person_following/nodes/person_following.py
+++ b/common/navigation/lasr_person_following/nodes/person_following.py
@@ -47,7 +47,7 @@ def __init__(self) -> None:
def _execute_cb(self, _: FollowGoal) -> None:
print("Executing follow_person action")
- while not self._follower.begin_tracking(ask=False):
+ while not self._follower.begin_tracking():
rospy.logwarn("No people found, retrying...")
rospy.sleep(rospy.Duration(1.0))
warnings.warn(
diff --git a/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py b/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
index 50e190825..2282eb8ed 100644
--- a/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
+++ b/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
@@ -26,7 +26,7 @@
from play_motion_msgs.msg import PlayMotionAction
from play_motion_msgs.msg import PlayMotionGoal
-from lasr_vision_msgs.srv import DetectWaveRequest
+from lasr_vision_msgs.srv import DetectWaveRequest, DetectWaveResponse
from math import atan2
import numpy as np
@@ -79,17 +79,17 @@ def __init__(
self,
start_following_radius: float = 2.0,
start_following_angle: float = 45.0,
- n_secs_static_finished: float = 8.0,
- n_secs_static_plan_close: float = 10.0,
new_goal_threshold: float = 2.0,
stopping_distance: float = 1.0,
static_speed: float = 0.0015,
+ max_speed: float = 0.5,
):
self._start_following_radius = start_following_radius
self._start_following_angle = start_following_angle
self._new_goal_threshold = new_goal_threshold
self._stopping_distance = stopping_distance
self._static_speed = static_speed
+ self._max_speed = max_speed
self._track_id = None
@@ -147,7 +147,9 @@ def _tf_pose(self, pose: PoseStamped, target_frame: str):
def _robot_pose_in_odom(self) -> Union[PoseStamped, None]:
try:
current_pose: PoseWithCovarianceStamped = rospy.wait_for_message(
- "/robot_pose", PoseWithCovarianceStamped
+ "/robot_pose",
+ PoseWithCovarianceStamped,
+ timeout=rospy.Duration.from_sec(2.0),
)
except rospy.ROSException:
return None
@@ -208,10 +210,10 @@ def begin_tracking(self) -> bool:
def _recover_track(self, say) -> bool:
if not say:
- self._tts("I SAW A person waving", wait=True)
+ self._tts("I see you are waving", wait=True)
if self._tts_client_available and say:
- self._tts("I lost track of you, please come back", wait=True)
+ self._tts("Please could you come back...", wait=True)
while not self.begin_tracking() and not rospy.is_shutdown():
rospy.loginfo("Recovering track...")
@@ -221,21 +223,7 @@ def _recover_track(self, say) -> bool:
return True
- # recover with vision, look up and check if person is waving
- def _recover_vision(self, prev_pose: PoseStamped) -> bool:
- # look up with playmotion and detect wave service
- # if detected, begin tracking
-
- # use play motion to look up
- self._cancel_goal()
-
- goal = PlayMotionGoal()
- goal.motion_name = "look_centre"
- self._play_motion.send_goal_and_wait(goal)
-
- self._tts("Can you wave at me so that i can try to find you easily", wait=True)
-
- # detect wave
+ def _detect_waving_person(self) -> DetectWaveResponse:
try:
pcl = rospy.wait_for_message("/xtion/depth_registered/points", PointCloud2)
req = DetectWaveRequest()
@@ -243,35 +231,54 @@ def _recover_vision(self, prev_pose: PoseStamped) -> bool:
req.dataset = "resnet50"
req.confidence = 0.1
response = self._detect_wave(req)
- if response.wave_detected:
- rospy.loginfo("Wave detected, beginning tracking")
- if np.isnan(response.wave_position.point.x) or np.isnan(
- response.wave_position.point.y
- ):
- return False
- goal_pose = self._tf_pose(
- PoseStamped(
- pose=Pose(
- position=Point(
- x=response.wave_position.point.x,
- y=response.wave_position.point.y,
- z=response.wave_position.point.z,
- ),
- orientation=Quaternion(0, 0, 0, 1),
- ),
- header=pcl.header,
- ),
- "map",
- )
- rospy.loginfo(goal_pose.pose.position)
- goal_pose.pose.orientation = self._compute_face_quat(
- prev_pose.pose, goal_pose.pose
- )
- self._move_base(goal_pose)
- return True
+ return response
except rospy.ServiceException as e:
rospy.loginfo(f"Error detecting wave: {e}")
- return False
+ return DetectWaveResponse()
+
+ # recover with vision, look around and check if person is waving
+ def _recover_vision(self, prev_pose: PoseStamped) -> bool:
+
+ # cancel current goal
+ self._cancel_goal()
+
+ self._tts("Can you wave at me so that i can try to find you easily", wait=True)
+
+ for motion in self._vision_recovery_motions:
+ rospy.loginfo(f"Performing motion: {motion}")
+ goal = PlayMotionGoal()
+ goal.motion_name = motion
+ self._play_motion.send_goal_and_wait(goal)
+ for _ in range(self._vision_recovery_attempts):
+ response = self._detect_waving_person()
+ if response.wave_detected:
+ if np.isnan(response.wave_position.point.x) or np.isnan(
+ response.wave_position.point.y
+ ):
+ continue
+ else:
+ goal_pose = self._tf_pose(
+ PoseStamped(
+ pose=Pose(
+ position=Point(
+ x=response.wave_position.point.x,
+ y=response.wave_position.point.y,
+ z=response.wave_position.point.z,
+ ),
+ orientation=Quaternion(0, 0, 0, 1),
+ ),
+ header=prev_pose.header,
+ ),
+ "map",
+ )
+ goal_pose.pose.orientation = self._compute_face_quat(
+ prev_pose.pose, goal_pose.pose
+ )
+ rospy.loginfo(goal_pose.pose.position)
+ self._move_base(goal_pose)
+ return True
+
+ return False
def _euclidian_distance(self, p1: Pose, p2: Pose) -> float:
return np.linalg.norm(
@@ -375,7 +382,7 @@ def follow(self) -> None:
prev_goal: Union[None, PoseStamped] = None
last_goal_time: Union[None, rospy.Time] = None
going_to_person: bool = False
- track_vels: [float] = []
+ track_vels: List[float] = []
while not rospy.is_shutdown():
@@ -395,8 +402,27 @@ def follow(self) -> None:
if track is None:
rospy.loginfo("Lost track of person, recovering...")
person_trajectory = PoseArray()
- recovery = self._recover_vision(prev_goal)
- self._recover_track(say=not recovery)
+ ask_back: bool = False
+
+ if prev_track is not None:
+ robot_pose: PoseStamped = self._robot_pose_in_odom()
+ if robot_pose:
+ dist: float = self._euclidian_distance(
+ robot_pose.pose, prev_track.pose
+ )
+ rospy.loginfo(f"Distance to last known position: {dist}")
+ if dist >= MAX_VISION_DIST:
+ ask_back = True
+ else:
+ ask_back = True
+ else:
+ ask_back = True
+
+ if not ask_back:
+ self._recover_track(say=not self._recover_vision(prev_goal))
+ else:
+ self._recover_track(say=True)
+
prev_track = None
continue
@@ -418,6 +444,9 @@ def follow(self) -> None:
last_goal_time = rospy.Time.now()
prev_track = track
+ if np.mean([np.linalg.norm(vel) for vel in track_vels]) > self._max_speed:
+ self._tts("Please walk slower, I am struggling to keep up", wait=False)
+
# Distance to the previous pose
dist_to_prev = (
self._euclidian_distance(track.pose, prev_track.pose)
@@ -448,10 +477,51 @@ def follow(self) -> None:
elif (
np.mean([np.linalg.norm(vel) for vel in track_vels])
< self._static_speed
- ):
-
- rospy.logwarn("Person has been static for too long, stopping")
+ ) and len(track_vels) == 10:
+ rospy.logwarn(
+ "Person has been static for too long, going to them and stopping"
+ )
+ # cancel current goal
self._cancel_goal()
+
+ # clear velocity buffer
+ track_vels = []
+
+ robot_pose: PoseStamped = self._robot_pose_in_odom()
+ dist: float = self._euclidian_distance(track.pose, robot_pose.pose)
+ rospy.loginfo(f"Distance to person: {dist}")
+
+ # If the person is too far away, go to them
+ if dist > self._stopping_distance:
+ goal_pose = self._get_pose_on_path(
+ self._tf_pose(robot_pose, "map"),
+ self._tf_pose(
+ PoseStamped(
+ pose=track.pose,
+ header=tracks.header,
+ ),
+ "map",
+ ),
+ self._stopping_distance,
+ )
+ # If we can't find a path, face them
+ if goal_pose is None:
+ rospy.logwarn("Could not find a path to the person")
+ goal_pose = robot_pose
+ goal_pose.pose.orientation = self._compute_face_quat(
+ robot_pose.pose, track.pose
+ )
+ goal_pose = self._tf_pose(goal_pose, "map")
+ # Otherwise, face them
+ else:
+ goal_pose = robot_pose
+ goal_pose.pose.orientation = self._compute_face_quat(
+ robot_pose.pose, track.pose
+ )
+ goal_pose = self._tf_pose(goal_pose, "map")
+
+ self._move_base(goal_pose)
+
if self._check_finished():
rospy.loginfo("Finished following person")
break
diff --git a/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server b/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server
index eff2c232f..a8da0cbc6 100644
--- a/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server
+++ b/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server
@@ -290,12 +290,12 @@ def parse_args() -> dict:
help="Disable warming up the model by running inference on a test file.",
)
- # parser.add_argument(
- # "--energy_threshold",
- # type=int,
- # default=600,
- # help="Energy threshold for silence detection. Using this disables automatic adjustment",
- # )
+ parser.add_argument(
+ "--energy_threshold",
+ type=Optional[int],
+ default=None,
+ help="Energy threshold for silence detection. Using this disables automatic adjustment",
+ )
parser.add_argument(
"--pause_threshold",
diff --git a/common/third_party/leg_tracker b/common/third_party/leg_tracker
index e6cbb2bba..1f7c2a436 160000
--- a/common/third_party/leg_tracker
+++ b/common/third_party/leg_tracker
@@ -1 +1 @@
-Subproject commit e6cbb2bba218e0684714a08972a7bdfa99118e3c
+Subproject commit 1f7c2a43621e7cee319a2769537bca5d6b90f909
diff --git a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
index 6140b8639..e0517901f 100644
--- a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
+++ b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
@@ -152,10 +152,10 @@ def detect_3d(
f"Detected point: {detection.point} of object {detection.name}"
)
- markers.create_and_publish_marker(
- debug_point_publisher,
- PointStamped(point=detection.point, header=pcl_map.header),
- )
+ # markers.create_and_publish_marker(
+ # debug_point_publisher,
+ # PointStamped(point=detection.point, header=pcl_map.header),
+ # )
detected_objects.append(detection)
diff --git a/tasks/gpsr/launch/setup.launch b/tasks/gpsr/launch/setup.launch
index d44e62ce1..48ae7cb34 100644
--- a/tasks/gpsr/launch/setup.launch
+++ b/tasks/gpsr/launch/setup.launch
@@ -31,7 +31,7 @@
-
+
-
+
@@ -15,7 +15,6 @@
-
diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py
index 6bb3b3aaa..aea5ec969 100644
--- a/tasks/receptionist/scripts/main.py
+++ b/tasks/receptionist/scripts/main.py
@@ -49,10 +49,10 @@
max_people_on_sofa = rospy.get_param("/receptionist/max_people_on_sofa")
seat_area = ShapelyPolygon(seat_area_param)
- assert seat_area.is_valid
+ assert seat_area.is_valid, "Seat area is not valid"
sofa_area = ShapelyPolygon(sofa_area_param)
- assert sofa_area.is_valid
+ assert sofa_area.is_valid, "Sofa area is not valid"
sofa_point = Point(**sofa_point_param)
diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py
index 8d7dee20e..f1f8be629 100755
--- a/tasks/receptionist/src/receptionist/state_machine.py
+++ b/tasks/receptionist/src/receptionist/state_machine.py
@@ -280,6 +280,16 @@ def _guide_guest(self, guest_id: int) -> None:
smach.StateMachine.add(
f"SAY_WAIT_GUEST_{guest_id}",
Say(text="Please wait here on my left."),
+ transitions={
+ "succeeded": f"LOOK_EYES_{guest_id}",
+ "preempted": "failed",
+ "aborted": "failed",
+ },
+ )
+
+ smach.StateMachine.add(
+ f"LOOK_EYES_{guest_id}",
+ PlayMotion(motion_name="look_very_left"),
transitions={
"succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}",
"preempted": "failed",
diff --git a/tasks/receptionist/src/receptionist/states/handle_guest.py b/tasks/receptionist/src/receptionist/states/handle_guest.py
index d7ae0b420..bbfe2b649 100644
--- a/tasks/receptionist/src/receptionist/states/handle_guest.py
+++ b/tasks/receptionist/src/receptionist/states/handle_guest.py
@@ -46,7 +46,7 @@ def __init__(self, guest_id: str):
smach.StateMachine.add(
f"REPEAT_GET_NAME_AND_DRINK_GUEST_{guest_id}",
AskAndListen(
- "Sorry, I didn't get that, please raise your voice. What is your name and favourite drink?"
+ "Sorry, I didn't get that, please raise your voice. What is your name and favourite drink? Please remember to say 'hi tiago'"
),
transitions={
"succeeded": f"REPEAT_PARSE_NAME_AND_DRINK_GUEST_{guest_id}",
diff --git a/tasks/receptionist/src/receptionist/states/introduce.py b/tasks/receptionist/src/receptionist/states/introduce.py
index 2f14012ea..b0f091012 100644
--- a/tasks/receptionist/src/receptionist/states/introduce.py
+++ b/tasks/receptionist/src/receptionist/states/introduce.py
@@ -11,7 +11,9 @@
from typing import Dict, List, Any, Optional
-def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
+def stringify_guest_data(
+ guest_data: Dict[str, Any], guest_id: str, describe_features: bool
+) -> 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.
@@ -46,11 +48,9 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
},
)
- guest_str = ""
+ guest_str = f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. "
- guest_str += f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. "
-
- if not relevant_guest_data["detection"]:
+ if not relevant_guest_data["detection"] or not describe_features:
return guest_str
filtered_attributes = {}
@@ -107,6 +107,9 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
if len(top_4_attributes) < 4:
top_4_attributes.append(sorted_attributes[4])
+ wearing_items = []
+ not_wearing_items = []
+
for i in range(len(top_4_attributes)):
attribute_name = top_4_attributes[i]
attribute_value = filtered_attributes[top_4_attributes[i]]
@@ -115,25 +118,47 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str:
if attribute_name == "hair":
hair_shape = attribute_value["hair_shape"]
hair_colour = attribute_value["hair_colour"]
- guest_str += f"They have {hair_shape} and {hair_colour} . "
+ guest_str += f"They have {hair_shape} and {hair_colour}. "
elif attribute_name == "facial_hair":
if confidence < 0:
guest_str += "They don't have facial hair. "
else:
guest_str += "They have facial hair. "
else:
+ attribute = attribute_value["attribute"]
if confidence < 0:
- if isSingular(attribute_value["attribute"]):
- guest_str += (
- f"They are not wearing a {attribute_value['attribute']}."
- )
+ if isSingular(attribute):
+ not_wearing_items.append(f"a {attribute}")
else:
- guest_str += f"They are not wearing {attribute_value['attribute']}."
+ not_wearing_items.append(attribute)
else:
- if isSingular(attribute_value["attribute"]):
- guest_str += f"They are wearing a {attribute_value['attribute']}."
+ if isSingular(attribute):
+ wearing_items.append(f"a {attribute}")
else:
- guest_str += f"They are wearing {attribute_value['attribute']}."
+ wearing_items.append(attribute)
+
+ def grammatical_concat(items):
+ if len(items) > 1:
+ return ", ".join(items[:-1]) + " and " + items[-1]
+ elif items:
+ return items[0]
+ else:
+ return ""
+
+ # Combine wearing and not wearing items into guest_str
+ if wearing_items:
+ guest_str += "They are wearing " + grammatical_concat(wearing_items) + ". "
+ if not_wearing_items:
+ if wearing_items:
+ guest_str += (
+ "They are also not wearing "
+ + grammatical_concat(not_wearing_items)
+ + "."
+ )
+ else:
+ guest_str += (
+ "They are not wearing " + grammatical_concat(not_wearing_items) + "."
+ )
return guest_str
@@ -179,16 +204,19 @@ def isSingular(attribute: str):
class GetStrGuestData(smach.State):
- def __init__(self, guest_id: str):
+ def __init__(self, guest_id: str, describe_features: bool = False):
super().__init__(
outcomes=["succeeded"],
input_keys=["guest_data"],
output_keys=["guest_str"],
)
self._guest_id = guest_id
+ self._describe_features = describe_features
def execute(self, userdata: UserData) -> str:
- guest_str = stringify_guest_data(userdata.guest_data, self._guest_id)
+ guest_str = stringify_guest_data(
+ userdata.guest_data, self._guest_id, self._describe_features
+ )
userdata.guest_str = guest_str
return "succeeded"
@@ -229,6 +257,7 @@ def __init__(
self,
guest_to_introduce: str,
guest_to_introduce_to: Optional[str] = None,
+ describe_features: bool = False,
everyone: Optional[bool] = False,
):
super().__init__(
@@ -241,7 +270,9 @@ def __init__(
if everyone:
smach.StateMachine.add(
"GetStrGuestData",
- GetStrGuestData(guest_id=guest_to_introduce),
+ GetStrGuestData(
+ guest_id=guest_to_introduce, describe_features=describe_features
+ ),
transitions={"succeeded": "SayIntroduce"},
)
smach.StateMachine.add(
@@ -260,7 +291,9 @@ def __init__(
else:
smach.StateMachine.add(
"GetStrGuestData",
- GetStrGuestData(guest_id=guest_to_introduce),
+ GetStrGuestData(
+ guest_id=guest_to_introduce, describe_features=describe_features
+ ),
transitions={"succeeded": "GetGuestName"},
)
diff --git a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
index 7d98fefe3..54193976e 100644
--- a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
+++ b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
@@ -466,7 +466,7 @@ class GetLookPoint(smach.State):
def __init__(self, guest_id: str):
smach.State.__init__(
self,
- outcomes=["succeeded"],
+ outcomes=["succeeded", "failed"],
input_keys=["matched_face_detections"],
output_keys=["look_point"],
)
@@ -476,13 +476,14 @@ def execute(self, userdata):
if len(userdata.matched_face_detections) == 0:
userdata.look_point = PointStamped()
rospy.logwarn(f"Failed to find guest: {self._guest_id}")
- return "succeeded"
+ return "failed"
for detection in userdata.matched_face_detections:
if detection.name == self._guest_id:
look_point = PointStamped(
point=detection.point, header=Header(frame_id="map")
)
+ look_point.point.z = 0.75 # fixed height
userdata.look_point = look_point
return "succeeded"
@@ -518,10 +519,13 @@ def execute(self, userdata):
return "succeeded_sofa"
if len(userdata.empty_seat_detections) > 0:
- userdata.seat_position = PointStamped(
+ seat_position = PointStamped(
point=userdata.empty_seat_detections[0].point,
header=Header(frame_id="map"),
)
+ seat_position.point.z = 0.5 # fixed height
+ userdata.seat_position = seat_position
+
return "succeeded_chair"
return "failed"
@@ -633,7 +637,20 @@ def execute(self, userdata):
smach.StateMachine.add(
f"GET_LOOK_POINT_{guest_to_introduce_to}",
GetLookPoint(guest_to_introduce_to),
- transitions={"succeeded": f"LOOK_AT_{guest_to_introduce_to}"},
+ transitions={
+ "succeeded": f"LOOK_AT_{guest_to_introduce_to}",
+ "failed": f"LOOK_CENTRE_BACKUP_{guest_to_introduce_to}",
+ },
+ )
+
+ smach.StateMachine.add(
+ f"LOOK_CENTRE_BACKUP_{guest_to_introduce_to}",
+ PlayMotion(motion_name="look_centre"),
+ transitions={
+ "succeeded": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ "aborted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ "preempted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ },
)
# Look at the guest to introduce to
@@ -654,13 +671,36 @@ def execute(self, userdata):
Introduce(
guest_to_introduce=guest_id,
guest_to_introduce_to=guest_to_introduce_to,
+ describe_features=guest_to_introduce_to != "host",
+ ),
+ transitions={
+ "succeeded": f"LOOK_AT_WAITING_GUEST_{guest_id}_{guest_to_introduce_to}",
+ },
+ )
+
+ smach.StateMachine.add(
+ f"LOOK_AT_WAITING_GUEST_{guest_id}_{guest_to_introduce_to}",
+ PlayMotion(motion_name="look_very_left"),
+ transitions={
+ "succeeded": f"INTRODUCE_{guest_to_introduce_to}_TO_{guest_id}",
+ "aborted": f"INTRODUCE_{guest_to_introduce_to}_TO_{guest_id}",
+ "preempted": f"INTRODUCE_{guest_to_introduce_to}_TO_{guest_id}",
+ },
+ )
+
+ smach.StateMachine.add(
+ f"INTRODUCE_{guest_to_introduce_to}_TO_{guest_id}",
+ Introduce(
+ guest_to_introduce=guest_to_introduce_to,
+ guest_to_introduce_to=guest_id,
+ describe_features=guest_to_introduce_to != "host",
),
transitions={
"succeeded": (
"SELECT_SEAT"
if i == len(guests_to_introduce_to) - 1
else f"GET_LOOK_POINT_{guests_to_introduce_to[i+1]}"
- )
+ ),
},
)
@@ -671,14 +711,24 @@ def execute(self, userdata):
transitions={
"succeeded_sofa": "SAY_SOFA",
"succeeded_chair": "SAY_CHAIR",
- "failed": "SAY_ANY",
+ "failed": "LOOK_CENTRE_SEAT",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_CENTRE_SEAT",
+ PlayMotion(motion_name="look_centre"),
+ transitions={
+ "succeeded": "SAY_ANY",
+ "aborted": "SAY_ANY",
+ "preempted": "SAY_ANY",
},
)
# Say to sit on the sofa
smach.StateMachine.add(
"SAY_SOFA",
- Say(text="Please sit on the sofa"),
+ Say(text="Please sit on the sofa that I am looking at"),
transitions={
"succeeded": "LOOK_AT_SEAT",
"preempted": "LOOK_AT_SEAT",
@@ -702,9 +752,9 @@ def execute(self, userdata):
"SAY_ANY",
Say(text="Please sit on any empty seat"),
transitions={
- "succeeded": "succeeded",
- "preempted": "succeeded",
- "aborted": "succeeded",
+ "succeeded": "WAIT_SEAT",
+ "preempted": "WAIT_SEAT",
+ "aborted": "WAIT_SEAT",
},
)
@@ -713,12 +763,19 @@ def execute(self, userdata):
"LOOK_AT_SEAT",
LookToPoint(),
transitions={
- "succeeded": "succeeded",
- "aborted": "succeeded",
- "timed_out": "succeeded",
+ "succeeded": "WAIT_SEAT",
+ "aborted": "WAIT_SEAT",
+ "timed_out": "WAIT_SEAT",
},
remapping={"pointstamped": "seat_position"},
)
+
+ smach.StateMachine.add(
+ "WAIT_SEAT",
+ Wait(3),
+ transitions={"succeeded": "succeeded", "failed": "failed"},
+ )
+
else:
class RecognisePeople(smach.State):
@@ -869,7 +926,7 @@ def __init__(self, guest_id: str):
def execute(self, userdata):
if len(userdata.matched_face_detections) == 0:
userdata.look_point = PointStamped()
- return "succeeded"
+ return "failed"
for detection in userdata.matched_face_detections:
if detection.name == self._guest_id:
@@ -910,10 +967,12 @@ def execute(self, userdata):
return "succeeded_sofa"
if len(userdata.empty_seat_detections) > 0:
- userdata.seat_position = PointStamped(
+ seat_position = PointStamped(
point=userdata.empty_seat_detections[0][0].point,
header=Header(frame_id="map"),
)
+ seat_position.point.z = 0.5
+ userdata.seat_position = seat_position
return "succeeded_chair"
return "failed"
@@ -994,7 +1053,20 @@ def execute(self, userdata):
smach.StateMachine.add(
f"GET_LOOK_POINT_{guest_to_introduce_to}",
GetLookPoint(guest_to_introduce_to),
- transitions={"succeeded": f"LOOK_AT_{guest_to_introduce_to}"},
+ transitions={
+ "succeeded": f"LOOK_AT_{guest_to_introduce_to}",
+ "failed": "LOOK_CENTRE",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_CENTRE",
+ PlayMotion(motion_name="look_centre"),
+ transitions={
+ "succeeded": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ "aborted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ "preempted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}",
+ },
)
smach.StateMachine.add(
@@ -1013,6 +1085,7 @@ def execute(self, userdata):
Introduce(
guest_to_introduce=guest_id,
guest_to_introduce_to=guest_to_introduce_to,
+ describe_features=guest_to_introduce_to != "host",
),
transitions={
"succeeded": (