feat: go to person.

skills/src/lasr_skills/__init__.py

@@ -6,6 +6,7 @@
 from .describe_people import DescribePeople
 from .look_to_point import LookToPoint
 from .find_person import FindPerson
+from .go_to_person import GoToPerson
 
 # from .go_to_location import GoToLocation
 # from .go_to_semantic_location import GoToSemanticLocation

skills/src/lasr_skills/find_person.py

@@ -8,7 +8,7 @@
 
 from typing import List
 
-from lasr_skills import Detect3D, LookToPoint
+from lasr_skills import Detect3D, LookToPoint, GoToPerson
 
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 
@@ -88,6 +88,11 @@ def __init__(self, waypoints: List[Pose]):
                             "failed": "continue",
                         },
                     )
+                    smach.StateMachine.add(
+                        "GO_TO_PERSON",
+                        GoToPerson(),
+                        transitions={"succeeded": "LOOK_AT_PERSON", "failed": "failed"},
+                    )
                     smach.StateMachine.add(
                         "LOOK_AT_PERSON",
                         LookToPoint(),

skills/src/lasr_skills/go_to_person.py

@@ -0,0 +1,60 @@
+import smach
+from smach_ros import SimpleActionState
+
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+
+
+from navigation_helpers import plan_to_radius
+
+from geometry_msgs.msg import PoseStamped, Header
+
+
+class GoToPerson(smach.StateMachine):
+
+    class PlanToPerson(smach.State):
+        def __init__(self):
+            smach.State.__init__(
+                self,
+                outcomes=["succeeded", "failed"],
+                input_keys=["location"],
+                output_keys=["target_location"],
+            )
+
+        def execute(self, userdata):
+            points = plan_to_radius(userdata.location, 0.5)
+            if not points:
+                return "failed"
+            userdata.target_location = points[0]
+
+    def __init__(self):
+        smach.StateMachine.__init__(
+            self,
+            outcomes=["succeeded", "failed"],
+            input_keys=["location"],
+        )
+
+        with self:
+
+            smach.StateMachine.add(
+                "PLAN_TO_PERSON",
+                self.PlanToPerson(),
+                transitions={"succeeded": "succeeded", "failed": "failed"},
+            )
+
+            smach.StateMachine.add(
+                "GO_TO_LOCATION",
+                SimpleActionState(
+                    "move_base",
+                    MoveBaseAction,
+                    goal_cb=lambda ud, _: MoveBaseGoal(
+                        target_pose=PoseStamped(
+                            pose=ud.target_location, header=Header(frame_id="map")
+                        )
+                    ),
+                    input_keys=["target_location"],
+                ),
+                transitions={
+                    "succeeded": "succeeded",
+                    "aborted": "failed",
+                },
+            )