diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch
index 944df5bfa..377f824de 100644
--- a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch
+++ b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch
@@ -7,6 +7,7 @@
+
@@ -30,5 +31,4 @@
-
diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py
index d629df644..91514c01d 100644
--- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py
+++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py
@@ -29,145 +29,143 @@
class Vrx(NaviGatorMission):
nh: NodeHandle
- def __init__(self, *args, **kwargs):
- super().__init__(*args, **kwargs)
-
- @staticmethod
- async def init():
- if hasattr(Vrx, "_vrx_init"):
+ @classmethod
+ async def setup(cls):
+ if hasattr(cls, "_cls_init"):
return
- Vrx.from_lla = Vrx.nh.get_service_client("/fromLL", FromLL)
- Vrx.to_lla = Vrx.nh.get_service_client("/toLL", ToLL)
- Vrx.task_info_sub = Vrx.nh.subscribe("/vrx/task/info", Task)
- await Vrx.task_info_sub.setup()
- Vrx.scan_dock_color_sequence = Vrx.nh.get_service_client(
+ cls.from_lla = cls.nh.get_service_client("/fromLL", FromLL)
+ cls.to_lla = cls.nh.get_service_client("/toLL", ToLL)
+ cls.task_info_sub = cls.nh.subscribe("/vrx/task/info", Task)
+ cls.scan_dock_color_sequence = cls.nh.get_service_client(
"/vrx/scan_dock_deliver/color_sequence",
ColorSequence,
)
- Vrx.fire_ball = Vrx.nh.advertise("/wamv/shooters/ball_shooter/fire", Empty)
- Vrx.station_keep_goal = Vrx.nh.subscribe(
+ cls.fire_ball = cls.nh.advertise("/wamv/shooters/ball_shooter/fire", Empty)
+ cls.station_keep_goal = cls.nh.subscribe(
"/vrx/station_keeping/goal",
GeoPoseStamped,
)
- Vrx.wayfinding_path_sub = Vrx.nh.subscribe("/vrx/wayfinding/waypoints", GeoPath)
- Vrx.station_keeping_pose_error = Vrx.nh.subscribe(
+ cls.wayfinding_path_sub = cls.nh.subscribe("/vrx/wayfinding/waypoints", GeoPath)
+ cls.station_keeping_pose_error = cls.nh.subscribe(
"/vrx/station_keeping/pose_error",
Float64,
)
- Vrx.station_keeping_rms_error = Vrx.nh.subscribe(
+ cls.station_keeping_rms_error = cls.nh.subscribe(
"/vrx/station_keeping/rms_error",
Float64,
)
- Vrx.wayfinding_min_errors = Vrx.nh.subscribe(
+ cls.wayfinding_min_errors = cls.nh.subscribe(
"/vrx/wayfinding/min_errors",
Float64MultiArray,
)
- Vrx.wayfinding_mean_error = Vrx.nh.subscribe(
+ cls.wayfinding_mean_error = cls.nh.subscribe(
"/vrx/wayfinding/mean_error",
Float64,
)
- Vrx.perception_landmark = Vrx.nh.advertise(
+ cls.perception_landmark = cls.nh.advertise(
"/vrx/perception/landmark",
GeoPoseStamped,
)
await asyncio.gather(
- Vrx.fire_ball.setup(),
- Vrx.station_keep_goal.setup(),
- Vrx.wayfinding_path_sub.setup(),
- Vrx.station_keeping_pose_error.setup(),
- Vrx.station_keeping_rms_error.setup(),
- Vrx.wayfinding_min_errors.setup(),
- Vrx.wayfinding_mean_error.setup(),
- Vrx.perception_landmark.setup(),
+ cls.task_info_sub.setup(),
+ cls.fire_ball.setup(),
+ cls.station_keep_goal.setup(),
+ cls.wayfinding_path_sub.setup(),
+ cls.station_keeping_pose_error.setup(),
+ cls.station_keeping_rms_error.setup(),
+ cls.wayfinding_min_errors.setup(),
+ cls.wayfinding_mean_error.setup(),
+ cls.perception_landmark.setup(),
)
- Vrx.animal_landmarks = Vrx.nh.subscribe("/vrx/wildlife/animals/poses", GeoPath)
- Vrx.beacon_landmark = Vrx.nh.get_service_client("beaconLocator", AcousticBeacon)
- Vrx.circle_animal = Vrx.nh.get_service_client("/choose_animal", ChooseAnimal)
- Vrx.set_long_waypoint = Vrx.nh.get_service_client(
+ cls.animal_landmarks = cls.nh.subscribe("/vrx/wildlife/animals/poses", GeoPath)
+ cls.beacon_landmark = cls.nh.get_service_client("beaconLocator", AcousticBeacon)
+ cls.circle_animal = cls.nh.get_service_client("/choose_animal", ChooseAnimal)
+ cls.set_long_waypoint = cls.nh.get_service_client(
"/set_long_waypoint",
MoveToWaypoint,
)
- Vrx.yolo_objects = Vrx.nh.subscribe("/yolov7/detections", Detection2DArray)
- Vrx.tf_listener = axros_tf.TransformListener(Vrx.nh)
- await Vrx.tf_listener.setup()
- Vrx.database_response = Vrx.nh.get_service_client(
+ cls.yolo_objects = cls.nh.subscribe("/yolov7/detections", Detection2DArray)
+ cls.tf_listener = axros_tf.TransformListener(cls.nh)
+ await cls.tf_listener.setup()
+ cls.database_response = cls.nh.get_service_client(
"/database/requests",
ObjectDBQuery,
)
- Vrx.get_two_closest_cones = Vrx.nh.get_service_client(
+ cls.get_two_closest_cones = cls.nh.get_service_client(
"/get_two_closest_cones",
TwoClosestCones,
)
await asyncio.gather(
- Vrx.animal_landmarks.setup(),
- Vrx.yolo_objects.setup(),
+ cls.animal_landmarks.setup(),
+ cls.yolo_objects.setup(),
)
- Vrx.pcodar_reset = Vrx.nh.get_service_client("/pcodar/reset", Trigger)
+ cls.pcodar_reset = cls.nh.get_service_client("/pcodar/reset", Trigger)
- Vrx.front_left_camera_info_sub = None
- Vrx.front_left_camera_sub = None
- Vrx.front_right_camera_info_sub = None
- Vrx.front_right_camera_sub = None
+ cls.front_left_camera_info_sub = None
+ cls.front_left_camera_sub = None
+ cls.front_right_camera_info_sub = None
+ cls.front_right_camera_sub = None
- Vrx._vrx_init = True
+ cls._cls_init = True
- @staticmethod
- async def shutdown():
+ @classmethod
+ async def shutdown(cls):
+ return
await asyncio.gather(
- Vrx.task_info_sub.shutdown(),
- Vrx.animal_landmarks.shutdown(),
- Vrx.yolo_objects.shutdown(),
- Vrx.fire_ball.shutdown(),
- Vrx.station_keep_goal.shutdown(),
- Vrx.wayfinding_path_sub.shutdown(),
- Vrx.station_keeping_pose_error.shutdown(),
- Vrx.station_keeping_rms_error.shutdown(),
- Vrx.wayfinding_min_errors.shutdown(),
- Vrx.wayfinding_mean_error.shutdown(),
- Vrx.perception_landmark.shutdown(),
+ cls.task_info_sub.shutdown(),
+ cls.animal_landmarks.shutdown(),
+ cls.yolo_objects.shutdown(),
+ cls.fire_ball.shutdown(),
+ cls.station_keep_goal.shutdown(),
+ cls.wayfinding_path_sub.shutdown(),
+ cls.station_keeping_pose_error.shutdown(),
+ cls.station_keeping_rms_error.shutdown(),
+ cls.wayfinding_min_errors.shutdown(),
+ cls.wayfinding_mean_error.shutdown(),
+ cls.perception_landmark.shutdown(),
)
def cleanup(self):
pass
- @staticmethod
- async def init_front_left_camera():
- if Vrx.front_left_camera_sub is None:
- Vrx.front_left_camera_sub = Vrx.nh.subscribe(
+ @classmethod
+ async def init_front_left_camera(cls):
+ if cls.front_left_camera_sub is None:
+ cls.front_left_camera_sub = cls.nh.subscribe(
"/wamv/sensors/cameras/front_left_camera/image_raw",
Image,
)
- if Vrx.front_left_camera_info_sub is None:
- Vrx.front_left_camera_info_sub = Vrx.nh.subscribe(
+ if cls.front_left_camera_info_sub is None:
+ cls.front_left_camera_info_sub = cls.nh.subscribe(
"/wamv/sensors/cameras/front_left_camera/camera_info",
CameraInfo,
)
await asyncio.gather(
- Vrx.front_left_camera_sub.setup(),
- Vrx.front_left_camera_info_sub.setup(),
+ cls.front_left_camera_sub.setup(),
+ cls.front_left_camera_info_sub.setup(),
)
- @staticmethod
- async def init_front_right_camera():
- if Vrx.front_right_camera_sub is None:
- Vrx.front_right_camera_sub = Vrx.nh.subscribe(
+ @classmethod
+ async def init_front_right_camera(cls):
+ if cls.front_right_camera_sub is None:
+ cls.front_right_camera_sub = cls.nh.subscribe(
"/wamv/sensors/cameras/front_right_camera/image_raw",
Image,
)
- if Vrx.front_right_camera_info_sub is None:
- Vrx.front_right_camera_info_sub = Vrx.nh.subscribe(
+ if cls.front_right_camera_info_sub is None:
+ cls.front_right_camera_info_sub = cls.nh.subscribe(
"/wamv/sensors/cameras/front_right_camera/camera_info",
CameraInfo,
)
await asyncio.gather(
- Vrx.front_right_camera_sub.setup(),
- Vrx.front_right_camera_info_sub.setup(),
+ cls.front_right_camera_sub.setup(),
+ cls.front_right_camera_info_sub.setup(),
)
async def geo_pose_to_enu_pose(self, geo):
diff --git a/mil_common/axros b/mil_common/axros
index 1b0399935..a4951d333 160000
--- a/mil_common/axros
+++ b/mil_common/axros
@@ -1 +1 @@
-Subproject commit 1b03999351fb5a61b202ff125f493229c2a1676b
+Subproject commit a4951d33356c349856532ddef5c97c6ec6810c28
diff --git a/mil_common/mil_missions/nodes/mission_server b/mil_common/mil_missions/nodes/mission_server
index 3548d0b58..0009b8b8e 100755
--- a/mil_common/mil_missions/nodes/mission_server
+++ b/mil_common/mil_missions/nodes/mission_server
@@ -29,8 +29,10 @@ class MissionRunner:
missions_loaded: bool
base_mission: type[BaseMission]
missions: dict[str, type[BaseMission]]
+ _running_tasks: set[asyncio.Task]
def __init__(self):
+ self._running_tasks = set()
pass
async def init(self):
@@ -213,6 +215,7 @@ class MissionRunner:
self.mission = self.missions[goal.mission]()
self.mission_future = asyncio.create_task(self.run_with_callback(parameters))
self.mission_future.add_done_callback(self.mission_finished_cb)
+ self._running_tasks.add(self.mission_future)
async def run_with_callback(self, parameters):
try:
@@ -242,6 +245,7 @@ class MissionRunner:
mission, or raises an exception. Publishes the correct result to the action clients.
"""
result = DoMissionResult()
+ self._running_tasks.remove(task)
try:
task_result = task.result()