diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 3ce8d38f..d8bcc77c 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include @@ -25,6 +27,7 @@ #define MIN_DISTANCE 1.0 // in meters +#define CONTROL_MODE_ATTITUDE 4 // attitude control mode bitmask #define DEFAULT_CONTROL_MODE 16 // default velocity control bitmask // 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))) @@ -48,12 +51,14 @@ 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->failsafe_publisher = this->create_publisher("/drone/failsafe", 10); this->trajectory_client = this->create_client("/drone/set_trajectory"); this->vehicle_control_client = this->create_client("/drone/set_vehicle_control"); this->failsafe_client = this->create_client("/drone/enable_failsafe"); + this->arm_drone_client = this->create_client("/drone/arm"); RCLCPP_INFO(this->get_logger(), "waiting for trajectory service..."); wait_for_service::SharedPtr>(this->trajectory_client, "/drone/set_trajectory"); @@ -61,10 +66,13 @@ public: wait_for_service::SharedPtr>(this->vehicle_control_client, "/drone/set_vehicle_control"); RCLCPP_INFO(this->get_logger(), "waiting for failsafe service..."); wait_for_service::SharedPtr>(this->failsafe_client, "/drone/enable_failsafe"); + RCLCPP_INFO(this->get_logger(), "waiting for arm service..."); + wait_for_service::SharedPtr>(this->arm_drone_client, "/drone/arm"); this->trajectory_request = std::make_shared(); this->vehicle_control_request = std::make_shared(); this->failsafe_request = std::make_shared(); + this->arm_drone_request = std::make_shared(); lidar_health_timer = this->create_wall_timer(1s, std::bind(&PositionChanger::check_lidar_health, this)); @@ -84,6 +92,24 @@ public: if (status == std::future_status::ready) { RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success); + if (this->got_ready_drone_request && future.get()->success) + { + this->got_ready_drone_request = false; + auto arm_response = this->arm_drone_client->async_send_request(this->arm_drone_request, std::bind(&PositionChanger::arm_drone_service_callback, this, std::placeholders::_1)); + } + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + + void arm_drone_service_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + RCLCPP_INFO(this->get_logger(), "Arm result: %d", future.get()->success); } else { @@ -145,7 +171,7 @@ public: /** * @brief Enables the failsafe with the specified message - * + * * @param message the message indicating the cause of the failsafe */ void enable_failsafe(std::u16string message) @@ -224,7 +250,7 @@ public: void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) { this->has_received_first_lidar_message = true; - + this->received_lidar_message = true; if (message->sensor_1 > 0) { @@ -253,7 +279,7 @@ public: /** * @brief Checks if the LIDAR is still sending messages. If not, enable failsafe - * + * */ void check_lidar_health() { @@ -295,7 +321,8 @@ public: { if (!this->failsafe_enabled) { - if (!this->has_received_first_lidar_message) { + if (!this->has_received_first_lidar_message) + { this->enable_failsafe(u"Waiting for LIDAR timed out! Check the LIDAR USB connection and consider restarting the drone."); return; } @@ -312,11 +339,37 @@ public: RCLCPP_INFO(this->get_logger(), "Calculated speed x: %f, y: %f", this->current_speed_x, this->current_speed_y); send_trajectory_message(); response->success = true; - } else { + } + else + { response->success = false; } } + void handle_ready_drone_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if (this->failsafe_enabled) + { + response->success = false; + return; + } + if (!this->has_received_first_lidar_message) + { + this->enable_failsafe(u"Waiting for LIDAR timed out! Check the LIDAR USB connection and consider restarting the drone."); + response->success = false; + return; + } + this->got_ready_drone_request = true; + this->vehicle_control_request->control = CONTROL_MODE_ATTITUDE; + auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request, + std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1)); + // TODO set motors to spin at 30% + response->success = true; + } + /** * @brief Get the yaw angle from a specified normalized quaternion. * Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -355,14 +408,17 @@ private: rclcpp::Client::SharedPtr trajectory_client; rclcpp::Client::SharedPtr vehicle_control_client; rclcpp::Client::SharedPtr failsafe_client; + rclcpp::Client::SharedPtr arm_drone_client; rclcpp::Service::SharedPtr move_position_service; + rclcpp::Service::SharedPtr ready_done_service; rclcpp::TimerBase::SharedPtr lidar_health_timer; drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; drone_services::srv::EnableFailsafe::Request::SharedPtr failsafe_request; + drone_services::srv::ArmDrone::Request::SharedPtr arm_drone_request; float lidar_sensor_values[4]{MIN_DISTANCE}; // last distance measured by the lidar sensors float lidar_imu_readings[4]; // last imu readings from the lidar sensors @@ -376,6 +432,7 @@ private: bool received_lidar_message = false; int lidar_health_checks = 0; bool has_received_first_lidar_message = false; + bool got_ready_drone_request = false; /** * @brief waits for a service to be available