From d62e7eb44db5bfa78d7983cdb73b3a557b5e7d91 Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Tue, 5 Nov 2024 23:47:31 +0000 Subject: [PATCH] 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