mil_poi: Ensure that new/initial POIs never have a z-axis rotation

docs/reference/poi.rst

@@ -27,8 +27,10 @@ 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, 0.0, 0.0, 0.0, 0.0, 0.0] # Name (key) and location
-      (position & orientation) (value) of specific POIs
+      # Name (key) and location  (position & orientation) (value) of specific POIs
+      start_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      # ... or just position only (quaternion of [0, 0, 0, 0] assumed)
+      position_only: [0, 0, 0]
 
 POIServer
 ^^^^^^^^^

mil_common/utils/mil_poi/mil_poi/server.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
 
+from __future__ import annotations
+
+import math
 from threading import Lock
-from typing import Optional
 
 import rospy
 import tf2_ros
@@ -10,6 +12,7 @@
 from mil_ros_tools.msg_helpers import numpy_to_point
 from mil_tools import thread_lock
 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+from tf.transformations import euler_from_quaternion
 from visualization_msgs.msg import (
     InteractiveMarker,
     InteractiveMarkerControl,
@@ -74,7 +77,7 @@ def __init__(self):
             for key, value in pois.items():
                 assert isinstance(key, str)
                 assert isinstance(value, list)
-                assert len(value) == 3 or 7
+                assert len(value) == 3 or len(value) == 7
                 name = key
                 pose = Pose()
                 pose.position = numpy_to_point(value[:3])
@@ -102,7 +105,7 @@ def __init__(self):
             self.save_to_param_cb,
         )
 
-    def transform_position(self, ps: PointStamped) -> Optional[Point]:
+    def transform_position(self, ps: PointStamped) -> Point | None:
         """
         Attempt to transform a PointStamped message into the global frame, returning
         the position of the transformed point or None if transform failed.
@@ -334,11 +337,29 @@ def _remove_poi(self, name: str) -> bool:
                 return True
         return False
 
+    def _valid_orientation(self, orientation: Quaternion) -> tuple[bool, float]:
+        """
+        Ensures that POIs have no rotation in the z axis.
+        """
+        z_component = (
+            euler_from_quaternion(
+                [orientation.x, orientation.y, orientation.z, orientation.w],
+            )[2]
+            * 360
+            / math.pi
+        )
+        return -0.1 < z_component < 0.1, z_component
+
     def _add_poi(self, name: str, position: Pose) -> bool:
         """
         Internal implementation of add_poi, which is NOT thread safe and does
         NOT update clients of change.
         """
+        res, z = self._valid_orientation(position.orientation)
+        if not res:
+            raise ValueError(
+                f"Orientation should have no rotation in the z (found rotation of: {z} degrees)",
+            )
         if self.interactive_marker_server.get(name) is not None:
             return False
         poi = POI()