can now add orientation to initial poi yaml files

NaviGator/mission_control/navigator_launch/config/poi.yaml

@@ -1,5 +1,5 @@
 ---
 global_frame: enu
 initial_pois:
-  start_gate: [0, 0, 0]
-  entrance_gate: [0, 0, 0]
+  start_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0]
+  entrance_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0]

NaviGator/mission_control/navigator_launch/config/poi_sim.yaml

@@ -1,10 +1,10 @@
 ---
 global_frame: enu
 initial_pois:
-  circle_totems: [26.09821319580078, 59.91523361206055, 0.0]
-  dock: [108.05, -1.07, 0.0]
-  entrance_gate: [51.28, -48.69, 0.0]
-  obstacle_course: [-32.803, -83.41, 0.0]
-  ring_challenge: [61.271873474121094, 15.894840240478516, 0.0]
-  start_gate: [12.75, 0.433, 0.0]
-  stc: [49.49, -82.65, 0.0]
+  circle_totems: [26.09821319580078, 59.91523361206055, 0.0, 0.0, 0.0, 0.0, 0.0]
+  dock: [108.05, -1.07, 0.0, 0.0, 0.0, 0.0, 0.0]
+  entrance_gate: [51.28, -48.69, 0.0, 0.0, 0.0, 0.0, 0.0]
+  obstacle_course: [-32.803, -83.41, 0.0, 0.0, 0.0, 0.0, 0.0]
+  ring_challenge: [61.271873474121094, 15.894840240478516, 0.0, 0.0, 0.0, 0.0, 0.0]
+  start_gate: [12.75, 0.433, 0.0, 0.0, 0.0, 0.0, 0.0]
+  stc: [49.49, -82.65, 0.0, 0.0, 0.0, 0.0, 0.0]

docs/reference/poi.rst

@@ -27,7 +27,8 @@ The default format of the POI configuration file is the following format:
     ---
     global_frame: enu # Name of the frame to derive POI locations from
     initial_pois: # List of POIs that are spawned by default
-      start_gate: [0, 0, 0] # Name (key) and location (value) of specific POIs
+      start_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0] # Name (key) and location
+      (position & orientation) (value) of specific POIs
 
 POIServer
 ^^^^^^^^^

mil_common/utils/mil_poi/mil_poi/server.py

@@ -74,11 +74,19 @@ def __init__(self):
             for key, value in pois.items():
                 assert isinstance(key, str)
                 assert isinstance(value, list)
-                assert len(value) == 3
+                assert len(value) == 3 or 7
                 name = key
                 pose = Pose()
-                pose.position = numpy_to_point(value)
-                pose.orientation = Quaternion(0.0, 0.0, 0.0, 0.0)
+                pose.position = numpy_to_point(value[:3])
+                if len(value) == 3:
+                    pose.orientation = Quaternion(0.0, 0.0, 0.0, 0.0)
+                elif len(value) == 7:
+                    pose.orientation = Quaternion(
+                        value[3],
+                        value[4],
+                        value[5],
+                        value[6],
+                    )
                 self._add_poi(name, pose)
 
         # Update clients / markers of changes from param