GPSR configs & GoTo(Semantic)Location (#163)

* feat: Bordeaux example config.

* feat: go to named location.

* fix: correct param

* fix: distinguish between approach pose and search pose for beacons.

* fix: correct Pose.

* feat: update template.

* feat: lab setup

* refactor: remove GoToSemanticLocation

skills/src/lasr_skills/__init__.py

@@ -13,7 +13,6 @@
 from .ask_and_listen import AskAndListen
 from .find_person import FindPerson
 from .find_named_person import FindNamedPerson
-from .go_to_semantic_location import GoToSemanticLocation
 from .receive_object import ReceiveObject
 from .handover_object import HandoverObject
 from .ask_and_listen import AskAndListen

skills/src/lasr_skills/go_to_location.py

@@ -3,7 +3,7 @@
 import rospy
 import rosservice
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from geometry_msgs.msg import Pose, PoseStamped
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 from std_msgs.msg import Header
 
 from typing import Union
@@ -25,9 +25,13 @@
 
 class GoToLocation(smach.StateMachine):
 
-    def __init__(self, location: Union[Pose, None] = None):
+    def __init__(
+        self,
+        location: Union[Pose, None] = None,
+        location_param: Union[str, None] = None,
+    ):
 
-        if location is not None:
+        if location is not None or location_param is not None:
             super(GoToLocation, self).__init__(outcomes=["succeeded", "failed"])
         else:
             super(GoToLocation, self).__init__(
@@ -94,6 +98,38 @@ def __init__(self, location: Union[Pose, None] = None):
                         "preempted": "failed",
                     },
                 )
+            elif location_param is not None:
+                smach.StateMachine.add(
+                    "GO_TO_LOCATION",
+                    smach_ros.SimpleActionState(
+                        "move_base",
+                        MoveBaseAction,
+                        goal=MoveBaseGoal(
+                            target_pose=PoseStamped(
+                                pose=Pose(
+                                    position=Point(
+                                        **rospy.get_param(f"{location_param}/position")
+                                    ),
+                                    orientation=Quaternion(
+                                        **rospy.get_param(
+                                            f"{location_param}/orientation"
+                                        )
+                                    ),
+                                ),
+                                header=Header(frame_id="map"),
+                            )
+                        ),
+                    ),
+                    transitions={
+                        "succeeded": (
+                            "DISABLE_HEAD_MANAGER"
+                            if not IS_SIMULATION
+                            else "RAISE_BASE"
+                        ),
+                        "aborted": "failed",
+                        "preempted": "failed",
+                    },
+                )
             else:
                 smach.StateMachine.add(
                     "GO_TO_LOCATION",

tasks/gpsr/config/bordeaux_example.yaml

@@ -0,0 +1,36 @@
+arena:
+  polygon: [[1.0, 0.0], [1.0, 1.0], [0.0, 1.0], [0.0, 0.0]] # Vertices composing the polygon of the arena
+  rooms: # Rooms in the arena
+    bedroom: # Room name
+      pose: # Fixed pose in the room, will be used for navigating to the room 
+        position: {x: 0.0, y: 0.0, z: 0.0}
+        orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+      polygon: [[0.0, 0.0], [0.0, 1.0], [1.0, 1.0], [1.0, 0.0]] # Vertices composing the polygon of the room
+      beacons: # Beacons in the room
+        bed: # Beacon name
+          near_pose: # Fixed pose in the room, will be used for navigating to the beacon
+            position: {x: 0.5, y: 0.5, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+          near_polygon:  [[0.4, 0.4], [0.4, 0.6], [0.6, 0.6], [0.6, 0.4]]
+          search_pose: # Pose to search for the beacon for objects
+            position: {x: 0.5, y: 0.5, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+          search_polygon: [[0.4, 0.4], [0.4, 0.6], [0.6, 0.6], [0.6, 0.4]] # Vertices composing the polygon of the beacon, for search
+    kitchen:
+      pose:
+        orientation: {}
+        position: {}
+      polygon:
+      beacons:
+    office:
+      pose:
+        orientation: {}
+        position: {}
+      polygon:
+      beacons:
+    living_room:
+      pose:
+        orientation: {}
+        position: {}
+      polygon:
+      beacons:

tasks/gpsr/config/lab.yaml

@@ -0,0 +1,38 @@
+arena:
+  polygon: [[-1.76, 1.94], [1.87, 2.80], [1.43, 4.71], [0.88, 5.15], [3.05, 5.74], [4.96, 7.79], [7.49, 5.57], [4.62, 0.62], [5.28, -2.72], [3.75, -4.79], [2.36, -5.15], [-0.40, -3.94]]
+  rooms:
+    office:
+      pose:
+        position: {x: 2.49, y: 2.51, z: 0.0}
+        orientation: {x: 0.0, y: 0.0, z: -0.58, w: 0.81}
+      polygon: [[1.75, 3.65], [2.88, 3.91], [3.0, 3.55], [5.21, 1.49], [5.59, 0.64], [5.23, -2.72], [3.72, -4.79], [2.41, -5.21], [0.30, -4.45], [0.25, -4.47], [0.07, -3.84], [-0.41, -3.91], [-1.76, 1.93], [1.93, 2.79]]
+      beacons:
+        tv_desk:
+          near_pose:
+            position: {x: 0.96, y: 0.82, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: 0.76, w: 0.65}
+          near_polygon: [[-0.11, 0.00], [-0.08, 2.36], [1.84, 2.75], [2.02, 1.31]]
+          search_pose:
+            position: {x: 0.97, y: 1.60, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+          search_polygon: [[0.40, 1.92], [0.31, 2.45], [1.36, 2.70], [1.456, 2.17]]
+        window_desk:
+          near_pose:
+            position: {x: 2.38, y: -3.0, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: -0.62, w: 0.79}
+          near_polygon: [[3.34, -3.27], [3.75, -4.80], [2.47, -5.20], [1.75, -3.58]]
+          search_pose:
+            position: {x: 2.47, y: -3.73, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: -0.61, w: 0.79}
+          search_polygon: [[3.53, -3.98], [2.38, -5.18], [1.90, -4.36], [3.54, -4.03]]
+    kitchen:
+      pose:
+        position: {x: 2.26, y: 4.68, z: 0.0}
+        orientation: {x: 0.0, y: 0.0, z: 0.44, w: 0.90}
+      polygon: [[1.75, 3.68], [1.37, 5.23], [3.08, 5.69], [2.78, 7.16], [4.96, 7.67], [7.58, 5.42], [7.74, 4.70], [6.20, 2.69], [4.17, 4.28], [3.50, 4.24], [2.85, 4.26], [2.88, 4.04]]
+      beacons:
+        door:
+          near_pose:
+            position: {x: 4.04, y: 6.02, z: 0.0}
+            orientation: {x: 0.0, y: 0.0, z: 0.41, w: 0.91}
+          near_polygon: [[5.65, 7.34], [4.83, 7.72], [2.78, 7.11], [3.0, 6.11]]