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

fix collision prevention

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-12 15:16:09 +02:00
parent b7cba3e2ec
commit 8cb03ec4e7
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -192,7 +192,6 @@ public:
*/ */
void send_trajectory_message() void send_trajectory_message()
{ {
check_move_direction_allowed();
this->trajectory_request->values[0] = this->current_speed_x; this->trajectory_request->values[0] = this->current_speed_x;
this->trajectory_request->values[1] = this->current_speed_y; this->trajectory_request->values[1] = this->current_speed_y;
this->trajectory_request->values[2] = this->current_speed_z; this->trajectory_request->values[2] = this->current_speed_z;
@@ -221,12 +220,13 @@ public:
*/ */
void apply_collision_weights() void apply_collision_weights()
{ {
if (this->current_speed_x > 0) // if moving forward if (this->current_speed_x > 0) // if moving forward
{ {
if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT])
{ {
RCLCPP_INFO(this->get_logger(), "Collision prevention front: %d", collision_prevention_weights[MOVE_DIRECTION_FRONT]) RCLCPP_INFO(this->get_logger(), "Collision prevention front: %d", collision_prevention_weights[MOVE_DIRECTION_FRONT])
this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_FRONT]; get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_FRONT], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
} }
} }
else // moving backward else // moving backward
@@ -234,7 +234,7 @@ public:
if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) if (!this->move_direction_allowed[MOVE_DIRECTION_BACK])
{ {
RCLCPP_INFO(this->get_logger(), "Collision prevention back: %d", collision_prevention_weights[MOVE_DIRECTION_BACK]) RCLCPP_INFO(this->get_logger(), "Collision prevention back: %d", collision_prevention_weights[MOVE_DIRECTION_BACK])
this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_BACK]; get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_BACK], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
} }
} }
if (this->current_speed_y > 0) // moving right if (this->current_speed_y > 0) // moving right
@@ -242,7 +242,7 @@ public:
if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT])
{ {
RCLCPP_INFO(this->get_logger(), "Collision prevention right: %d", collision_prevention_weights[MOVE_DIRECTION_RIGHT]) RCLCPP_INFO(this->get_logger(), "Collision prevention right: %d", collision_prevention_weights[MOVE_DIRECTION_RIGHT])
this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; get_x_y_with_rotation(0,collision_prevention_weights[MOVE_DIRECTION_RIGHT], this->current_yaw, &this->current_speed_x, &this->current_speed_y)
} }
} }
else // moving left else // moving left
@@ -250,7 +250,7 @@ public:
if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT])
{ {
RCLCPP_INFO(this->get_logger(), "Collision prevention left: %d", collision_prevention_weights[MOVE_DIRECTION_LEFT]) RCLCPP_INFO(this->get_logger(), "Collision prevention left: %d", collision_prevention_weights[MOVE_DIRECTION_LEFT])
this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT]; get_x_y_with_rotation(0,collision_prevention_weights[MOVE_DIRECTION_LEFT], this->current_yaw, &this->current_speed_x, &this->current_speed_y)
} }
} }
} }
@@ -435,6 +435,7 @@ public:
this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians
this->current_speed_z = request->up_down; 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); get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
check_move_direction_allowed();
RCLCPP_INFO(this->get_logger(), "Calculated speed x: %f, y: %f", 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(); send_trajectory_message();
response->success = true; response->success = true;