From 9f9b0e33927b1c5d0d967984b6efd4ad21e11b43 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 20:02:37 +0200 Subject: [PATCH] add service --- src/drone_controls/src/PositionChanger.cpp | 239 ++++++++++++--------- src/drone_services/CMakeLists.txt | 4 + src/drone_services/srv/MovePosition.srv | 6 + 3 files changed, 147 insertions(+), 102 deletions(-) create mode 100644 src/drone_services/srv/MovePosition.srv diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 4d70aec9..aebf429d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -1,10 +1,11 @@ #include "rclcpp/rclcpp.hpp" -#include "px4_msgs/msg/VehicleOdometry.msg" -#include "object_detection/msg/LidarReading.msg" -#include "height/msg/HeightData.msg" -#include "drone_services/srv/SetVehicleControl.srv" -#include "drone_services/srv/SetTrajectory/srv" +#include +#include +#include +#include +#include +#include #define _USE_MATH_DEFINES #include @@ -21,6 +22,8 @@ #define MIN_DISTANCE 1.0 // in meters +#define DEFAULT_CONTROL_MODE 4 // default velocity control bitmask + using namespace std::chrono_literals; struct Quaternion @@ -38,6 +41,7 @@ public: this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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->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->trajectory_client = this->create_client("/drone/set_trajectory"); this->vehicle_control_client = this->create_client("/drone/set_vehicle_control"); @@ -46,126 +50,157 @@ public: 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); - } - } - /** - * @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 - */ - void check_move_direction_allowed() - { - this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE; - this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE; - this->move_direction_allowed[MOVE_DIRECTION_BACK] = this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE; - this->move_direction_allowed[MOVE_DIRECTION_RIGHT] = this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE; + this->trajectory_request = std::make_shared(); + this->vehicle_control_request = std::make_shared(); - // calculate the amount we need to move away from the object to be at the minimum distance - collision_prevention_weights[MOVE_DIRECTION_FRONT] = this->move_direction_allowed[MOVE_DIRECTION_FRONT] ? 0 - : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL])); - collision_prevention_weights[MOVE_DIRECTION_LEFT] = this->move_direction_allowed[MOVE_DIRECTION_LEFT] ? 0 - : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL])); - collision_prevention_weights[MOVE_DIRECTION_BACK] = this->move_direction_allowed[MOVE_DIRECTION_BACK] ? 0 - : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR])); - collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0 - : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); - } + this->vehicle_control_request->control_mode = DEFAULT_CONTROL_MODE; + auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request); - void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) - { - - if (message->sensor_1 > 0) + if (rclcpp::spin_until_future_complete(this, control_mode_response) == + rclcpp::FutureReturnCode::SUCCESS) { - this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1; + RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", control_mode_response.get()->success); } - if (message->sensor_2 > 0) + else { - this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2; + RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); } - if (message->sensor_3 > 0) - { - this->lidar_sensor_values[LIDAR_SENSOR_RL] = message->sensor_3; - } - if (message->sensor_4 > 0) - { - this->lidar_sensor_values[LIDAR_SENSOR_RR] = message->sensor_4; - } - - for (int i = 0; i < 4; i++) - { - this->lidar_imu_readings[i] = message->imu_data[i]; - } - - check_move_direction_allowed(); } +} - void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message) +/** + * @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 + */ +void +check_move_direction_allowed() +{ + this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_BACK] = this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_RIGHT] = this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE; + + // calculate the amount we need to move away from the object to be at the minimum distance + collision_prevention_weights[MOVE_DIRECTION_FRONT] = this->move_direction_allowed[MOVE_DIRECTION_FRONT] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL])); + collision_prevention_weights[MOVE_DIRECTION_LEFT] = this->move_direction_allowed[MOVE_DIRECTION_LEFT] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL])); + collision_prevention_weights[MOVE_DIRECTION_BACK] = this->move_direction_allowed[MOVE_DIRECTION_BACK] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR])); + collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); +} + +void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) +{ + + if (message->sensor_1 > 0) { - Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]}; - current_yaw = get_yaw_angle(q); + this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1; + } + if (message->sensor_2 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2; + } + if (message->sensor_3 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_RL] = message->sensor_3; + } + if (message->sensor_4 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_RR] = message->sensor_4; } - /** - * @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 - * - * @param q the quaternion that holds the rotation - * @return the yaw angle in radians - */ - static float get_yaw_angle(Quaternion q) + for (int i = 0; i < 4; i++) { - float siny_cosp = 2 * (q.w * q.z + q.x * q.y); - float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); - return std::atan2(siny_cosp, cosy_cosp); + this->lidar_imu_readings[i] = message->imu_data[i]; } - /** - * @brief Get the new x and y coordinates after a rotation of yaw radians - * - * @param x the original x coordinate - * @param y the original y coordinate - * @param yaw the angle to rotate by in radians - * @param x_out the resulting x coordinate - * @param y_out the resulting y coordinate - */ - static void get_x_y_with_rotation(float x, float y, float yaw, float &x_out, float &y_out) - { - x_out = x * std::cos(yaw) - y * std::sin(yaw); - y_out = x * std::sin(yaw) + y * std::cos(yaw); - } + check_move_direction_allowed(); +} + +void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message) +{ + Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]}; + current_yaw = get_yaw_angle(q); +} + +void handle_move_position_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + //TODO add check_move_direction_allowed results to this calculation + this->current_yaw = request->angle * (M_PI / 180.0); // get the angle in radians + get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); +} + +/** + * @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 + * + * @param q the quaternion that holds the rotation + * @return the yaw angle in radians + */ +static float get_yaw_angle(Quaternion q) +{ + float siny_cosp = 2 * (q.w * q.z + q.x * q.y); + float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); +} + +/** + * @brief Get the new x and y coordinates after a rotation of yaw radians + * + * @param x the original x coordinate + * @param y the original y coordinate + * @param yaw the angle to rotate by in radians + * @param x_out the resulting x coordinate + * @param y_out the resulting y coordinate + */ +static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, float *y_res) +{ + *x_res = x * std::cos(yaw) - y * std::sin(yaw); + *y_res = x * std::sin(yaw) + y * std::cos(yaw); +} private: - rclcpp::Subscription lidar_subscription; - rclcpp::Subscription odometry_subscription; - rclcpp::Client::SharedPtr trajectory_client; - rclcpp::Client::SharedPtr vehicle_control_client; - drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; - drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; +rclcpp::Subscription lidar_subscription; +rclcpp::Subscription odometry_subscription; - float lidar_sensor_values[4]; // last distance measured by the lidar sensors - float lidar_imu_readings[4]; // last imu readings from the lidar sensors - float currrent_yaw = 0; // in radians - float current_x_speed = 0; - float current_y_speed = 0; - float current_z_speed = 0; - bool move_direction_allowed[4] = {true}; - float collision_prevention_weights[4] = {0}; +rclcpp::Client::SharedPtr trajectory_client; +rclcpp::Client::SharedPtr vehicle_control_client; - template - void wait_for_service(T client) +rclcpp::Service::SharedPtr move_position_service; + +drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; +drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; + +float lidar_sensor_values[4]; // last distance measured by the lidar sensors +float lidar_imu_readings[4]; // last imu readings from the lidar sensors +float currrent_yaw = 0; // in radians +float current_x_speed = 0; +float current_y_speed = 0; +float current_z_speed = 0; +bool move_direction_allowed[4] = {true}; +float collision_prevention_weights[4] = {0}; + +template +void wait_for_service(T client) +{ + while (!client->wait_for_service(1s)) { - while (!client->wait_for_service(1s)) + if (!rclcpp::ok()) { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return 0; - } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return 0; } - + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } -}; +} +} +; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 130f0f65..344655d3 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -29,7 +29,11 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ArmDrone.srv" "srv/DisarmDrone.srv" "srv/ControlRelais.srv" + "srv/MovePosition.srv" "msg/DroneControlMode.msg" + "msg/DroneArmStatus.msg" + "msg/DroneRouteStatus.msg" + "msg/DroneStatus.msg" ) if(BUILD_TESTING) diff --git a/src/drone_services/srv/MovePosition.srv b/src/drone_services/srv/MovePosition.srv new file mode 100644 index 00000000..25d82e00 --- /dev/null +++ b/src/drone_services/srv/MovePosition.srv @@ -0,0 +1,6 @@ +float front_back +float left_right +float up_down +float angle # in degrees +--- +bool success \ No newline at end of file