From b9270f7745db13be2fd62f0ef97f6e9acbdaa54d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 6 Jun 2023 16:42:22 +0200 Subject: [PATCH] add ready drone to api listener --- .../api_communication/api_listener.py | 22 ++++++++++++++++--- src/drone_controls/src/PositionChanger.cpp | 2 +- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index cdfdc798..05bb2cb6 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -8,7 +8,7 @@ from drone_services.srv import MovePosition from drone_services.srv import EnableFailsafe from drone_services.srv import ArmDrone from drone_services.srv import DisarmDrone -from drone_services.srv import SetAttitude +from drone_services.srv import ReadyDrone import asyncio import websockets.server @@ -94,6 +94,10 @@ class ApiListener(Node): self.wait_for_service(self.disarm_drone_client, "Disarm drone") self.disarm_drone_request = DisarmDrone.Request() + self.ready_drone_client = self.create_client(ReadyDrone, "/drone/ready") + self.wait_for_service(self.ready_drone_client, "Ready drone") + self.ready_drone_request = ReadyDrone.Request() + self.status_data = {} self.status_data_received = False self.last_message = "" @@ -365,8 +369,20 @@ class ApiListener(Node): future.add_done_callback(partial(self.disarm_service_callback)) else: self.get_logger().info('Arm command received') - future = self.arm_drone_client.call_async(self.arm_drone_request) - future.add_done_callback(partial(self.arm_service_callback)) + future = self.ready_drone_client.call_async( + self.ready_drone_request) + future.add_done_callback(partial(self.ready_drone_callback)) + + def ready_drone_callback(self, future): + try: + result = future.result() + if result.success: + self.get_logger().info('Ready service call success') + else: + self.get_logger().error('Ready service call failed') + except Exception as e: + self.get_logger().error( + 'Something went wrong while calling the ready service!\n' + str(e)) def arm_service_callback(self, future): try: diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index d5fd206a..b03c70a6 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -52,7 +52,7 @@ public: // vehicle_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); this->move_position_service = this->create_service("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - this->ready_drone_service = this->create_service("/drone/ready_drone", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + this->ready_drone_service = this->create_service("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); this->failsafe_publisher = this->create_publisher("/drone/failsafe", 10);