sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add failsafe landing

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-26 22:05:50 +02:00
parent aaa7bce6b7
commit 90f3b55dc9
1 changed files with 83 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
src/drone_controls/src/PositionChanger.cpp
View File

@@ -7,6 +7,9 @@
#include <drone_services/srv/set_trajectory.hpp>
#include <drone_services/srv/move_position.hpp>
#include <drone_services/srv/enable_failsafe.hpp>
#include <drone_services/msg/failsafe_msg.hpp>
#define _USE_MATH_DEFINES
#include <cmath>
@@ -42,21 +45,28 @@ public:
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
this->lidar_subscription = this->create_subscription<drone_services::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1));
// vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/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_services::msg::FailsafeMsg>("/drone/failsafe", 10);
this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
this->vehicle_control_client = this->create_client<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control");
this->failsafe_client = this->create_client<drone_services::srv::EnableFailsafe>("/drone/enable_failsafe");
RCLCPP_INFO(this->get_logger(), "waiting for trajectory service...");
wait_for_service<rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr>(this->trajectory_client);
RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service...");
wait_for_service<rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr>(this->vehicle_control_client);
RCLCPP_INFO(this->get_logger(), "waiting for failsafe service...");
wait_for_service<rclcpp::Client<drone_services::srv::EnableFailsafe>::SharedPtr>(this->failsafe_client);
this->trajectory_request = std::make_shared<drone_services::srv::SetTrajectory::Request>();
this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
this->failsafe_request = std::make_shared<drone_services::srv::EnableFailsafe::Request>();
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,
@@ -99,6 +109,24 @@ 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<drone_services::srv::EnableFailsafe>::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
*
@@ -115,6 +143,13 @@ 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.
* 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,7 +211,6 @@ public:
apply_collision_weights();
}
/**
* @brief Callback function for receiving new LIDAR data
*
@@ -187,6 +219,7 @@ public:
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;
@@ -212,6 +245,20 @@ public:
check_move_direction_allowed();
}
/**
* @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,
@@ -236,6 +283,8 @@ public:
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::MovePosition::Request> request,
const std::shared_ptr<drone_services::srv::MovePosition::Response> response)
{
if (!this->failsafe_enabled)
{
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)
@@ -250,6 +299,9 @@ 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 {
response->success = false;
}
}
/**
@@ -283,16 +335,21 @@ public:
}
private:
rclcpp::Publisher<drone_services::msg::FailsafeMsg>::SharedPtr failsafe_publisher;
rclcpp::Subscription<drone_services::msg::LidarReading>::SharedPtr lidar_subscription;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr odometry_subscription;
rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_client;
rclcpp::Client<drone_services::srv::EnableFailsafe>::SharedPtr failsafe_client;
rclcpp::Service<drone_services::srv::MovePosition>::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
@@ -302,6 +359,8 @@ private:
float current_speed_z = 0;
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