diff --git a/src/fc_network_apps/arm_disarm.py b/src/fc_network_apps/arm_disarm.py index 21f8386..d61eb67 100644 --- a/src/fc_network_apps/arm_disarm.py +++ b/src/fc_network_apps/arm_disarm.py @@ -37,7 +37,8 @@ def arm_disarm( param1: 1.0 to arm, 0.0 to disarm. param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed. """ - rclpy.init(args=None) + if not rclpy.ok(): + rclpy.init(args=None) node: Optional[Node] = None try: node = Node("fc_arm_disarm_client_once") @@ -82,7 +83,6 @@ def arm_disarm( finally: if node is not None: node.destroy_node() - rclpy.shutdown() if __name__ == "__main__": diff --git a/src/fc_network_apps/changeMode.py b/src/fc_network_apps/changeMode.py index aa98230..09e04b4 100644 --- a/src/fc_network_apps/changeMode.py +++ b/src/fc_network_apps/changeMode.py @@ -42,7 +42,8 @@ def change_mode( ) -> ChangeModeResult: """One-shot helper for collaborators who want minimal code.""" - rclpy.init(args=None) + if not rclpy.ok(): + rclpy.init(args=None) node: Optional[Node] = None try: node = Node("fc_change_mode_client_once") @@ -87,7 +88,6 @@ def change_mode( finally: if node is not None: node.destroy_node() - rclpy.shutdown() if __name__ == "__main__": diff --git a/src/fc_network_apps/takeoff.py b/src/fc_network_apps/takeoff.py index 47754b7..040e783 100644 --- a/src/fc_network_apps/takeoff.py +++ b/src/fc_network_apps/takeoff.py @@ -35,7 +35,8 @@ def takeoff( service_name: str = DEFAULT_SERVICE_NAME, ) -> TakeoffResult: """One-shot MAV_CMD_NAV_TAKEOFF wrapper.""" - rclpy.init(args=None) + if not rclpy.ok(): + rclpy.init(args=None) node: Optional[Node] = None try: node = Node("fc_takeoff_client_once") @@ -80,7 +81,6 @@ def takeoff( finally: if node is not None: node.destroy_node() - rclpy.shutdown() if __name__ == "__main__":