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 412c2a240..3780359b2 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 @@ -192,9 +192,11 @@ def detect_faces( try: faces = DeepFace.extract_faces( - cv_im, detector_backend="opencv", enforce_detection=True + cv_im, detector_backend="mtcnn", enforce_detection=True ) - except ValueError: + except ValueError as e: + rospy.loginfo(f"Error: {e}") + debug_publisher.publish(cv2_img.cv2_img_to_msg(cv_im)) return response for i, face in enumerate(faces): diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index 3d411227a..27f228733 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -54,6 +54,32 @@ seat_pose: y: 0.0 z: 0.816644293927375 w: 0.577141314753899 + +sweep_points: + - x : -0.9 + y : 1.5 + z: 0.5 + - x : 0.5 + y : 2.5 + z: 0.5 + - x: 0.17 + y: 1.66 + z: 0.5 + - x: 1.26 + y: 1.89 + z: 0.5 + - x : -0.9 + y : 1.5 + z: 1.0 + - x: 0.17 + y: 1.66 + z: 1.0 + - x: 1.26 + y: 1.89 + z: 1.0 + - x : 0.5 + y : 2.5 + z: 1.0 seat_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]] max_people_on_sofa: 1 diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 611c765e7..a9dc8cd5b 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -3,7 +3,7 @@ import smach_ros from lasr_vision_msgs.srv import Recognise -from geometry_msgs.msg import Pose, Point +from geometry_msgs.msg import Pose, Point, PointStamped from shapely.geometry import Polygon from lasr_skills import ( GoToLocation, @@ -33,9 +33,9 @@ def __init__( seat_area: Polygon, sofa_area: Polygon, host_data: dict, - # sweep_points: List[Tuple[float, float, float]], max_people_on_sofa: int = 3, face_detection_confidence: float = 0.2, + known_host: bool = False, ): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) self.wait_pose = wait_pose @@ -46,11 +46,12 @@ def __init__( with self: self.userdata.guest_data = { "host": host_data, - "guest1": {"name": ""}, - "guest2": {"name": ""}, + "guest1": {"name": "jared", "drink": "water", "detection": False}, + "guest2": {"name": "nicole", "drink": "tea", "detection": False}, } self.userdata.confidence = face_detection_confidence self.userdata.dataset = "receptionist" + self.userdata.seat_position = PointStamped() smach.StateMachine.add( "SAY_START", @@ -66,6 +67,8 @@ def __init__( First guest """ + self._goto_waiting_area(guest_id=1) + smach.StateMachine.add( "INTRODUCE_ROBOT", Say( @@ -78,14 +81,13 @@ def __init__( }, ) - self._goto_waiting_area(guest_id=1) - """ - GET GUEST ATTRIBUTES - """ + # """ + # GET GUEST ATTRIBUTES + # """ smach.StateMachine.add( "HANDLE_GUEST_1", - HandleGuest("guest1", True), + HandleGuest("guest1", not known_host), transitions={ "succeeded": "SAY_FOLLOW_GUEST_1", "failed": "SAY_FOLLOW_GUEST_1", @@ -95,38 +97,33 @@ def __init__( self._guide_guest(guest_id=1) smach.StateMachine.add( - "FIND_AND_LOOK_AT_HOST_1", - FindAndLookAt( - "host", - [ - [0.0, 0.0], - [-0.8, 0.0], - [0.8, 0.0], - ], + "SWEEP_GUEST_1", + PointCloudSweep( + sweep_points=sweep_points, ), transitions={ - "succeeded": "INTRODUCE_GUEST_1_TO_HOST", - "failed": "SAY_NO_HOST_1", + "succeeded": "RUN_AND_PROCESS_DETECTIONS_GUEST_1", + "failed": "failed", }, ) smach.StateMachine.add( - "SAY_NO_HOST_1", - Say(text="Wow, I can't find the host, I'm sure they're here"), + "RUN_AND_PROCESS_DETECTIONS_GUEST_1", + RunAndProcessDetections(seat_area), transitions={ - "succeeded": "LOOK_AT_WAITING_GUEST_1_1", - "preempted": "failed", - "aborted": "failed", + "succeeded": "FIND_AND_LOOK_AT_HOST_1", + "failed": "FIND_AND_LOOK_AT_HOST_1", }, + remapping={"empty_seat_point": "seat_position"}, ) + # Look at host smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_1_1", - PlayMotion(motion_name="look_very_left"), + "FIND_AND_LOOK_AT_HOST_1", + FindAndLookAt(guest_id=None), # assume host is the only person there? transitions={ "succeeded": "INTRODUCE_GUEST_1_TO_HOST", - "aborted": "INTRODUCE_GUEST_1_TO_HOST", - "preempted": "INTRODUCE_GUEST_1_TO_HOST", + "failed": "INTRODUCE_GUEST_1_TO_HOST", }, ) @@ -134,13 +131,13 @@ def __init__( "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", + "succeeded": "LOOK_AT_WAITING_GUEST_1_1", + "failed": "LOOK_AT_WAITING_GUEST_1_1", }, ) smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_1_2", + "LOOK_AT_WAITING_GUEST_1_1", PlayMotion(motion_name="look_very_left"), transitions={ "succeeded": "INTRODUCE_HOST_TO_GUEST_1", @@ -177,9 +174,9 @@ def __init__( }, ) - """ - Guest 2 - """ + # """ + # Guest 2 + # """ smach.StateMachine.add( "SAY_RETURN_WAITING_AREA", @@ -204,113 +201,76 @@ def __init__( self._guide_guest(guest_id=2) - # INTRODUCE GUEST 2 TO HOST - - """ - Logic should be as follows: - - 1. 3D cropped detection of seating area - 2. Pass RGB image to face recognition - 3. Extract named point(s) of face detections. - 4. Repeat until correct number of detections are made, or timeout. - 5. If host is found, look at host and introduce. - - - """ - smach.StateMachine.add( - "FIND_AND_LOOK_AT_HOST_2", - FindAndLookAt( - "host", - [ - [0.0, 0.0], - [-0.8, 0.0], - [0.8, 0.0], - ], + "SWEEP_GUEST_2", + PointCloudSweep( + sweep_points=sweep_points, ), transitions={ - "succeeded": "INTRODUCE_GUEST_2_TO_HOST", - "failed": "LOOK_AT_WAITING_GUEST_2_1", + "succeeded": "RUN_AND_PROCESS_DETECTIONS_GUEST_2", + "failed": "failed", }, ) smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_1", - PlayMotion(motion_name="look_very_left"), + "RUN_AND_PROCESS_DETECTIONS_GUEST_2", + RunAndProcessDetections(seat_area), transitions={ - "succeeded": "INTRODUCE_GUEST_2_TO_HOST", - "aborted": "INTRODUCE_GUEST_2_TO_HOST", - "preempted": "INTRODUCE_GUEST_2_TO_HOST", + "succeeded": "FIND_AND_LOOK_AT_HOST_2", + "failed": "FIND_AND_LOOK_AT_HOST_2", }, + remapping={"empty_seat_point": "seat_position"}, ) - # Check if host is sat where they are sat - # Look at the host - smach.StateMachine.add( - "INTRODUCE_GUEST_2_TO_HOST", - Introduce(guest_to_introduce="guest2", guest_to_introduce_to="host"), + "FIND_AND_LOOK_AT_HOST_2", + FindAndLookAt( + guest_id="host" if known_host else None, + mask=["guest1"] if not known_host else None, + ), transitions={ - "succeeded": "LOOK_AT_WAITING_GUEST_2_2", - "failed": "LOOK_AT_WAITING_GUEST_2_2", + "succeeded": "INTRODUCE_GUEST_2_TO_HOST", + "failed": "INTRODUCE_GUEST_2_TO_HOST", }, ) smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_2", - PlayMotion(motion_name="look_very_left"), + "INTRODUCE_GUEST_2_TO_HOST", + Introduce(guest_to_introduce="guest2", guest_to_introduce_to="host"), transitions={ - "succeeded": "INTRODUCE_GUEST_HOST_TO_GUEST_2", - "aborted": "INTRODUCE_GUEST_HOST_TO_GUEST_2", - "preempted": "INTRODUCE_GUEST_HOST_TO_GUEST_2", + "succeeded": "LOOK_AT_WAITING_GUEST_2_1", + "failed": "LOOK_AT_WAITING_GUEST_2_1", }, ) smach.StateMachine.add( - "INTRODUCE_GUEST_HOST_TO_GUEST_2", - Introduce(guest_to_introduce="host", guest_to_introduce_to="guest2"), + "LOOK_AT_WAITING_GUEST_2_1", + PlayMotion(motion_name="look_very_left"), transitions={ - "succeeded": "SAY_WAIT_GUEST_2_SEATED_GUEST_1", - "failed": "SAY_WAIT_GUEST_2_SEATED_GUEST_1", + "succeeded": "INTRODUCE_HOST_TO_GUEST_2", + "aborted": "INTRODUCE_HOST_TO_GUEST_2", + "preempted": "INTRODUCE_HOST_TO_GUEST_2", }, ) smach.StateMachine.add( - "SAY_WAIT_GUEST_2_SEATED_GUEST_1", - Say(text="Can the seated guest look into my eyes please."), + "INTRODUCE_HOST_TO_GUEST_2", + Introduce(guest_to_introduce="host", guest_to_introduce_to="guest2"), transitions={ "succeeded": "FIND_AND_LOOK_AT_GUEST_1", - "preempted": "FIND_AND_LOOK_AT_GUEST_1", - "aborted": "FIND_AND_LOOK_AT_GUEST_1", + "failed": "FIND_AND_LOOK_AT_GUEST_1", }, ) smach.StateMachine.add( "FIND_AND_LOOK_AT_GUEST_1", FindAndLookAt( - "guest1", - [ - [0.0, 0.0], - [-0.8, 0.0], - [0.8, 0.0], - ], + guest_id="guest1" if not known_host else None, + mask=["host"] if known_host else None, ), transitions={ "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1", - "failed": "LOOK_AT_WAITING_GUEST_2_3", - }, - ) - - # Check if host is sat where they are sat - # Look at the host - - smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_3", - PlayMotion(motion_name="look_very_left"), - transitions={ - "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1", - "aborted": "INTRODUCE_GUEST_2_TO_GUEST_1", - "preempted": "INTRODUCE_GUEST_2_TO_GUEST_1", + "failed": "INTRODUCE_GUEST_2_TO_GUEST_1", }, ) @@ -318,13 +278,13 @@ def __init__( "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", + "succeeded": "LOOK_AT_WAITING_GUEST_2_2", + "failed": "LOOK_AT_WAITING_GUEST_2_2", }, ) smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_4", + "LOOK_AT_WAITING_GUEST_2_2", PlayMotion(motion_name="look_very_left"), transitions={ "succeeded": "INTRODUCE_GUEST_1_TO_GUEST_2", @@ -333,7 +293,6 @@ def __init__( }, ) - # Look at guest 1 smach.StateMachine.add( "INTRODUCE_GUEST_1_TO_GUEST_2", Introduce(guest_to_introduce="guest1", guest_to_introduce_to="guest2"), @@ -471,84 +430,10 @@ 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. Can the seated host look into my eyes please." - ), + Say(text="Please wait here on my left."), transitions={ - "succeeded": f"FIND_AND_LOOK_AT_HOST_{guest_id}", + "succeeded": f"SWEEP_GUEST_{guest_id}", "preempted": "failed", "aborted": "failed", }, ) - - @smach.cb_interface(input_keys=["person_detection"], output_keys=["pointstamped"]) - def _compute_person_point(self, userdata): - """Computes the point to look at for the person. - - Args: - userdata (dict): Userdata containing the guest data. - - Returns: - PointStamped: The point to look at. - """ - pass - - def _detect_sweep_recognise_seat( - self, - expected_seated_guests: int, - ): - - # Looks to a list of given points in the seating area and obtains the transformed - # pointclouds - smach.StateMachine.add( - f"SWEEP_{expected_seated_guests}", - PointCloudSweep(self.sweep_points), - transitions={ - "succeeded": f"RUN_AND_PROCESS_DETECTIONS_{expected_seated_guests}", - "failed": "failed", - }, - ) - - # For each pointcloud, runs the 3D masked cropped detection service, looking for chairs - # and people, and removes any duplicate detections, and return empty chairs by removing - # any chair detections that are over people. - smach.StateMachine.add( - f"RUN_AND_PROCESS_DETECTIONS_{expected_seated_guests}", - RunAndProcessDetections(seating_area=self.seat_area), - transitions={ - "succeeded": f"RECOGNISE_{expected_seated_guests}", - "failed": "failed", - }, - ) - - smach.StateMachine.add( - f"RECOGNISE_{expected_seated_guests}", - RecognisePeople(self.userdata.dataset), - transitions={ - "succeeded": f"LOOK_AT_SEAT_{expected_seated_guests}", - "failed": "failed", - }, - ) - - # Look at guest or host - smach.StateMachine.add( - f"GET_POINTSTAMPED_{expected_seated_guests}", - PointCloudSweep.GetPointStamped(), - transitions={ - "succeeded": f"LOOK_AT_SEAT_{expected_seated_guests}", - "failed": "failed", - }, - ) - - smach.StateMachine.add( - f"LOOK_AT_SEAT_{expected_seated_guests}", - LookToPoint(pointstamped=None), - transitions={ - "succeeded": f"GET_POINTSTAMPED_{expected_seated_guests}", - "failed": "failed", - }, - ) - - # Handle sofas. - - # Seat guest. 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 aefa94d58..ff44382f5 100755 --- a/tasks/receptionist/src/receptionist/states/find_and_look_at.py +++ b/tasks/receptionist/src/receptionist/states/find_and_look_at.py @@ -17,241 +17,307 @@ from trajectory_msgs.msg import JointTrajectoryPoint from smach import CBState import rosservice +from std_msgs.msg import Header -PUBLIC_CONTAINER = False -try: - from pal_startup_msgs.srv import ( - StartupStart, - StartupStop, - StartupStartRequest, - StartupStopRequest, - ) -except ModuleNotFoundError: - PUBLIC_CONTAINER = True - - -def send_head_goal(_point, look_at_pub): - goal = FollowJointTrajectoryGoal() - goal.trajectory.joint_names = ["head_1_joint", "head_2_joint"] - point = JointTrajectoryPoint() - point.positions = _point - point.time_from_start = rospy.Duration(1) - goal.trajectory.points.append(point) - look_at_pub.send_goal(goal) +from typing import Union class FindAndLookAt(smach.StateMachine): - class GetLookPoint(smach.State): - def __init__(self, look_positions: List[List[float]]): - smach.State.__init__( - self, - outcomes=["succeeded", "failed"], - input_keys=[], - output_keys=["look_positions"], - ) - self.look_positions = look_positions - - def execute(self, userdata): - userdata.look_positions = self.look_positions - return "succeeded" - class GetPoint(smach.State): - def __init__(self): + class GetLookPoint(smach.State): + def __init__( + self, guest_id: Union[None, str] = None, mask: Union[None, List[str]] = None + ): smach.State.__init__( self, outcomes=["succeeded", "failed"], - input_keys=["point_index", "look_positions"], - output_keys=["pointstamped"], - ) - self.look_at_pub = actionlib.SimpleActionClient( - "/head_controller/follow_joint_trajectory", FollowJointTrajectoryAction + input_keys=["matched_face_detections"], + output_keys=["look_position"], ) + self.guest_id = guest_id + self.mask = mask def execute(self, userdata): - rospy.sleep(3.0) - _point = userdata.look_positions[userdata.point_index] - print(f"Looking at {_point}") - userdata.pointstamped = PointStamped( - point=Point(x=_point[0], y=_point[1], z=1.0) - ) - send_head_goal(_point, self.look_at_pub) - rospy.sleep(3.0) - - return "succeeded" - - def check_name(self, ud): - rospy.logwarn( - f"Checking name {self.guest_name_in} in detections {ud.deepface_detection}" - ) - if len(ud.deepface_detection) == 0: - return "no_detection" - for detection in ud.deepface_detection: - if ( - detection.name == self.guest_name_in - and detection.confidence > ud.confidence - ): - rospy.loginfo( - f"Detection {detection.name} has confidence {detection.confidence} > {ud.confidence}" - ) - return "succeeded" + if self.guest_id is not None: + for detection in userdata.matched_face_detections: + if detection.name == self.guest_id: + look_position = detection.point + look_position.z = 1.25 + userdata.look_position = PointStamped( + point=look_position, header=Header(frame_id="map") + ) + return "succeeded" + elif self.mask is not None: + for detection in userdata.matched_face_detections: + if detection.name not in self.mask: + look_position = detection.point + look_position.z = 1.25 + userdata.look_position = PointStamped( + point=look_position, header=Header(frame_id="map") + ) + return "succeeded" else: - rospy.loginfo( - f"Detection {detection.name} has confidence {detection.confidence} <= {ud.confidence}" - ) - return "failed" + if len(userdata.matched_face_detections) > 0: + look_position = userdata.matched_face_detections[0].point + look_position.z = 1.25 + userdata.look_position = PointStamped( + point=look_position, + header=Header(frame_id="map"), + ) + return "succeeded" + userdata.look_position = PointStamped() + return "failed" def __init__( - self, - guest_name_in: str, - look_positions: Union[List[List[float]], None] = None, + self, guest_id: Union[None, str] = None, mask: Union[None, List[str]] = None ): - self.guest_name_in = guest_name_in - smach.StateMachine.__init__( self, outcomes=["succeeded", "failed"], - input_keys=["dataset", "confidence", "guest_data"], - output_keys=[], + input_keys=["matched_face_detections"], ) - - if look_positions is None: - all_look_positions: List[List[float]] = [] - look_positions = [ - [0.0, 0.0], - [-1.0, 0.0], - [1.0, 0.0], - ] - - all_look_positions = look_positions - IS_SIMULATION = ( - "/pal_startup_control/start" not in rosservice.get_service_list() - ) - with self: + smach.StateMachine.add( "GET_LOOK_POINT", - self.GetLookPoint(all_look_positions), - transitions={"succeeded": "LOOK_ITERATOR", "failed": "failed"}, + self.GetLookPoint(guest_id=guest_id, mask=mask), + transitions={"succeeded": "LOOK_AT_PERSON", "failed": "failed"}, + remapping={"look_position": "pointstamped"}, ) - look_iterator = smach.Iterator( - outcomes=["succeeded", "failed"], - it=lambda: range(len(all_look_positions)), - it_label="point_index", - input_keys=["look_positions", "dataset", "confidence", "guest_data"], - output_keys=[], - exhausted_outcome="failed", - ) - with look_iterator: - container_sm = smach.StateMachine( - outcomes=["succeeded", "failed", "continue"], - input_keys=[ - "point_index", - "look_positions", - "dataset", - "confidence", - "guest_data", - ], - output_keys=[], - ) - with container_sm: - if not IS_SIMULATION: - if PUBLIC_CONTAINER: - rospy.logwarn( - "You are using a public container. The head manager will not be stopped during navigation." - ) - else: - smach.StateMachine.add( - "DISABLE_HEAD_MANAGER", - smach_ros.ServiceState( - "/pal_startup_control/stop", - StartupStop, - request=StartupStopRequest("head_manager"), - ), - transitions={ - "succeeded": "GET_POINT", - "aborted": "failed", - "preempted": "failed", - }, - ) - 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( - "GET_IMAGE", - GetPointCloud("/xtion/depth_registered/points"), - transitions={ - "succeeded": "RECOGNISE", - }, - remapping={"pcl_msg": "pcl_msg"}, - ) - smach.StateMachine.add( - "RECOGNISE", - smach_ros.ServiceState( - "/recognise", - Recognise, - input_keys=["pcl_msg", "dataset", "confidence"], - request_cb=lambda ud, _: RecogniseRequest( - image_raw=pcl_to_img_msg(ud.pcl_msg), - dataset=ud.dataset, - confidence=ud.confidence, - ), - response_slots=["detections"], - output_keys=["detections"], - ), - transitions={ - "succeeded": "CHECK_NAME", - "aborted": "failed", - "preempted": "failed", - }, - remapping={ - "pcl_msg": "pcl_msg", - "detections": "deepface_detection", - }, - ) - smach.StateMachine.add( - "CHECK_NAME", - CBState( - self.check_name, - outcomes=["succeeded", "failed", "no_detection"], - input_keys=[ - "deepface_detection", - "confidence", - "guest_data", - ], - ), - transitions={ - "succeeded": "LOOK_AT_PERSON", - "failed": "continue", - "no_detection": "continue", - }, - ) - smach.StateMachine.add( - "LOOK_AT_PERSON", - LookAtPerson(filter=True), - transitions={ - "succeeded": "succeeded", - "no_detection": "continue", - "failed": "failed", - }, - ) - look_iterator.set_contained_state( - "CONTAINER_STATE", container_sm, loop_outcomes=["continue"] - ) smach.StateMachine.add( - "LOOK_ITERATOR", - look_iterator, - transitions={"succeeded": "succeeded", "failed": "failed"}, + "LOOK_AT_PERSON", + LookToPoint(), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "timed_out": "failed", + }, ) + + +# def send_head_goal(_point, look_at_pub): +# goal = FollowJointTrajectoryGoal() +# goal.trajectory.joint_names = ["head_1_joint", "head_2_joint"] +# point = JointTrajectoryPoint() +# point.positions = _point +# point.time_from_start = rospy.Duration(1) +# goal.trajectory.points.append(point) +# look_at_pub.send_goal(goal) + + +# class FindAndLookAt(smach.StateMachine): +# class GetLookPoint(smach.State): +# def __init__(self, look_positions: List[List[float]]): +# smach.State.__init__( +# self, +# outcomes=["succeeded", "failed"], +# input_keys=[], +# output_keys=["look_positions"], +# ) +# self.look_positions = look_positions + +# def execute(self, userdata): +# userdata.look_positions = self.look_positions +# return "succeeded" + +# class GetPoint(smach.State): +# def __init__(self): +# smach.State.__init__( +# self, +# outcomes=["succeeded", "failed"], +# input_keys=["point_index", "look_positions"], +# output_keys=["pointstamped"], +# ) +# self.look_at_pub = actionlib.SimpleActionClient( +# "/head_controller/follow_joint_trajectory", FollowJointTrajectoryAction +# ) + +# def execute(self, userdata): +# rospy.sleep(3.0) +# _point = userdata.look_positions[userdata.point_index] +# print(f"Looking at {_point}") +# userdata.pointstamped = PointStamped( +# point=Point(x=_point[0], y=_point[1], z=1.0) +# ) +# send_head_goal(_point, self.look_at_pub) +# rospy.sleep(3.0) + +# return "succeeded" + +# def check_name(self, ud): +# rospy.logwarn( +# f"Checking name {self.guest_name_in} in detections {ud.deepface_detection}" +# ) +# if len(ud.deepface_detection) == 0: +# return "no_detection" +# for detection in ud.deepface_detection: +# if ( +# detection.name == self.guest_name_in +# and detection.confidence > ud.confidence +# ): +# rospy.loginfo( +# f"Detection {detection.name} has confidence {detection.confidence} > {ud.confidence}" +# ) +# return "succeeded" +# else: +# rospy.loginfo( +# f"Detection {detection.name} has confidence {detection.confidence} <= {ud.confidence}" +# ) +# return "failed" + +# def __init__( +# self, +# 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"], +# output_keys=[], +# ) + +# if look_positions is None: +# all_look_positions: List[List[float]] = [] +# look_positions = [ +# [0.0, 0.0], +# [-1.0, 0.0], +# [1.0, 0.0], +# ] + +# all_look_positions = look_positions +# IS_SIMULATION = ( +# "/pal_startup_control/start" not in rosservice.get_service_list() +# ) + +# with self: +# smach.StateMachine.add( +# "GET_LOOK_POINT", +# self.GetLookPoint(all_look_positions), +# transitions={"succeeded": "LOOK_ITERATOR", "failed": "failed"}, +# ) +# look_iterator = smach.Iterator( +# outcomes=["succeeded", "failed"], +# it=lambda: range(len(all_look_positions)), +# it_label="point_index", +# input_keys=["look_positions", "dataset", "confidence", "guest_data"], +# output_keys=[], +# exhausted_outcome="failed", +# ) +# with look_iterator: +# container_sm = smach.StateMachine( +# outcomes=["succeeded", "failed", "continue"], +# input_keys=[ +# "point_index", +# "look_positions", +# "dataset", +# "confidence", +# "guest_data", +# ], +# output_keys=[], +# ) +# with container_sm: +# if not IS_SIMULATION: +# if PUBLIC_CONTAINER: +# rospy.logwarn( +# "You are using a public container. The head manager will not be stopped during navigation." +# ) +# else: +# smach.StateMachine.add( +# "DISABLE_HEAD_MANAGER", +# smach_ros.ServiceState( +# "/pal_startup_control/stop", +# StartupStop, +# request=StartupStopRequest("head_manager"), +# ), +# transitions={ +# "succeeded": "GET_POINT", +# "aborted": "failed", +# "preempted": "failed", +# }, +# ) +# 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( +# "GET_IMAGE", +# GetPointCloud("/xtion/depth_registered/points"), +# transitions={ +# "succeeded": "RECOGNISE", +# }, +# remapping={"pcl_msg": "pcl_msg"}, +# ) +# smach.StateMachine.add( +# "RECOGNISE", +# smach_ros.ServiceState( +# "/recognise", +# Recognise, +# input_keys=["pcl_msg", "dataset", "confidence"], +# request_cb=lambda ud, _: RecogniseRequest( +# image_raw=pcl_to_img_msg(ud.pcl_msg), +# dataset=ud.dataset, +# confidence=ud.confidence, +# ), +# response_slots=["detections"], +# output_keys=["detections"], +# ), +# transitions={ +# "succeeded": "CHECK_NAME", +# "aborted": "failed", +# "preempted": "failed", +# }, +# remapping={ +# "pcl_msg": "pcl_msg", +# "detections": "deepface_detection", +# }, +# ) +# smach.StateMachine.add( +# "CHECK_NAME", +# CBState( +# self.check_name, +# outcomes=["succeeded", "failed", "no_detection"], +# input_keys=[ +# "deepface_detection", +# "confidence", +# "guest_data", +# ], +# ), +# transitions={ +# "succeeded": "LOOK_AT_PERSON", +# "failed": "continue", +# "no_detection": "continue", +# }, +# ) +# smach.StateMachine.add( +# "LOOK_AT_PERSON", +# LookAtPerson(filter=True), +# transitions={ +# "succeeded": "succeeded", +# "no_detection": "continue", +# "failed": "failed", +# }, +# ) +# look_iterator.set_contained_state( +# "CONTAINER_STATE", container_sm, loop_outcomes=["continue"] +# ) +# smach.StateMachine.add( +# "LOOK_ITERATOR", +# look_iterator, +# transitions={"succeeded": "succeeded", "failed": "failed"}, +# ) diff --git a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py index ea718a2ef..87e94cf72 100755 --- a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py +++ b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py @@ -13,6 +13,12 @@ CroppedDetection, ) from lasr_vision_msgs.msg import CDRequest, CDResponse +from lasr_vision_msgs.srv import ( + Recognise, + RecogniseRequest, + DetectFaces, + DetectFacesRequest, +) class RunAndProcessDetections(smach.StateMachine): @@ -28,7 +34,7 @@ def __init__( output_keys=[ "empty_seat_detections", "people_detections", - "empty_seat_point", + "matched_face_detections", ], ) self._detection_service = detection_service @@ -39,7 +45,7 @@ def __init__( with self: smach.StateMachine.add( - "RunDetections", + "RUN_DETECTIONS", self.RunDetections( detection_client=self._detection_client, seating_area=self.seating_area, @@ -51,22 +57,27 @@ def __init__( return_sensor_reading=False, object_names=["person", "chair"], ), - transitions={"succeeded": "ProcessDetections", "failed": "failed"}, + transitions={"succeeded": "PROCESS_DETECTIONS", "failed": "failed"}, remapping={ "transformed_pointclouds": "transformed_pointclouds", "detections": "detections", }, ) smach.StateMachine.add( - "ProcessDetections", + "PROCESS_DETECTIONS", self.ProcessDetections(closesness_distance=0.5, overlap_threshold=0.8), - transitions={"succeeded": "succeeded", "failed": "failed"}, + transitions={"succeeded": "RUN_FACE_DETECTIONS", "failed": "failed"}, remapping={ "detections": "detections", "empty_seat_detections": "empty_seat_detections", "people_detections": "people_detections", }, ) + smach.StateMachine.add( + "RUN_FACE_DETECTIONS", + self.RunFaceDetections(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) class RunDetections(smach.State): def __init__( @@ -136,6 +147,56 @@ def execute(self, userdata): return "failed" return "succeeded" + class RunFaceDetections(smach.State): + def __init__(self): + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["transformed_pointclouds", "people_detections"], + output_keys=["matched_face_detections"], + ) + + self._dataset = "receptionist" + self._recognise = rospy.ServiceProxy("/recognise", Recognise) + self._recognise.wait_for_service() + + self._detect_faces = rospy.ServiceProxy("/detect_faces", DetectFaces) + self._detect_faces.wait_for_service() + + def execute(self, userdata): + try: + face_detections = [] + for person_detection in userdata.people_detections: + rospy.loginfo( + f"Running face detection on {person_detection[0].name}" + ) + img_msg = person_detection[1] + face_detection_result = self._detect_faces(img_msg) + if len(face_detection_result.detections) > 0: + + recognise_result = self._recognise(img_msg, self._dataset, 0.2) + if len(recognise_result.detections) > 0: + + face_detection = person_detection + face_detection[0].name = recognise_result.detections[0].name + face_detections.append(face_detection[0]) + rospy.loginfo( + f"Recognised face as {recognise_result.detections[0].name}" + ) + else: + face_detection = person_detection + face_detection[0].name = "unknown" + face_detections.append(face_detection[0]) + rospy.loginfo("Detected face but could not recognise it.") + else: + rospy.loginfo("No face detected.") + + userdata.matched_face_detections = face_detections + except rospy.ServiceException as e: + rospy.logwarn(f"Unable to perform face detection. ({str(e)})") + return "failed" + return "succeeded" + class ProcessDetections(smach.State): def __init__( self, closesness_distance: float = 0.15, overlap_threshold: float = 0.8 @@ -147,7 +208,6 @@ def __init__( output_keys=[ "empty_seat_detections", "people_detections", - "empty_seat_point", ], ) self.closesness_distance = closesness_distance @@ -190,15 +250,15 @@ def execute(self, userdata): ) rospy.loginfo(f"Found chair at: {detection.point}") elif detection.name == "person": - for added_person in people_detections: - if ( - self._euclidian_distance( - added_person[0].point, - detection.point, - ) - < self.closesness_distance - ): - add_person = False + # for added_person in people_detections: + # if ( + # self._euclidian_distance( + # added_person[0].point, + # detection.point, + # ) + # < self.closesness_distance + # ): + # add_person = False if add_person: people_detections.append( (detection, detection_set.cropped_imgs[index]) @@ -235,13 +295,6 @@ def execute(self, userdata): f"Found {len(filtered_seats)} empty seats and {len(people_detections)} people." ) - if len(filtered_seats) > 0: - userdata.empty_seat_point = PointStamped( - point=filtered_seats[0][0].point, header=Header(frame_id="map") - ) - else: - return "failed" - except Exception as e: rospy.logerr(f"Failed to process detections: {str(e)}") return "failed" diff --git a/tasks/receptionist/src/receptionist/states/seat_guest.py b/tasks/receptionist/src/receptionist/states/seat_guest.py index b36cf2aba..e4ad35423 100755 --- a/tasks/receptionist/src/receptionist/states/seat_guest.py +++ b/tasks/receptionist/src/receptionist/states/seat_guest.py @@ -103,6 +103,27 @@ def execute(self, userdata) -> str: return "failed" + class SelectSeat(smach.State): + + def __init__(self): + + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["empty_seat_detections"], + output_keys=["seat_position"], + ) + + def execute(self, userdata) -> str: + if len(userdata.empty_seat_detections) == 0: + return "failed" + + seat = userdata.empty_seat_detections[0][0] + userdata.seat_position = PointStamped( + point=seat.point, header=Header(frame_id="map") + ) + return "succeeded" + def __init__( self, seat_area: Polygon, @@ -110,65 +131,75 @@ def __init__( sweep_points: List[Point], max_people_on_sofa: int, ): - smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + smach.StateMachine.__init__( + self, outcomes=["succeeded", "failed"], input_keys=["empty_seat_detections"] + ) with self: # TODO: stop doing this self.userdata.people_detections = [] self.userdata.seat_detections = [] - self.userdata.seat_position = PointStamped() + # self.userdata.seat_position = PointStamped() + + # smach.StateMachine.add( + # "LOOK_CENTRE", + # PlayMotion(motion_name="look_centre"), + # transitions={ + # "succeeded": "DETECT_SOFA", + # "aborted": "DETECT_SOFA", + # "preempted": "DETECT_SOFA", + # }, + # ) + + # smach.StateMachine.add( + # "DETECT_SOFA", + # Detect3DInArea(sofa_area, filter=["person"]), + # transitions={"succeeded": "CHECK_SOFA", "failed": "failed"}, + # ) + # smach.StateMachine.add( + # "CHECK_SOFA", + # self.ProcessDetectionsSofa(max_people_on_sofa), + # transitions={ + # "full_sofa": "SWEEP", + # "has_free_space": "SAY_SIT_ON_SOFA", + # }, + # ) + + # smach.StateMachine.add( + # "SAY_SIT_ON_SOFA", + # Say("Please sit on the sofa."), + # transitions={ + # "succeeded": "WAIT_FOR_GUEST_SEAT", + # "aborted": "failed", + # "preempted": "failed", + # }, + # ) + + # smach.StateMachine.add( + # "SWEEP", + # PointCloudSweep( + # sweep_points=sweep_points, + # ), + # transitions={ + # "succeeded": "RUN_AND_PROCESS_DETECTIONS", + # "failed": "failed", + # }, + # ) + + # smach.StateMachine.add( + # "RUN_AND_PROCESS_DETECTIONS", + # RunAndProcessDetections(seat_area), + # transitions={"succeeded": "LOOK_TO_POINT", "failed": "failed"}, + # remapping={"empty_seat_point": "seat_position"}, + # ) smach.StateMachine.add( - "LOOK_CENTRE", - PlayMotion(motion_name="look_centre"), + "SELECT_SEAT", + self.SelectSeat(), transitions={ - "succeeded": "DETECT_SOFA", - "aborted": "DETECT_SOFA", - "preempted": "DETECT_SOFA", - }, - ) - - smach.StateMachine.add( - "DETECT_SOFA", - Detect3DInArea(sofa_area, filter=["person"]), - transitions={"succeeded": "CHECK_SOFA", "failed": "failed"}, - ) - smach.StateMachine.add( - "CHECK_SOFA", - self.ProcessDetectionsSofa(0), - transitions={ - "full_sofa": "SWEEP", - "has_free_space": "SAY_SIT_ON_SOFA", - }, - ) - - smach.StateMachine.add( - "SAY_SIT_ON_SOFA", - Say("Please sit on the sofa."), - transitions={ - "succeeded": "WAIT_FOR_GUEST_SEAT", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "SWEEP", - PointCloudSweep( - sweep_points=sweep_points, - ), - transitions={ - "succeeded": "RUN_AND_PROCESS_DETECTIONS", + "succeeded": "LOOK_TO_POINT", "failed": "failed", }, ) - - smach.StateMachine.add( - "RUN_AND_PROCESS_DETECTIONS", - RunAndProcessDetections(seat_area), - transitions={"succeeded": "LOOK_TO_POINT", "failed": "failed"}, - remapping={"empty_seat_point": "seat_position"}, - ) - smach.StateMachine.add( "LOOK_TO_POINT", LookToPoint(),