diff --git a/.gitmodules b/.gitmodules
index 32c10c10d..646f6f192 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
[submodule "common/third_party/leg_tracker"]
path = common/third_party/leg_tracker
- url = git@github.com:angusleigh/leg_tracker.git
+ url = https://github.com/LASR-at-Home/leg_tracker
diff --git a/common/navigation/lasr_person_following/launch/person_following.launch b/common/navigation/lasr_person_following/launch/person_following.launch
index e6fec1f19..6cdce47c5 100644
--- a/common/navigation/lasr_person_following/launch/person_following.launch
+++ b/common/navigation/lasr_person_following/launch/person_following.launch
@@ -1,8 +1,7 @@
-
-
-
+
+
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 55d2ee9fc..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,19 +26,23 @@
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
from scipy.spatial.transform import Rotation as R
import actionlib
-from pal_interaction_msgs.msg import TtsGoal, TtsAction
from lasr_speech_recognition_msgs.msg import (
TranscribeSpeechAction,
TranscribeSpeechGoal,
)
+from pal_interaction_msgs.msg import TtsGoal, TtsAction
+
+
+MAX_VISION_DIST: float = 5.0
+
class PersonFollower:
# Parameters
@@ -48,6 +52,8 @@ class PersonFollower:
_n_secs_static_plan_close: float
_new_goal_threshold: float
_stopping_distance: float
+ _vision_recovery_motions: List[str] = ["look_centre", "look_left", "look_right"]
+ _vision_recovery_attempts: int = 3
# State
_track_id: Union[None, int]
@@ -73,19 +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._n_secs_static_finished = n_secs_static_finished
- self._n_secs_static_plan_close = n_secs_static_plan_close
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
@@ -143,8 +147,12 @@ 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
except AttributeError:
return None
@@ -154,7 +162,7 @@ def _robot_pose_in_odom(self) -> Union[PoseStamped, None]:
return self._tf_pose(current_pose_stamped, "odom")
- def begin_tracking(self, ask: bool = False) -> bool:
+ def begin_tracking(self) -> bool:
"""
Chooses the closest person as the target
"""
@@ -195,23 +203,6 @@ def begin_tracking(self, ask: bool = False) -> bool:
if not closest_person:
return False
- if ask:
- if self._tts_client_available and self._transcribe_speech_client_available:
-
- # Ask if we should follow
- self._tts("Should I follow you? Please answer yes or no", wait=True)
-
- # listen
- self._transcribe_speech_client.send_goal_and_wait(
- TranscribeSpeechGoal()
- )
- transcription = self._transcribe_speech_client.get_result().sequence
-
- if "yes" not in transcription.lower():
- return False
-
- self._tts("I will follow you", wait=False)
-
self._track_id = closest_person.id
rospy.loginfo(f"Tracking person with ID {self._track_id}")
@@ -219,32 +210,20 @@ def begin_tracking(self, ask: bool = False) -> 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(ask=True) and not rospy.is_shutdown():
+ while not self.begin_tracking() and not rospy.is_shutdown():
rospy.loginfo("Recovering track...")
rospy.sleep(1)
- 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
+ self._tts("I see you again", wait=False)
- # 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)
+ return True
- # detect wave
+ def _detect_waving_person(self) -> DetectWaveResponse:
try:
pcl = rospy.wait_for_message("/xtion/depth_registered/points", PointCloud2)
req = DetectWaveRequest()
@@ -252,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(
@@ -329,8 +327,6 @@ def _check_finished(self) -> bool:
transcription = self._transcribe_speech_client.get_result().sequence
return "yes" in transcription.lower()
-
- return True
return True
def _get_pose_on_path(
@@ -386,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():
@@ -406,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
@@ -429,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)
@@ -459,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/launch/camera.launch b/common/vision/lasr_vision_yolov8/launch/camera.launch
index e0b478f32..44d11a1af 100644
--- a/common/vision/lasr_vision_yolov8/launch/camera.launch
+++ b/common/vision/lasr_vision_yolov8/launch/camera.launch
@@ -8,7 +8,6 @@
-
diff --git a/common/vision/lasr_vision_yolov8/launch/demo.launch b/common/vision/lasr_vision_yolov8/launch/demo.launch
index 1dc0b3ec9..4a98c9c8d 100644
--- a/common/vision/lasr_vision_yolov8/launch/demo.launch
+++ b/common/vision/lasr_vision_yolov8/launch/demo.launch
@@ -9,7 +9,6 @@
-
diff --git a/common/vision/lasr_vision_yolov8/launch/service.launch b/common/vision/lasr_vision_yolov8/launch/service.launch
index a29113497..6446b1f03 100644
--- a/common/vision/lasr_vision_yolov8/launch/service.launch
+++ b/common/vision/lasr_vision_yolov8/launch/service.launch
@@ -3,11 +3,9 @@
debug:=true preload:=['yolov8n.pt','yolov8n-seg.pt']
-
-
\ No newline at end of file
diff --git a/common/vision/lasr_vision_yolov8/nodes/service b/common/vision/lasr_vision_yolov8/nodes/service
index b949873b3..0821ba7df 100644
--- a/common/vision/lasr_vision_yolov8/nodes/service
+++ b/common/vision/lasr_vision_yolov8/nodes/service
@@ -5,6 +5,8 @@ import rospy
import rospkg
import lasr_vision_yolov8 as yolo
+from typing import Dict
+
from sensor_msgs.msg import Image
from visualization_msgs.msg import Marker
@@ -29,31 +31,27 @@ os.chdir(os.path.abspath(os.path.join(package_path, "models")))
rospy.init_node("yolov8_service")
# Determine variables
-DEBUG = rospy.get_param("~debug", False)
PRELOAD = rospy.get_param("~preload", [])
for model in PRELOAD:
yolo.load_model(model)
# Prepare publisher
-debug_publishers = {}
-if DEBUG:
- debug_publisher = rospy.Publisher("/yolov8/debug", Image, queue_size=1)
+debug_publishers: Dict[str, rospy.Publisher] = {}
+debug_publisher = rospy.Publisher("/yolov8/debug", Image, queue_size=1)
def detect(request: YoloDetectionRequest) -> YoloDetectionResponse:
"""
Hand off detection request to yolo library
"""
- debug_publisher = None
- if DEBUG:
- if request.dataset in debug_publishers:
- debug_publisher = debug_publishers[request.dataset]
- else:
- topic_name = re.sub(r"[\W_]+", "", request.dataset)
- debug_publisher = rospy.Publisher(
- f"/yolov8/debug/{topic_name}", Image, queue_size=1
- )
+ if request.dataset in debug_publishers:
+ debug_publisher = debug_publishers[request.dataset]
+ else:
+ topic_name = re.sub(r"[\W_]+", "", request.dataset)
+ debug_publisher = rospy.Publisher(
+ f"/yolov8/debug/{topic_name}", Image, queue_size=1
+ )
return yolo.detect(request, debug_publisher)
@@ -61,20 +59,18 @@ def detect_3d(request: YoloDetection3DRequest) -> YoloDetection3DResponse:
"""
Hand off detection request to yolo library
"""
- debug_inference_publisher, debug_point_publisher = None, None
- if DEBUG:
- if request.dataset in debug_publishers:
- debug_inference_publisher, debug_point_publisher = debug_publishers[
- request.dataset
- ]
- else:
- topic_name = re.sub(r"[\W_]+", "", request.dataset)
- debug_inference_publisher = rospy.Publisher(
- f"/yolov8/debug/{topic_name}", Image, queue_size=1
- )
- debug_point_publisher = rospy.Publisher(
- f"/yolov8/debug/points", Marker, queue_size=10
- )
+ if request.dataset in debug_publishers:
+ debug_inference_publisher, debug_point_publisher = debug_publishers[
+ request.dataset
+ ]
+ else:
+ topic_name = re.sub(r"[\W_]+", "", request.dataset)
+ debug_inference_publisher = rospy.Publisher(
+ f"/yolov8/debug/{topic_name}", Image, queue_size=1
+ )
+ debug_point_publisher = rospy.Publisher(
+ f"/yolov8/debug/points", Marker, queue_size=100
+ )
return yolo.detect_3d(request, debug_inference_publisher, debug_point_publisher)
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 f5d51a0c4..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
@@ -88,8 +88,7 @@ def detect(
detected_objects.append(detection)
# publish to debug topic
- if debug_publisher is not None:
- debug_publisher.publish(cv2_img.cv2_img_to_msg(result.plot()))
+ debug_publisher.publish(cv2_img.cv2_img_to_msg(result.plot()))
response = YoloDetectionResponse()
response.detected_objects = detected_objects
@@ -153,17 +152,15 @@ def detect_3d(
f"Detected point: {detection.point} of object {detection.name}"
)
- if debug_point_publisher is not None:
- 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)
# publish to debug topic
- if debug_inference_publisher is not None:
- debug_inference_publisher.publish(cv2_img.cv2_img_to_msg(result.plot()))
+ debug_inference_publisher.publish(cv2_img.cv2_img_to_msg(result.plot()))
response = YoloDetection3DResponse()
response.detected_objects = detected_objects
diff --git a/skills/launch/unit_test_describe_people.launch b/skills/launch/unit_test_describe_people.launch
index b14a085e3..f1b76530e 100644
--- a/skills/launch/unit_test_describe_people.launch
+++ b/skills/launch/unit_test_describe_people.launch
@@ -1,6 +1,5 @@
-
diff --git a/skills/src/lasr_skills/handover_object.py b/skills/src/lasr_skills/handover_object.py
index b68a0a741..29be3a7bd 100755
--- a/skills/src/lasr_skills/handover_object.py
+++ b/skills/src/lasr_skills/handover_object.py
@@ -95,12 +95,21 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True):
"LOOK_CENTRE",
PlayMotion(motion_name="look_centre"),
transitions={
- "succeeded": "REACH_ARM",
+ "succeeded": "SAY_REACH_ARM",
"aborted": "failed",
"preempted": "failed",
},
)
+ smach.StateMachine.add(
+ "SAY_REACH_ARM",
+ Say(text="Please step back, I am going to reach my arm out."),
+ transitions={
+ "succeeded": "REACH_ARM",
+ "aborted": "REACH_ARM",
+ "preempted": "REACH_ARM",
+ },
+ )
if vertical:
smach.StateMachine.add(
"REACH_ARM",
diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py
index 6defc9d99..c7663b361 100755
--- a/skills/src/lasr_skills/receive_object.py
+++ b/skills/src/lasr_skills/receive_object.py
@@ -31,6 +31,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True):
rosparam.upload_params(ns, param)
with self:
+
smach.StateMachine.add(
"CLEAR_OCTOMAP",
smach_ros.ServiceState("clear_octomap", Empty),
@@ -95,12 +96,22 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True):
"LOOK_CENTRE",
PlayMotion(motion_name="look_centre"),
transitions={
- "succeeded": "REACH_ARM",
+ "succeeded": "SAY_REACH_ARM",
"aborted": "failed",
"preempted": "failed",
},
)
+ smach.StateMachine.add(
+ "SAY_REACH_ARM",
+ Say(text="Please step back, I am going to reach my arm out."),
+ transitions={
+ "succeeded": "REACH_ARM",
+ "aborted": "REACH_ARM",
+ "preempted": "REACH_ARM",
+ },
+ )
+
if vertical:
smach.StateMachine.add(
"REACH_ARM",
diff --git a/tasks/carry_my_luggage/launch/setup.launch b/tasks/carry_my_luggage/launch/setup.launch
index d9d28141c..afdf7138b 100644
--- a/tasks/carry_my_luggage/launch/setup.launch
+++ b/tasks/carry_my_luggage/launch/setup.launch
@@ -15,7 +15,7 @@
-
+
diff --git a/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py b/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py
index 91e32c3e2..af78e206d 100644
--- a/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py
+++ b/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py
@@ -1,3 +1,4 @@
+import rospy
import smach
import smach_ros
from lasr_skills import (
@@ -8,8 +9,19 @@
HandoverObject,
)
from lasr_skills.vision import GetCroppedImage
+from lasr_skills import PlayMotion
from lasr_person_following.msg import FollowAction
+from std_msgs.msg import Empty
+from std_srvs.srv import Empty as EmptySrv
+
+from pal_startup_msgs.srv import (
+ StartupStart,
+ StartupStop,
+ StartupStartRequest,
+ StartupStopRequest,
+)
+
class CarryMyLuggage(smach.StateMachine):
@@ -33,8 +45,50 @@ def execute(self, userdata):
def __init__(self):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
-
with self:
+
+ def wait_cb(ud, msg):
+ rospy.loginfo("Received start signal")
+ return False
+
+ smach.StateMachine.add(
+ "WAIT_START",
+ smach_ros.MonitorState(
+ "/carry_my_luggage/start",
+ Empty,
+ wait_cb,
+ ),
+ transitions={
+ "valid": "WAIT_START",
+ "invalid": "STOP_HEAD_MANAGER",
+ "preempted": "WAIT_START",
+ },
+ )
+
+ smach.StateMachine.add(
+ "STOP_HEAD_MANAGER",
+ smach_ros.ServiceState(
+ "/pal_startup_control/stop",
+ StartupStop,
+ request=StartupStopRequest("head_manager"),
+ ),
+ transitions={
+ "succeeded": "LOOK_CENTRE",
+ "aborted": "LOOK_CENTRE",
+ "preempted": "LOOK_CENTRE",
+ },
+ )
+
+ smach.StateMachine.add(
+ "LOOK_CENTRE",
+ PlayMotion(motion_name="look_centre"),
+ transitions={
+ "succeeded": "WAIT_FOR_PERSON",
+ "aborted": "WAIT_FOR_PERSON",
+ "preempted": "WAIT_FOR_PERSON",
+ },
+ )
+
smach.StateMachine.add(
"WAIT_FOR_PERSON",
WaitForPerson(),
@@ -56,9 +110,7 @@ def __init__(self):
smach.StateMachine.add(
"GET_IMAGE",
- GetCroppedImage(
- object_name="person", crop_method="closest", use_mask=True
- ),
+ GetCroppedImage(object_name="person", method="closest", use_mask=True),
transitions={
"succeeded": "DETECT_POINTING_DIRECTION",
"failed": "failed",
@@ -70,7 +122,8 @@ def __init__(self):
DetectGesture(),
transitions={
"succeeded": "PROCESS_POINTING_DIRECTION",
- "failed": "SAY_FAILED_POINTING",
+ "failed": "GET_IMAGE",
+ "missing_keypoints": "GET_IMAGE",
},
)
@@ -79,42 +132,57 @@ def __init__(self):
CarryMyLuggage.ProcessPointingDirection(),
transitions={
"succeeded": "SAY_BAG",
- "failed": "SAY_FAILED_POINTING",
+ "failed": "GET_IMAGE",
},
)
smach.StateMachine.add(
- "SAY_FAILED_POINTING",
- Say(
- text="I could not detect the direction that you are pointing. I'll try again."
- ),
+ "SAY_BAG",
+ Say(format_str="I need you to give me the bag on your {}."),
transitions={
- "succeeded": "GET_IMAGE",
+ "succeeded": "START_HEAD_MANAGER",
"aborted": "failed",
"preempted": "failed",
},
+ remapping={"placeholders": "pointing_direction"},
)
smach.StateMachine.add(
- "SAY_BAG",
- Say(format_str="I need you to give me the bag on your {}."),
+ "START_HEAD_MANAGER",
+ smach_ros.ServiceState(
+ "/pal_startup_control/start",
+ StartupStart,
+ request=StartupStartRequest("head_manager", ""),
+ ),
transitions={
"succeeded": "RECEIVE_BAG",
- "aborted": "failed",
- "preempted": "failed",
+ "aborted": "RECEIVE_BAG",
+ "preempted": "RECEIVE_BAG",
},
- remapping={"placeholders": "pointing_direction"},
)
smach.StateMachine.add(
"RECEIVE_BAG",
ReceiveObject(object_name="bag", vertical=True),
transitions={
- "succeeded": "SAY_FOLLOW",
+ "succeeded": "CLEAR_COSTMAPS",
"failed": "failed",
},
)
+ smach.StateMachine.add(
+ "CLEAR_COSTMAPS",
+ smach_ros.ServiceState(
+ "/move_base/clear_costmaps",
+ EmptySrv,
+ ),
+ transitions={
+ "succeeded": "SAY_FOLLOW",
+ "aborted": "SAY_FOLLOW",
+ "preempted": "SAY_FOLLOW",
+ },
+ )
+
smach.StateMachine.add(
"SAY_FOLLOW",
Say(text="I will follow you now."),
diff --git a/tasks/coffee_shop/launch/core.launch b/tasks/coffee_shop/launch/core.launch
index e9e01006b..49c5bd78a 100644
--- a/tasks/coffee_shop/launch/core.launch
+++ b/tasks/coffee_shop/launch/core.launch
@@ -13,7 +13,6 @@
-
diff --git a/tasks/receptionist/config/6_floor_k.yaml b/tasks/receptionist/config/6_floor_k.yaml
new file mode 100644
index 000000000..ce87ffd5a
--- /dev/null
+++ b/tasks/receptionist/config/6_floor_k.yaml
@@ -0,0 +1,57 @@
+priors:
+ names:
+ - Adel
+ - Angel
+ - Axel
+ - Charlie
+ - Jane
+ - Jules
+ - Morgan
+ - Paris
+ - Robin
+ - Simone
+ drinks:
+ - cola
+ - iced tea
+ - juice pack
+ - milk
+ - orange juice
+ - red wine
+ - tropical juice
+
+# WAIT POSE KITCHEN:
+wait_pose:
+ position:
+ x: 8.245934441303595
+ y: 24.285935396834816
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: 0.08719394681831685
+ w: 0.9961913549304895
+
+# WAIT AREA KITCHEN:
+wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]]
+
+seat_pose:
+ position:
+ x: 7.439730848846352
+ y: 22.667057212175145
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: -0.6336387580418182
+ w: 0.7736290611832146
+
+search_motions: ["look_left", "look_right"]
+sofa_point:
+ x: 7.78
+ y: 20.1
+ z: 0.5
+seat_area: [[10.8, 20.2], [5.2, 18.9], [4.41, 21.6], [10.1, 23.1]]
+max_people_on_sofa: 2
+sofa_area: [[8.52, 20.1], [7.1, 19.8], [6.71, 21.1], [8.06, 21.5]]
+
+sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/config/arcade.yaml b/tasks/receptionist/config/arcade.yaml
new file mode 100644
index 000000000..c8300d5ca
--- /dev/null
+++ b/tasks/receptionist/config/arcade.yaml
@@ -0,0 +1,73 @@
+priors:
+ names:
+ - Adel
+ - Angel
+ - Axel
+ - Charlie
+ - Jane
+ - Jules
+ - Morgan
+ - Paris
+ - Robin
+ - Simone
+ drinks:
+ - cola
+ - iced tea
+ - juice pack
+ - milk
+ - orange juice
+ - red wine
+ - tropical juice
+
+
+#WAIT POSE LAB:
+wait_pose:
+ position:
+ x: -15.027493553116143
+ y: 8.731164058220495
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: -0.7865759968179794
+ w: 0.6174934827427752
+
+
+
+#556918144226074
+
+
+#0.478893309417269
+#0.8778731105321406
+
+
+#WAIT AREA LAB:
+# From robot POV: [top left, top right,bottom right, bottom left ]
+wait_area: [[-15.9, 8.19], [-17.2, 8.11], [-16.2, 11.2], [-14.9, 10.0]]
+
+#Where to position self for seating guests
+seat_pose:
+ position:
+ x: -13.659998294234812
+ y: 6.421172168922483
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: -0.6021594916272762
+ w: 0.7983758179223494
+
+
+
+search_motions: ["look_left", "look_right"]
+sofa_point:
+ x: -15.0
+ y: 4.21
+ z: 0.5
+
+seat_area: [[-12.2, 2.63], [-18.2, 4.61], [-17.4, 7.31], [-11.7, 5.06]]
+max_people_on_sofa: 2
+
+sofa_area: [[-13.8, 4.61], [-14.4, 3.54], [-15.85, 4.16], [-15.4, 5.33]]
+
+sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/config/bh_arcade.yaml b/tasks/receptionist/config/bh_arcade.yaml
new file mode 100644
index 000000000..ce87ffd5a
--- /dev/null
+++ b/tasks/receptionist/config/bh_arcade.yaml
@@ -0,0 +1,57 @@
+priors:
+ names:
+ - Adel
+ - Angel
+ - Axel
+ - Charlie
+ - Jane
+ - Jules
+ - Morgan
+ - Paris
+ - Robin
+ - Simone
+ drinks:
+ - cola
+ - iced tea
+ - juice pack
+ - milk
+ - orange juice
+ - red wine
+ - tropical juice
+
+# WAIT POSE KITCHEN:
+wait_pose:
+ position:
+ x: 8.245934441303595
+ y: 24.285935396834816
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: 0.08719394681831685
+ w: 0.9961913549304895
+
+# WAIT AREA KITCHEN:
+wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]]
+
+seat_pose:
+ position:
+ x: 7.439730848846352
+ y: 22.667057212175145
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: -0.6336387580418182
+ w: 0.7736290611832146
+
+search_motions: ["look_left", "look_right"]
+sofa_point:
+ x: 7.78
+ y: 20.1
+ z: 0.5
+seat_area: [[10.8, 20.2], [5.2, 18.9], [4.41, 21.6], [10.1, 23.1]]
+max_people_on_sofa: 2
+sofa_area: [[8.52, 20.1], [7.1, 19.8], [6.71, 21.1], [8.06, 21.5]]
+
+sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml
index 1079f3eec..9174b60ba 100644
--- a/tasks/receptionist/config/lab.yaml
+++ b/tasks/receptionist/config/lab.yaml
@@ -18,52 +18,52 @@ 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 LAB:
wait_pose:
position:
- x: 8.245934441303595
- y: 24.285935396834816
+ x: 2.4307581363168773
+ y: -1.661594410669659
z: 0.0
orientation:
x: 0.0
y: 0.0
- z: 0.08719394681831685
- w: 0.9961913549304895
+ z: 0.012769969339563213
+ w: 0.9999184606171978
+
+#556918144226074
+
#0.478893309417269
#0.8778731105321406
-# wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
-wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]]
+
+#WAIT AREA LAB:
+# From robot POV: [top left, top right,bottom right, bottom left ]
+wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
+
+#Where to position self for seating guests
seat_pose:
position:
- x: 7.439730848846352
- y: 22.667057212175145
+ x: 1.10349540659
+ y: 0.17802904565
z: 0.0
orientation:
x: 0.0
y: 0.0
- z: -0.6336387580418182
- w: 0.7736290611832146
+ z: 0.816644293927375
+ w: 0.577141314753899
search_motions: ["look_left", "look_right"]
-sofa_point:
- x: 7.78
- y: 20.1
- z: 0.5
-seat_area: [[10.8, 20.2], [5.2, 18.9], [4.41, 21.6], [10.1, 23.1]]
-max_people_on_sofa: 2
-sofa_area: [[8.52, 20.1], [7.1, 19.8], [6.71, 21.1], [8.06, 21.5]]
+sofa_point:
+ x: 0.4604474902153015
+ y: 1.877323865890503
+ z: 0.002471923828125
+
+seat_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]]
+max_people_on_sofa: 0
+sofa_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]]
sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/config/year_10_demo.yaml b/tasks/receptionist/config/year_10_demo.yaml
new file mode 100644
index 000000000..bfeb7a0ea
--- /dev/null
+++ b/tasks/receptionist/config/year_10_demo.yaml
@@ -0,0 +1,61 @@
+priors:
+ names:
+ - Adel
+ - Angel
+ - Axel
+ - Charlie
+ - Jane
+ - Jules
+ - Morgan
+ - Paris
+ - Robin
+ - Simone
+ drinks:
+ - cola
+ - iced tea
+ - juice pack
+ - milk
+ - orange juice
+ - red wine
+ - tropical juice
+
+# WAIT POSE YEAR 10 DEMO:
+wait_pose:
+ position:
+ x: 2.675762687658822
+ y: 2.3335752842925213
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: 0.77370619167885
+ w: 0.6335445753518931
+
+# WAIT AREA YEAR 10 DEMO:
+# From robot POV: [top left, top right,bottom right, bottom left ]
+wait_area: [[1.26,4.6],[2.80,4.82],[3.03,3.63],[1.53,3.4]]
+
+#Where to position self for seating guests
+seat_pose:
+ position:
+ x: 1.10349540659
+ y: 0.17802904565
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: 0.816644293927375
+ w: 0.577141314753899
+
+
+search_motions: ["look_left", "look_right"]
+sofa_point:
+ x: 0.4604474902153015
+ y: 1.877323865890503
+ z: 0.002471923828125
+seat_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]]
+max_people_on_sofa: 0
+#sofa_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]]
+sofa_area: [[0.83, 2.40], [1.16, 2.51], [1.23, 2.27], [0.91, 2.17]]
+
+sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch
index 901894d17..95a4c899f 100644
--- a/tasks/receptionist/launch/setup.launch
+++ b/tasks/receptionist/launch/setup.launch
@@ -1,7 +1,7 @@
-
+
@@ -15,12 +15,9 @@
-
-
-
-
+
diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py
index 4f387420b..aea5ec969 100644
--- a/tasks/receptionist/scripts/main.py
+++ b/tasks/receptionist/scripts/main.py
@@ -11,7 +11,10 @@
if __name__ == "__main__":
rospy.init_node("receptionist_robocup")
- wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+
+ wait_area_publisher = rospy.Publisher(
+ "/receptionist/wait_area", PolygonStamped, queue_size=1, latch=True
+ )
seat_area_publisher = rospy.Publisher(
"/receptionist/seat_area", PolygonStamped, queue_size=1, latch=True
@@ -21,6 +24,8 @@
"/receptionist/sofa_area", PolygonStamped, queue_size=1, latch=True
)
+ wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+
wait_pose = Pose(
position=Point(**wait_pose_param["position"]),
orientation=Quaternion(**wait_pose_param["orientation"]),
@@ -44,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)
@@ -58,6 +63,15 @@
sweep = rospy.get_param("/receptionist/sweep")
+ wait_area_publisher.publish(
+ PolygonStamped(
+ polygon=Polygon(
+ points=[Point(x=x, y=y, z=0.0) for (x, y) in wait_area.exterior.coords]
+ ),
+ header=Header(frame_id="map"),
+ )
+ )
+
seat_area_publisher.publish(
PolygonStamped(
polygon=Polygon(
diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py
index 74b1e40f6..f1f8be629 100755
--- a/tasks/receptionist/src/receptionist/state_machine.py
+++ b/tasks/receptionist/src/receptionist/state_machine.py
@@ -47,7 +47,7 @@ def __init__(
self.wait_area = wait_area
self.seat_pose = seat_pose
self.seat_area = seat_area
- # self.sweep_points = sweep_points
+
with self:
self.userdata.guest_data = {
"host": host_data,
@@ -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/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
index a9c81430a..b20bd862e 100644
--- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
+++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py
@@ -77,6 +77,7 @@ def execute(self, userdata: UserData) -> str:
outcome = "failed"
return outcome
+
class PostRecoveryDecision(smach.State):
def __init__(
self,
@@ -93,6 +94,7 @@ def __init__(
prior_data: Dict[str, List[str]] = rospy.get_param(param_key)
self._possible_names = [name.lower() for name in prior_data["names"]]
self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]]
+
def execute(self, userdata: UserData) -> str:
if not self._recovery_name_and_drink_required(userdata):
if userdata.guest_data[self._guest_id]["name"] == "unknown":
diff --git a/tasks/receptionist/src/receptionist/states/handle_guest.py b/tasks/receptionist/src/receptionist/states/handle_guest.py
index 8f9c39947..bbfe2b649 100644
--- a/tasks/receptionist/src/receptionist/states/handle_guest.py
+++ b/tasks/receptionist/src/receptionist/states/handle_guest.py
@@ -22,7 +22,9 @@ def __init__(self, guest_id: str):
with self:
smach.StateMachine.add(
f"GET_NAME_AND_DRINK_GUEST_{guest_id}",
- AskAndListen("What is your name and favourite drink?"),
+ AskAndListen(
+ "Please say 'Hi Tiago' when talking to me. What is your name and favourite drink?"
+ ),
transitions={
"succeeded": f"PARSE_NAME_AND_DRINK_GUEST_{guest_id}",
"failed": f"PARSE_NAME_AND_DRINK_GUEST_{guest_id}",
@@ -44,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 77777bf0c..54193976e 100644
--- a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
+++ b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
@@ -329,8 +329,9 @@ def identify(img_msg):
self._expected_detections[0]
)
else:
- rospy.logwarn("Failed to find expected guest")
- return "failed"
+ rospy.logwarn(
+ f"Failed to find expected guest {self._expected_detections[0]}"
+ )
print("+" * 50)
print(([(d.name, d.point) for d in filtered_face_detections]))
@@ -350,24 +351,26 @@ def identify(img_msg):
if detection.name
== self._expected_detections[1]
)
- # TODO: handle this being empty
+
other_detections = [
detection
for detection in filtered_face_detections
if detection.name == "unknown"
]
- if not other_detections:
- return "failed"
- furthest_unknown = max(
- other_detections,
- key=lambda x: _euclidian_distance(
- x.point, other.point
- ),
- )
- furthest_unknown.name = self._expected_detections[0]
+ if other_detections:
+ furthest_unknown = max(
+ other_detections,
+ key=lambda x: _euclidian_distance(
+ x.point, other.point
+ ),
+ )
+ furthest_unknown.name = (
+ self._expected_detections[0]
+ )
else:
- rospy.logwarn("Failed to find expected guest")
- return "failed"
+ rospy.logwarn(
+ f"Failed to find expected guest {self._expected_detections[1]}"
+ )
elif self._expected_detections[1] not in [
detection.name for detection in filtered_face_detections
] and self._expected_detections[0] in [
@@ -380,21 +383,25 @@ def identify(img_msg):
if detection.name
== self._expected_detections[0]
)
-
- furthest_unknown = max(
- [
- detection
- for detection in filtered_face_detections
- if detection.name == "unknown"
- ],
- key=lambda x: _euclidian_distance(
- x.point, other.point
- ),
- )
- furthest_unknown.name = self._expected_detections[1]
+ other_detections = [
+ detection
+ for detection in filtered_face_detections
+ if detection.name == "unknown"
+ ]
+ if other_detections:
+ furthest_unknown = max(
+ other_detections,
+ key=lambda x: _euclidian_distance(
+ x.point, other.point
+ ),
+ )
+ furthest_unknown.name = (
+ self._expected_detections[1]
+ )
else:
- rospy.logwarn("Failed to find expected guest")
- return "failed"
+ rospy.logwarn(
+ f"Failed to find expected guest {self._expected_detections[0]}"
+ )
print("-" * 50)
print(([(d.name, d.point) for d in filtered_face_detections]))
@@ -459,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"],
)
@@ -469,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"
@@ -511,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"
@@ -588,7 +599,17 @@ def execute(self, userdata):
smach.StateMachine.add(
"DETECT_PEOPLE_AND_SEATS",
DetectPeopleAndSeats(seating_area, motions),
- transitions={"succeeded": "HANDLE_RESPONSES", "failed": "failed"},
+ transitions={"succeeded": "LOOK_CENTRE", "failed": "failed"},
+ )
+
+ smach.StateMachine.add(
+ "LOOK_CENTRE",
+ PlayMotion(motion_name="look_centre"),
+ transitions={
+ "succeeded": "HANDLE_RESPONSES",
+ "aborted": "HANDLE_RESPONSES",
+ "preempted": "HANDLE_RESPONSES",
+ },
)
# Handle the responses from the detections
@@ -616,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
@@ -637,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]}"
- )
+ ),
},
)
@@ -654,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",
@@ -685,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",
},
)
@@ -696,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):
@@ -852,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:
@@ -893,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"
@@ -977,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(
@@ -996,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": (
diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py
index b46858b61..5f79f4d83 100644
--- a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py
+++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py
@@ -60,6 +60,7 @@ def speech_recovery(sentence):
print(f"final name: {handle_name(sentence_list, True)}")
print(f"final drink: {handle_drink(sentence_list, True)}")
+
def handle_name(sentence_list, last_resort):
result = handle_similar_spelt(sentence_list, available_names, 1)
if result != "unknown":
@@ -74,6 +75,7 @@ def handle_name(sentence_list, last_resort):
print("Last resort name")
return handle_closest_spelt(sentence_list, available_names)
+
def handle_drink(sentence_list, last_resort):
result = infer_second_drink(sentence_list)
if result != "unknown":
@@ -156,11 +158,13 @@ def handle_closest_spelt(sentence_list, choices):
def get_damerau_levenshtein_distance(word_1, word_2):
return jf.damerau_levenshtein_distance(word_1, word_2)
+
def get_levenshtein_soundex_distance(word_1, word_2):
soundex_word_1 = jf.soundex(word_1)
soundex_word_2 = jf.soundex(word_2)
return jf.levenshtein_distance(soundex_word_1, soundex_word_2)
+
# print(get_damerau_levenshtein_distance("juice", "shoes"))
# print(get_levenshtein_soundex_distance("juice", "shoes"))
diff --git a/tasks/receptionist/src/receptionist/states/speech_recovery.py b/tasks/receptionist/src/receptionist/states/speech_recovery.py
index 7a86814c4..956353f11 100644
--- a/tasks/receptionist/src/receptionist/states/speech_recovery.py
+++ b/tasks/receptionist/src/receptionist/states/speech_recovery.py
@@ -132,7 +132,7 @@ def _handle_name(self, sentence_list, last_resort):
return self._handle_closest_spelt(sentence_list, self._available_names)
def _handle_drink(self, sentence_list, last_resort):
- result = self._infer_second_drink(sentence_list)
+ result = self._infer_second_drink(sentence_list, last_resort)
if result != "unknown":
return result
result = self._handle_similar_spelt(sentence_list, self._available_drinks, 1)
@@ -150,7 +150,7 @@ def _handle_drink(self, sentence_list, last_resort):
return result
else:
sentence_list.append(result)
- return self._infer_second_drink(sentence_list)
+ return self._infer_second_drink(sentence_list, last_resort)
else:
if not last_resort:
return "unknown"
@@ -164,7 +164,7 @@ def _handle_drink(self, sentence_list, last_resort):
return closest_spelt
else:
sentence_list.append(closest_spelt)
- return self._infer_second_drink(closest_spelt)
+ return self._infer_second_drink(sentence_list, last_resort)
def _handle_similar_spelt(self, sentence_list, available_words, distance_threshold):
for input_word in sentence_list:
@@ -187,17 +187,20 @@ def _handle_similar_sound(self, sentence_list, available_words, distance_thresho
return available_word
return "unknown"
- def _infer_second_drink(self, sentence_list):
+ def _infer_second_drink(self, sentence_list, recover_juice=False):
for input_word in sentence_list:
if input_word == "juice":
- choices = ["pack", "orange", "tropical"]
- closest_word = self._handle_closest_spelt(sentence_list, choices)
- if closest_word == "pack":
- return "juice pack"
- elif closest_word == "orange":
- return "orange juice"
+ if recover_juice:
+ choices = ["pack", "orange", "tropical"]
+ closest_word = self._handle_closest_spelt(sentence_list, choices)
+ if closest_word == "pack":
+ return "juice pack"
+ elif closest_word == "orange":
+ return "orange juice"
+ else:
+ return "tropical juice"
else:
- return "tropical juice"
+ return "unknown"
for available_word in self._available_double_drinks:
if input_word == available_word:
return self._double_drinks_dict[input_word]