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

add response

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-25 12:37:27 +02:00
parent e7dc035a83
commit 4532e33c7f
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -92,7 +92,7 @@ public:
this->trajectory_request->values[1] = this->current_speed_y;
this->trajectory_request->values[2] = this->current_speed_z;
this->trajectory_request->yaw = this->current_yaw;
auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request,std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
// if (rclcpp::spin_until_future_complete(this, trajectory_response) ==
// rclcpp::FutureReturnCode::SUCCESS)
@@ -168,8 +168,9 @@ 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 += request->angle * (M_PI / 180.0); // get the angle in radians
this->current_yaw += (request->angle % 360) * (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);
response->success = true;
}
/**
@@ -215,7 +216,7 @@ private:
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 current_yaw = 0; // in radians
float current_yaw = 0; // in radians
float current_speed_x = 0;
float current_speed_y = 0;
float current_speed_z = 0;