From 01345e5a38c942f6fce4d92d3f1fcfecdc190833 Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Mon, 30 Dec 2024 18:21:22 -0700
Subject: [PATCH] Tools: ros2: Clean up copter takeoff

* Finish timeout implementation missing variables
* Remove unused imports

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
---
 .../ardupilot_dds_tests/copter_takeoff.py     | 20 +++++++------------
 1 file changed, 7 insertions(+), 13 deletions(-)

diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
index 4443c17ab24fcb..48aaef2a91d06f 100755
--- a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
+++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
@@ -20,27 +20,21 @@
 Warning - This is NOT production code; it's a simple demo of capability.
 """
 
-import math
 import rclpy
 import time
-import errno
 
 from rclpy.node import Node
-from builtin_interfaces.msg import Time
-from ardupilot_msgs.msg import GlobalPosition
 from geographic_msgs.msg import GeoPoseStamped
-from geopy import distance
-from geopy import point
 from ardupilot_msgs.srv import ArmMotors
 from ardupilot_msgs.srv import ModeSwitch
 from ardupilot_msgs.srv import Takeoff
 
 
-
 COPTER_MODE_GUIDED = 4
 
 TAKEOFF_ALT = 10.5
 
+
 class CopterTakeoff(Node):
     """Copter takeoff using guided control."""
 
@@ -65,7 +59,7 @@ def __init__(self):
         self._client_takeoff = self.create_client(Takeoff, self._takeoff_topic)
         while not self._client_takeoff.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('takeoff service not available, waiting again...')
-        
+
         self.declare_parameter("geopose_topic", "/ap/geopose/filtered")
         self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value
         qos = rclpy.qos.QoSProfile(
@@ -84,7 +78,6 @@ def geopose_cb(self, msg: GeoPoseStamped):
             # Store current state
             self._cur_geopose = msg
 
-
     def arm(self):
         req = ArmMotors.Request()
         req.arm = True
@@ -113,14 +106,14 @@ def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Du
         """Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
         is_in_desired_mode = False
         start = self.get_clock().now()
-        while not is_in_desired_mode:
+        while not is_in_desired_mode and self.get_clock().now() - start < timeout:
             result = self.switch_mode(desired_mode)
             # Handle successful switch or the case that the vehicle is already in expected mode
             is_in_desired_mode = result.status or result.curr_mode == desired_mode
             time.sleep(1)
 
         return is_in_desired_mode
-    
+
     def takeoff(self, alt):
         req = Takeoff.Request()
         req.alt = alt
@@ -132,17 +125,18 @@ def takeoff_with_timeout(self, alt: int, timeout: rclpy.duration.Duration):
         """Try to takeoff. Returns true on success, or false if takeoff fails or times out."""
         takeoff_success = False
         start = self.get_clock().now()
-        while not takeoff_success:
+        while not takeoff_success and self.get_clock().now() - start < timeout:
             result = self.takeoff(alt)
             takeoff_success = result.status
             time.sleep(1)
 
         return takeoff_success
-    
+
     def get_cur_geopose(self):
         """Return latest geopose."""
         return self._cur_geopose
 
+
 def main(args=None):
     """Node entry point."""
     rclpy.init(args=args)