diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index b6b27ab5..8a2a5cd3 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -7,6 +7,9 @@ #include #include +#include +#include + #define _USE_MATH_DEFINES #include @@ -42,21 +45,28 @@ public: auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1)); - // 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->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"); RCLCPP_INFO(this->get_logger(), "waiting for trajectory service..."); wait_for_service::SharedPtr>(this->trajectory_client); RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service..."); wait_for_service::SharedPtr>(this->vehicle_control_client); + RCLCPP_INFO(this->get_logger(), "waiting for failsafe service..."); + wait_for_service::SharedPtr>(this->failsafe_client); this->trajectory_request = std::make_shared(); this->vehicle_control_request = std::make_shared(); + this->failsafe_request = std::make_shared(); + + lidar_health_timer = this->create_wall_timer(1s, std::bind(&PositionChanger::check_lidar_health, this)); this->vehicle_control_request->control = DEFAULT_CONTROL_MODE; auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request, @@ -65,7 +75,7 @@ public: /** * @brief callback function for the vehicle control service - * + * * @param future the future object that holds the result of the service call */ void vehicle_control_service_callback(rclcpp::Client::SharedFuture future) @@ -83,7 +93,7 @@ public: /** * @brief callback function for the trajectory service - * + * * @param future the future object that holds the result of the service call */ void trajectory_message_callback(rclcpp::Client::SharedFuture future) @@ -99,9 +109,27 @@ public: } } + /** + * @brief callback function for the failsafe service + * + * @param future the future object that holds the result of the service call + */ + void failsafe_message_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + RCLCPP_INFO(this->get_logger(), "Set failsafe enabled: %d, message: ", future.get()->enabled, future.get()->message); + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + /** * @brief sends the current x, y and z speed and yaw to the trajectory service - * + * */ void send_trajectory_message() { @@ -115,8 +143,15 @@ public: auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); } + void enable_failsafe(std::string message) + { + this->failsafe_enabled = true; + this->failsafe_request->message = message; + auto failsafe_response = this->failsafe_client->async_send_request(this->failsafe_request, std::bind(&PositionChanger::failsafe_message_callback, this, std::placeholders::_1)); + } + /** - * @brief applies the collision prevention weights to the current x and y speed if the drone is too close to an object. + * @brief applies the collision prevention weights to the current x and y speed if the drone is too close to an object. * It moves the drone away from the object until it is far enough away * */ @@ -142,7 +177,6 @@ public: { this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; } - } else // moving left { @@ -153,7 +187,6 @@ public: } } - /** * @brief checks for every direction is an object is too close and if we can move in that direction. * If the object is too close to the drone, calculate the amount we need to move away from it @@ -178,15 +211,15 @@ public: apply_collision_weights(); } - /** * @brief Callback function for receiving new LIDAR data - * + * * @param message the message containing the LIDAR data */ void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) { + this->received_lidar_message = true; if (message->sensor_1 > 0) { this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1; @@ -213,10 +246,24 @@ public: } /** - * @brief Callback function for receiving the odometry data from the drone. - * This is used to get the current yaw angle before the drone takes off, - * to make sure it doesn't want to spin to a certain angle while taking off. + * @brief Checks if the LIDAR is still sending messages. If not, enable failsafe * + */ + void check_lidar_health() + { + if (!this->received_lidar_message) + { + RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe"); + enable_failsafe("No healthy connection to LIDAR!"); + } + this->received_lidar_message = false; + } + + /** + * @brief Callback function for receiving the odometry data from the drone. + * This is used to get the current yaw angle before the drone takes off, + * to make sure it doesn't want to spin to a certain angle while taking off. + * * @param message the message containing the odometry data */ void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message) @@ -227,7 +274,7 @@ public: /** * @brief Callback function for when the /drone/move_position service is called - * + * * @param request_header the header of the request * @param request the request containing the new parameters to move to * @param response the response to send back. true if the request was successful, false otherwise @@ -237,19 +284,24 @@ public: const std::shared_ptr request, const std::shared_ptr response) { - RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle); - if (request->angle > 360 || request->angle < -360) + if (!this->failsafe_enabled) { - RCLCPP_ERROR(this->get_logger(), "Angle is not in range [-360, 360]"); + RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle); + if (request->angle > 360 || request->angle < -360) + { + RCLCPP_ERROR(this->get_logger(), "Angle is not in range [-360, 360]"); + response->success = false; + return; + } + this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians + this->current_speed_z = request->up_down; + get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); + 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 { response->success = false; - return; } - this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians - this->current_speed_z = request->up_down; - get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); - 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; } /** @@ -283,16 +335,21 @@ public: } private: + rclcpp::Publisher::SharedPtr failsafe_publisher; rclcpp::Subscription::SharedPtr lidar_subscription; rclcpp::Subscription::SharedPtr odometry_subscription; rclcpp::Client::SharedPtr trajectory_client; rclcpp::Client::SharedPtr vehicle_control_client; + rclcpp::Client::SharedPtr failsafe_client; rclcpp::Service::SharedPtr move_position_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; 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 @@ -300,8 +357,10 @@ private: float current_speed_x = 0; float current_speed_y = 0; float current_speed_z = 0; - bool move_direction_allowed[4] = {true}; // whether the drone can move in a certain direction + 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 + bool failsafe_enabled = false; + bool received_lidar_message = false; /** * @brief waits for a service to be available