From d62e7eb44db5bfa78d7983cdb73b3a557b5e7d91 Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Tue, 5 Nov 2024 23:47:31 +0000 Subject: [PATCH 1/3] can now add orientation to initial poi yaml files --- .../navigator_launch/config/poi.yaml | 4 ++-- .../navigator_launch/config/poi_sim.yaml | 14 +++++++------- docs/reference/poi.rst | 3 ++- mil_common/utils/mil_poi/mil_poi/server.py | 14 +++++++++++--- 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/config/poi.yaml b/NaviGator/mission_control/navigator_launch/config/poi.yaml index b91cbba8c..f5bcb71d6 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi.yaml +++ b/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] diff --git a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml index 273c9c99d..400eb8344 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml +++ b/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] diff --git a/docs/reference/poi.rst b/docs/reference/poi.rst index 53a6ccc8c..c740533ab 100644 --- a/docs/reference/poi.rst +++ b/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 ^^^^^^^^^ diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py index 1bad17210..b2e37c0f1 100644 --- a/mil_common/utils/mil_poi/mil_poi/server.py +++ b/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 From 9c2336ba0d1f21e42487b23a6f930d692b215287 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Thu, 7 Nov 2024 09:43:28 -0500 Subject: [PATCH 2/3] 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() From db4d0435673e5e340dad4ea2d19d88632375f695 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 16 Dec 2024 00:06:45 -0500 Subject: [PATCH 3/3] navigator_launch: pre-commit format --- NaviGator/mission_control/navigator_launch/config/poi.yaml | 2 +- NaviGator/mission_control/navigator_launch/config/poi_sim.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/config/poi.yaml b/NaviGator/mission_control/navigator_launch/config/poi.yaml index af34dbe04..253095182 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi.yaml +++ b/NaviGator/mission_control/navigator_launch/config/poi.yaml @@ -3,4 +3,4 @@ global_frame: enu initial_pois: 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] - wildlife: [0, 0, 0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file + wildlife: [0, 0, 0, 0.0, 0.0, 0.0, 0.0] diff --git a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml index f23bd33e8..2a27948a7 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml +++ b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml @@ -7,4 +7,4 @@ initial_pois: 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] \ No newline at end of file + stc: [49.49, -82.65, 0.0]