From 9c2336ba0d1f21e42487b23a6f930d692b215287 Mon Sep 17 00:00:00 2001
From: Cameron Brown <me@cbrxyz.com>
Date: Thu, 7 Nov 2024 09:43:28 -0500
Subject: [PATCH] mil_poi: Ensure that new/initial POIs never have a z-axis
 rotation

---
 docs/reference/poi.rst                     |  6 +++--
 mil_common/utils/mil_poi/mil_poi/server.py | 27 +++++++++++++++++++---
 2 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/docs/reference/poi.rst b/docs/reference/poi.rst
index c740533ab..38cd511e0 100644
--- a/docs/reference/poi.rst
+++ b/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
 ^^^^^^^^^
diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py
index b2e37c0f1..763fd2131 100644
--- a/mil_common/utils/mil_poi/mil_poi/server.py
+++ b/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()