WIP: FindNamedPerson.

skills/src/lasr_skills/find_named_person.py

@@ -3,37 +3,61 @@
 import smach
 from smach_ros import SimpleActionState
 
-from geometry_msgs.msg import Pose, PoseStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from std_msgs.msg import Header
 
 from typing import List
 
-from lasr_skills import Detect3D, LookToPoint, GoToPerson
+from lasr_skills import Detect3D, LookToPoint, GoToPerson, GoToLocation, AskAndListen
 
-from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 
+import navigation_helpers
 
-class FindPerson(smach.StateMachine):
+import rospy
+
+
+class FindNamedPerson(smach.StateMachine):
 
     class HandleDetections(smach.State):
         def __init__(self):
             smach.State.__init__(
                 self,
                 outcomes=["succeeded", "failed"],
                 input_keys=["detections_3d"],
-                output_keys=["point"],
+                output_keys=["person_point", "person_max_point"],
             )
 
         def execute(self, userdata):
             if len(userdata.detections_3d.detected_objects) == 0:
                 return "failed"
 
-            userdata.point = userdata.detections_3d.detected_objects[0].point
+            userdata.person_point = userdata.detections_3d.detected_objects[0].point
+            userdata.person_max_point = userdata.detections_3d.detected_objects[
+                0
+            ].max_point
             return "succeeded"
 
-    def __init__(self, waypoints: List[Pose], target_person: str):
+    class HandleResponse(smach.State):
+        def __init__(self):
+            smach.State.__init__(
+                self,
+                outcomes=["succeeded", "failed"],
+                input_keys=["transcribed_speech"],
+            )
+
+        def execute(self, userdata):
+            if "yes" in userdata.transcribed_speech.lower():
+                return "succeeded"
+            return "failed"
+
+    def __init__(self, waypoints: List[Pose], name: str):
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
-        self.waypoints = waypoints
+        current_point = rospy.wait_for_message(
+            "/amcl_pose", PoseWithCovarianceStamped
+        ).pose.pose
+        self.waypoints = navigation_helpers.min_hamiltonian_path(
+            current_point, waypoints
+        )
 
         with self:
             waypoint_iterator = smach.Iterator(
@@ -56,16 +80,7 @@ def __init__(self, waypoints: List[Pose], target_person: str):
 
                     smach.StateMachine.add(
                         "GO_TO_LOCATION",
-                        SimpleActionState(
-                            "move_base",
-                            MoveBaseAction,
-                            goal_cb=lambda ud, _: MoveBaseGoal(
-                                target_pose=PoseStamped(
-                                    pose=ud.location, header=Header(frame_id="map")
-                                )
-                            ),
-                            input_keys=["location"],
-                        ),
+                        GoToLocation(),
                         transitions={
                             "succeeded": "DETECT3D",
                             "preempted": "continue",
@@ -84,21 +99,42 @@ def __init__(self, waypoints: List[Pose], target_person: str):
                         "HANDLE_DETECTIONS",
                         self.HandleDetections(),
                         transitions={
-                            "succeeded": "LOOK_AT_PERSON",
+                            "succeeded": "GO_TO_PERSON",
                             "failed": "continue",
                         },
                     )
                     smach.StateMachine.add(
                         "GO_TO_PERSON",
                         GoToPerson(),
                         transitions={"succeeded": "LOOK_AT_PERSON", "failed": "failed"},
+                        remapping={"point": "person_point"},
                     )
                     smach.StateMachine.add(
                         "LOOK_AT_PERSON",
                         LookToPoint(),
-                        transitions={"succeeded": "succeeded"},
+                        transitions={
+                            "succeeded": "succeeded",
+                            "aborted": "failed",
+                            "preempted": "failed",
+                        },
+                        remapping={
+                            "point": "person_max_point",
+                        },
+                    )
+                    smach.StateMachine.add(
+                        "CHECK_NAME",
+                        AskAndListen(f"I'm looking for {name}. Are you {name}?"),
+                        transitions={
+                            "succeeded": "HANDLE_RESPONSE",
+                            "failed": "failed",
+                        },
                     )
-                    # TODO: Ask name of person
+                    smach.StateMachine.add(
+                        "HANDLE_RESPONSE",
+                        self.HandleResponse(),
+                        transitions={"succeeded": "succeeded", "failed": "continue"},
+                    )
+
                 waypoint_iterator.set_contained_state(
                     "CONTAINER_STATE", container_sm, loop_outcomes=["continue"]
                 )
@@ -115,15 +151,26 @@ def __init__(self, waypoints: List[Pose], target_person: str):
     from geometry_msgs.msg import Pose, Point, Quaternion
 
     rospy.init_node("find_person")
+
     sm = FindPerson(
         [
             Pose(
-                position=Point(4.5, 6.4, 0.0),
-                orientation=Quaternion(0.0, 0.0, 0.759, 0.650),
+                position=Point(1.446509335239015, -2.2460076897430974, 0.0),
+                orientation=Quaternion(
+                    0.0, 0.0, -0.6459923698644408, 0.763343866207703
+                ),
+            ),
+            Pose(
+                position=Point(-2.196456229125565, -0.27387058028873024, 0.0),
+                orientation=Quaternion(
+                    0.0, 0.0, 0.9778384065708362, 0.20936105329073992
+                ),
             ),
             Pose(
-                position=Point(3.2, 5.0, 0.0),
-                orientation=Quaternion(0.0, 0.0, -0.992, 0.121),
+                position=Point(-0.8129574905602319, -5.8536586556997445, 0.0),
+                orientation=Quaternion(
+                    0.0, 0.0, -0.9982013547068731, 0.05995044171116081
+                ),
             ),
         ]
     )