diff --git a/NaviGator/mission_control/navigator_launch/config/poi.yaml b/NaviGator/mission_control/navigator_launch/config/poi.yaml index 744c79762..253095182 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi.yaml +++ b/NaviGator/mission_control/navigator_launch/config/poi.yaml @@ -1,6 +1,6 @@ --- global_frame: enu initial_pois: - wildlife: [0, 0, 0] - 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] + wildlife: [0, 0, 0, 0.0, 0.0, 0.0, 0.0] diff --git a/docs/reference/poi.rst b/docs/reference/poi.rst index 53a6ccc8c..38cd511e0 100644 --- a/docs/reference/poi.rst +++ b/docs/reference/poi.rst @@ -27,7 +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] # Name (key) and location (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/hw/ws2/ci/docker-compose.yml b/hw/ws2/ci/docker-compose.yml deleted file mode 100644 index 07405df43..000000000 --- a/hw/ws2/ci/docker-compose.yml +++ /dev/null @@ -1,86 +0,0 @@ ---- -version: "2.3" -services: - mil_u_pre_commit_alpha: - image: myoung34/github-runner:latest - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_pre_commit_alpha - RUNNER_GROUP: mala-lab-pre-commit - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/pre-commit-alpha/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/pre-commit-alpha:/tmp/runners/pre-commit-alpha' - - mil_u_pre_commit_beta: - image: myoung34/github-runner:latest - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_pre_commit_beta - RUNNER_GROUP: mala-lab-pre-commit - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/pre-commit-beta/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/pre-commit-beta:/tmp/runners/pre-commit-beta' - - mil_u_main_alpha: - image: myoung34/github-runner:latest - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_main_alpha - RUNNER_GROUP: mala-lab-main - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/main-alpha/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/main-alpha:/tmp/runners/main-alpha' - - mil_u_main_beta: - image: myoung34/github-runner:latest - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_main_beta - RUNNER_GROUP: mala-lab-main - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/main-beta/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/main-beta:/tmp/runners/main-alpha' - - mil_u_noble_alpha: - image: myoung34/github-runner:ubuntu-noble - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_noble_alpha - RUNNER_GROUP: mala-lab-noble - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/noble-alpha/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/noble-alpha:/tmp/runners/noble-alpha' - - mil_u_noble_beta: - image: myoung34/github-runner:ubuntu-noble - environment: - ORG_NAME: uf-mil - RUNNER_NAME_PREFIX: mil_u_noble_beta - RUNNER_GROUP: mala-lab-noble - RUNNER_TOKEN: ${RUNNER_TOKEN} - RUNNER_WORKDIR: /tmp/runners/noble-beta/work - RUNNER_SCOPE: 'org' - LABELS: linux,x64,gpu - volumes: - - '/var/run/docker.sock:/var/run/docker.sock' - - '/tmp/runners/noble-beta:/tmp/runners/noble-beta' diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py index d6d7d4c25..728762d56 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 tf2_geometry_msgs import PointStamped from visualization_msgs.msg import ( InteractiveMarker, @@ -75,11 +78,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 len(value) == 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 @@ -95,7 +106,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. @@ -327,11 +338,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()