fix(GPSR): use beacons as waypoints (#165)

* feat: FindPerson (in room)

* fix: pathing

* chore: gitignore and gitkeep

* fix: some issues with SM factory.

* fix: iteration over beacons

skills/src/lasr_skills/find_named_person.py

@@ -4,10 +4,10 @@
 from lasr_skills import Detect3D, GoToLocation, AskAndListen
 import navigation_helpers
 
-from geometry_msgs.msg import Pose, PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion
 
 
-from typing import List
+from typing import List, Union
 
 
 class FindNamedPerson(smach.StateMachine):
@@ -85,11 +85,35 @@ def execute(self, userdata):
                 return "succeeded"
             return "failed"
 
-    def __init__(self, waypoints: List[Pose], name: str):
+    def __init__(
+        self,
+        name: str,
+        waypoints: Union[List[Pose], None] = None,
+        location_param: Union[str, None] = None,
+    ):
         smach.StateMachine.__init__(
             self, outcomes=["succeeded", "failed"], output_keys=["person_point"]
         )
 
+        if waypoints is None and location_param is None:
+            raise ValueError("Either waypoints or location_param must be provided")
+
+        if waypoints is None:
+            waypoints_to_iterate: List[Pose] = []
+
+            room = rospy.get_param(location_param)
+            beacons = room["beacons"]
+            for beacon in beacons:
+                waypoint: Pose = Pose(
+                    position=Point(**beacons[beacon]["near_pose"]["position"]),
+                    orientation=Quaternion(
+                        **beacons[beacon]["near_pose"]["orientation"]
+                    ),
+                )
+                waypoints_to_iterate.append(waypoint)
+        else:
+            waypoints_to_iterate: List[Pose] = waypoints
+
         with self:
 
             smach.StateMachine.add(
@@ -100,13 +124,13 @@ def __init__(self, waypoints: List[Pose], name: str):
 
             smach.StateMachine.add(
                 "COMPUTE_PATH",
-                self.ComputePath(waypoints),
+                self.ComputePath(waypoints_to_iterate),
                 transitions={"succeeded": "WAYPOINT_ITERATOR", "failed": "failed"},
             )
 
             waypoint_iterator = smach.Iterator(
                 outcomes=["succeeded", "failed"],
-                it=lambda: range(len(waypoints)),
+                it=lambda: range(len(waypoints_to_iterate)),
                 it_label="location_index",
                 input_keys=["waypoints"],
                 output_keys=["person_point"],

tasks/gpsr/data/command_data/.gitignore

@@ -0,0 +1 @@
+*.txt

tasks/gpsr/data/command_data/.gitkeep

@@ -0,0 +1 @@
+

tasks/gpsr/data/faiss_indices/.gitignore

@@ -0,0 +1 @@
+*.index

tasks/gpsr/data/faiss_indices/.gitkeep

@@ -0,0 +1 @@
+

tasks/gpsr/src/gpsr/state_machine_factory.py

@@ -26,18 +26,19 @@ def build_state_machine(parsed_command: Dict) -> smach.StateMachine:
         smach.StateMachine: paramaterized state machine ready to be executed.
     """
     command_verbs: List[str] = parsed_command["commands"]
-    command_params: List[Dict] = parsed_command["params"]
+    command_params: List[Dict] = parsed_command["command_params"]
     sm = smach.StateMachine(outcomes=["succeeded", "failed"])
     with sm:
         for command_verb, command_param in zip(command_verbs, command_params):
             if command_verb == "greet":
                 if "name" in command_param:
-                    location_param = (
-                        f"/gpsr/arena/rooms/{command_param['location']}/pose"
+                    location_param_room = (
+                        f"/gpsr/arena/rooms/{command_param['location']}"
                     )
+                    location_param_pose = f"{location_param_room}/pose"
                     sm.add(
                         f"STATE_{increment_state_count()}",
-                        GoToLocation(location_param=location_param),
+                        GoToLocation(location_param=location_param_pose),
                         transitions={
                             "succeeded": f"STATE_{STATE_COUNT + 1}",
                             "failed": "failed",
@@ -46,7 +47,8 @@ def build_state_machine(parsed_command: Dict) -> smach.StateMachine:
                     sm.add(
                         f"STATE_{increment_state_count()}",
                         FindNamedPerson(
-                            name=command_param["name"], location_param=location_param
+                            name=command_param["name"],
+                            location_param=location_param_room,
                         ),
                         transitions={
                             "succeeded": f"STATE_{STATE_COUNT + 1}",
@@ -66,7 +68,7 @@ def build_state_machine(parsed_command: Dict) -> smach.StateMachine:
                     sm.add(
                         f"STATE_{increment_state_count()}",
                         Talk(command_param["talk"]),
-                        transitions={"succeeded": "succeded", "failed": "failed"},
+                        transitions={"succeeded": "succeeded", "failed": "failed"},
                     )
                 else:
                     raise ValueError(

tasks/gpsr/src/gpsr/states/talk.py

@@ -65,6 +65,10 @@ def __init__(self, talk_phrase: str):
             smach.StateMachine.add(
                 "SAY_RESPONSE",
                 Say(),
-                transitions={"succeeded": "succeeded", "failed": "failed"},
+                transitions={
+                    "succeeded": "succeeded",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
                 remapping={"text": "response"},
             )