From cc783214219e63fe301071f904d45ba24b664bb9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 8 Jun 2023 21:44:27 +0200 Subject: [PATCH] add landing functionality with height meter --- .../api_communication/api_listener.py | 17 +++++ src/drone_controls/src/PositionChanger.cpp | 69 ++++++++++++++++++- 2 files changed, 84 insertions(+), 2 deletions(-) diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py index 855d97dc..3608d4a4 100644 --- a/src/api_communication/api_communication/api_listener.py +++ b/src/api_communication/api_communication/api_listener.py @@ -9,6 +9,7 @@ from drone_services.srv import EnableFailsafe from drone_services.srv import ArmDrone from drone_services.srv import DisarmDrone from drone_services.srv import ReadyDrone +from drone_services.srv import Land import asyncio import websockets.server @@ -100,6 +101,10 @@ class ApiListener(Node): self.wait_for_service(self.ready_drone_client, "Ready drone") self.ready_drone_request = ReadyDrone.Request() + self.land_client = self.create_client(Land, "/drone/land") + self.wait_for_service(self.land_client, "Land drone") + self.land_request = Land.Request() + self.status_data = {} self.status_data_received = False self.last_message = "" @@ -362,6 +367,8 @@ class ApiListener(Node): self.move_position_request.front_back = 0.0 self.move_position_request.angle = 0.0 self.send_move_position_request() + future = self.land_drone_client.call_async(self.land_drone_request) + future.add_done_callback(partial(self.land_service_callback)) def arm_disarm(self): """Sends an arm or disarm request to the PX4Controller""" @@ -411,6 +418,16 @@ class ApiListener(Node): except Exception as e: self.get_logger().error( 'Something went wrong while calling the disarm service!\n' + str(e)) + def land_service_callback(self, future): + try: + result = future.result() + if result.is_landing: + self.get_logger().info('Land service call success') + else: + self.get_logger().error('Land service call failed') + except Exception as e: + self.get_logger().error( + 'Something went wrong while calling the land service!\n' + str(e)) def consume_message(self, message): """Consumes a message from the client""" diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 667f0eb7..3bc2669c 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,8 @@ // converts bitmask control mode to control mode used by PX4Controller #define PX4_CONTROLLER_CONTROL_MODE(x) ((x) == 4 ? 1 : ((x) == 16 ? 2 : ((x) == 32 ? 3 : -1))) +#define HEIGHT_DELTA 0.1 + using namespace std::chrono_literals; struct Quaternion @@ -54,7 +57,8 @@ public: 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", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - + this->land_service = this->create_service("/drone/land", std::bind(&PositionChanger::handle_land_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + this->failsafe_publisher = this->create_publisher("/drone/failsafe", 10); this->trajectory_client = this->create_client("/drone/set_trajectory"); @@ -271,9 +275,60 @@ public: apply_collision_weights(); } + void attitude_land_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + if (future.get()->success) + { + this->get_logger()->info("Attitude set to 0 for landing, landing done"); + this->has_landed = true; + } + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + + void vehicle_control_land_request_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + if (future.get()->success) + { + this->get_logger()->info("Vehicle Control mode set to attitude for landing"); + this->attitude_request->pitch = 0; + this->attitude_request->roll = 0; + this->attitude_request->yaw = 0; + this->attitude_request->thrust = 0; + auto attitude_response = this->attitude_client->async_send_request(this->attitude_request, std::bind(&PositionChanger::attitude_land_callback, this, std::placeholders::_1)); + } + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + void handle_height_message(const drone_services::msg::HeightData::SharedPtr message) { this->current_height = message->height; + if (!this->got_ready_drone_request) + { + this->start_height = message->height; + } + if (this->is_landing) + { + if (this->current_height <= this->start_height + HEIGHT_DELTA) + { + this->vehicle_control_request->control = 4; + auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request, + std::bind(&PositionChanger::vehicle_control_land_request_callback, this, std::placeholders::_1)); + } + } } /** @@ -386,6 +441,15 @@ public: } } + void handle_land_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + this->is_landing = true; + response->is_landing = this->is_landing; + } + void handle_ready_drone_request( const std::shared_ptr request_header, const std::shared_ptr request, @@ -453,7 +517,7 @@ private: rclcpp::Service::SharedPtr move_position_service; rclcpp::Service::SharedPtr ready_drone_service; - + rclcpp::Service::SharedPtr land_service; rclcpp::TimerBase::SharedPtr lidar_health_timer; drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; @@ -469,6 +533,7 @@ private: float current_speed_y = 0; float current_speed_z = 0; float current_height = 0; + float start_height = -1; bool is_landing = false; bool move_direction_allowed[4] = {true}; // whether the drone can move in a certain direction float collision_prevention_weights[4] = {0}; // the amount to move away from an object in a certain direction if the drone is too close