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..b3136151c 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py @@ -6,6 +6,7 @@ from .docking import Docking from .entrance_gate2 import EntranceGate2 +from .go_to_poi import GoToPOI from .navigation import Navigation from .navigator import NaviGatorMission from .scan_the_code import ScanTheCodeMission @@ -17,28 +18,75 @@ class Autonomous2024(NaviGatorMission): # timeout (in secs) TIMEOUT = 180 - async def run_mission(self, mission_cls: type[NaviGatorMission], name: str): + async def run_mission( + self, + mission_cls: type[NaviGatorMission], + name: str, + *args, + str_arg: str = "", + **kwargs, + ): rospy.loginfo(f"[autonomous] 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}!") async def run(self, args: str): # Step 1: Entrance and exit gates - await self.run_mission(EntranceGate2, "entrance gate") + await self.send_feedback("[AUTONOMOUS] GOING TO ENTRANCE GATE") + await self.run_mission(GoToPOI, "entrance_gate", str_arg="entrance_gate") + await self.send_feedback("[AUTONOMOUS] STARTING AUTONOMOUS MISSION") + await self.run_mission(EntranceGate2, "entrance gate", scan_code=True) + + # Step 1.5: Launch the drone + # FILL IN HERE + await self.send_feedback( + "[AUTONOMOUS] LAUNCHING THE DRONE, T-45 SEC TO CONTINUE", + ) + await asyncio.sleep(10) + await self.send_feedback( + "[AUTONOMOUS] LAUNCHING THE DRONE, T-35 SEC TO CONTINUE", + ) + await asyncio.sleep(10) + await self.send_feedback( + "[AUTONOMOUS] LAUNCHING THE DRONE, T-25 SEC TO CONTINUE", + ) + await asyncio.sleep(10) + await self.send_feedback( + "[AUTONOMOUS] LAUNCHING THE DRONE, T-15 SEC TO CONTINUE", + ) + await asyncio.sleep(10) + await self.send_feedback( + "[AUTONOMOUS] LAUNCHING THE DRONE, T-5 SEC TO CONTINUE", + ) + await asyncio.sleep(5) # Step 2: Scan the Code + await self.send_feedback("[AUTONOMOUS] GOING TO SCAN THE CODE") + await self.run_mission(GoToPOI, "scan_the_code poi", str_arg="scan_the_code") await self.run_mission(ScanTheCodeMission, "scan the code") # Step 3: Wildlife Mission + await self.send_feedback("[AUTONOMOUS] GOING TO WILDLIFE") + await self.run_mission(GoToPOI, "wildlife poi", str_arg="wildlife") await self.run_mission(Wildlife, "wildlife") # Step 4: Navigation Mission + await self.send_feedback("[AUTONOMOUS] GOING TO NAVIGATION") + await self.run_mission(GoToPOI, "navigation poi", str_arg="navigation") await self.run_mission(Navigation, "navigation") + # Step 4.5: Receive the drone + # FILL IN HERE + # Step 5: Dock Mission + await self.send_feedback("[AUTONOMOUS] GOING TO DOCKING") + await self.run_mission(GoToPOI, "docking poi", str_arg="docking") await self.run_mission(Docking, "docking") - # Step 6: UAV Mission - pass + # Step 6: Leave the course + await self.send_feedback("[AUTONOMOUS] LEAVING COURSE") + await self.run_mission(GoToPOI, "exit_gate poi", str_arg="exit_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 18bf8fe6e..1a9ccf02a 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py @@ -3,23 +3,21 @@ import numpy as np from mil_tools import quaternion_matrix -from std_srvs.srv import SetBoolRequest from .navigator import NaviGatorMission class EntranceGate2(NaviGatorMission): - async def run(self, args): + async def run(self, args, *, scan_code=False): # Parameters: - scan_code = False return_to_start = True circle_radius = 10 circle_direction = "cw" yaw_offset = 1.57 self.traversal_distance = 3 - #await self.set_classifier_enabled.wait_for_service() - #await self.set_classifier_enabled(SetBoolRequest(data=True)) + # await self.set_classifier_enabled.wait_for_service() + # await self.set_classifier_enabled(SetBoolRequest(data=True)) # Inspect Gates await self.change_wrench("/wrench/autonomous") diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py index d95de503d..e575dc331 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py @@ -150,7 +150,7 @@ async def explore_closest_until(self, is_done, filter_and_sort) -> dict: if ret is not None else [] ) - self.send_feedback(f"Analyzing objects: {labels}") + # self.send_feedback(f"Analyzing objects: {labels}") if ret is not None: if move_id_tuple is not None: self.send_feedback("Condition met. Canceling investigation")