diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml index 582682cc9..85bfcaf07 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -28,4 +28,4 @@ intensity_filter_max_intensity: 1 # yamllint disable-line rule:line-length # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"] # yamllint disable-line rule:line-length -classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "UNKNOWN"] +classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "black_cylinder", "white_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"] diff --git a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml index 2a27948a7..b50413ccc 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml +++ b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml @@ -1,10 +1,8 @@ --- global_frame: enu initial_pois: - wildlife: [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] + wildlife: [-1.7053, 44.9246, 0.0] + docking: [-3.24, 58.64265, 0.0] + entrance_gate: [22.87, 8.66, 0.0] + navigation: [46.6878, 23.3085, 0.0] + scan_the_code: [29.868, 44.1452, 0.0] diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py index d1ff9007d..70afa21b5 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py @@ -1,14 +1,11 @@ from __future__ import annotations import asyncio +import datetime -import rospy - -from .docking import Docking from .entrance_gate2 import EntranceGate2 -from .navigation import Navigation +from .go_to_poi import GoToPOI from .navigator import NaviGatorMission -from .scan_the_code import ScanTheCodeMission from .wildlife import Wildlife @@ -17,28 +14,69 @@ class Autonomous2024(NaviGatorMission): # timeout (in secs) TIMEOUT = 180 - async def run_mission(self, mission_cls: type[NaviGatorMission], name: str): - rospy.loginfo(f"[autonomous] beginning {name}...") + async def run_mission( + self, + mission_cls: type[NaviGatorMission], + name: str, + *args, + str_arg: str = "", + **kwargs, + ): + # self.send_feedback(f"beginning {name}...") try: - await asyncio.wait_for(mission_cls().run(""), self.TIMEOUT) + await asyncio.wait_for( + mission_cls().run(str_arg, *args, **kwargs), + self.TIMEOUT, + ) except asyncio.TimeoutError: - rospy.logwarn(f"[autonomous] ran out of time on {name}!") + self.send_feedback(f"!!! ran out of time on {name}!") + + async def go_to_poi(self, name: str): + await self.run_mission(GoToPOI, f"go to {name} poi", str_arg=name) + + def time_elapsed(self) -> str: + last_min, last_sec = divmod( + (datetime.datetime.now() - self._last_checkpoint).total_seconds(), + 60, + ) + min, sec = divmod( + (datetime.datetime.now() - self.start_time).total_seconds(), + 60, + ) + self._last_checkpoint = datetime.datetime.now() + return f"{int(min)}m {int(sec)}s (+{int(last_min)}m {int(last_sec)}s)" + + def send_feedback(self, msg: str): + super().send_feedback(f"[autonomous, {self.time_elapsed()}] {msg}") async def run(self, args: str): - # Step 1: Entrance and exit gates - await self.run_mission(EntranceGate2, "entrance gate") + # Step 1: Entrance gate + self.start_time = datetime.datetime.now() + self._last_checkpoint = self.start_time + self.send_feedback("proceeding to entrance gate...") + await self.go_to_poi("entrance_gate") + self.send_feedback("starting entrance gate task...") + await self.run_mission( + EntranceGate2, + "entrance gate", + scan_code=True, + return_to_start=False, + circle=False, + ) # Step 2: Scan the Code - await self.run_mission(ScanTheCodeMission, "scan the code") + # await self.run_mission(ScanTheCodeMission, "scan the code") # Step 3: Wildlife Mission + self.send_feedback("going to wildlife poi...") + await self.go_to_poi("wildlife") + self.send_feedback("running wildlife mission...") await self.run_mission(Wildlife, "wildlife") # Step 4: Navigation Mission - await self.run_mission(Navigation, "navigation") + # await self.run_mission(Navigation, "navigation") # Step 5: Dock Mission - await self.run_mission(Docking, "docking") + # await self.run_mission(Docking, "docking") - # Step 6: UAV Mission - pass + # Step 6: Exit through the same gate diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py index a025b9b69..33acb2391 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py @@ -9,10 +9,14 @@ class EntranceGate2(NaviGatorMission): - async def run(self, args): - # Parameters: - scan_code = False - return_to_start = True + async def run( + self, + args, + *, + scan_code: bool = False, + return_to_start: bool = True, + circle: bool = True, + ): circle_radius = 5 circle_direction = "cw" yaw_offset = 1.57 @@ -52,7 +56,7 @@ async def run(self, args): if scan_code: pass - elif return_to_start: + elif return_to_start and circle: # Approach buoy we will rotate around buoy = await self.get_sorted_objects("black_cylinder", n=1) buoy = buoy[1][0] @@ -78,8 +82,9 @@ async def run(self, args): yaw_offset, ) + if return_to_start: # Go back through start gate - self.send_feedback("Navigating through gate") + self.send_feedback("Navigating through original gate") await self.move.set_position(traversal_points[1]).look_at( traversal_points[0], ).go() diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py index 1bad17210..d6d7d4c25 100644 --- a/mil_common/utils/mil_poi/mil_poi/server.py +++ b/mil_common/utils/mil_poi/mil_poi/server.py @@ -5,11 +5,12 @@ import rospy import tf2_ros -from geometry_msgs.msg import Point, PointStamped, Pose, Quaternion +from geometry_msgs.msg import Point, Pose, Quaternion from interactive_markers.interactive_marker_server import InteractiveMarkerServer 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 tf2_geometry_msgs import PointStamped from visualization_msgs.msg import ( InteractiveMarker, InteractiveMarkerControl,