From d59da4f5ac33ed1304aa47ada2bd001f08f23922 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:42:59 +0200 Subject: [PATCH] add checking of request angle --- src/drone_controls/src/PositionChanger.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index fbbaf52b..4ad2f307 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -168,7 +168,13 @@ public: { 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); // TODO add check_move_direction_allowed results to this calculation - this->current_yaw += (float)(((double)request->angle % 360.0) * (M_PI / 180.0)); // get the angle in radians + 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 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();