diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index f8e0d803..b826cac1 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -67,158 +67,156 @@ public: RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); } } -} -void send_trajectory_message() -{ - this->trajectory_request->x = this->current_speed_x; - this->trajectory_request->y = this->current_speed_y; - this->trajectory_request->z = this->current_z_speed; - this->trajectory_request->yaw = this->current_yaw; - auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); + void send_trajectory_message() + { + this->trajectory_request->x = this->current_speed_x; + this->trajectory_request->y = this->current_speed_y; + this->trajectory_request->z = this->current_z_speed; + this->trajectory_request->yaw = this->current_yaw; + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); - if (rclcpp::spin_until_future_complete(this, trajectory_response) == - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); - } -} - -/** - * @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 drone_services::msg::LidarReading::SharedPtr message) -{ - - if (message->sensor_1 > 0) - { - 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; + if (rclcpp::spin_until_future_complete(this, trajectory_response) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); + } } - for (int i = 0; i < 4; i++) + /** + * @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->lidar_imu_readings[i] = message->imu_data[i]; + 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])); } - check_move_direction_allowed(); -} + void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) + { -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]}; - this->current_yaw = get_yaw_angle(q); -} + if (message->sensor_1 > 0) + { + 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; + } -void handle_move_position_request( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - 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 - get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); -} + for (int i = 0; i < 4; i++) + { + this->lidar_imu_readings[i] = message->imu_data[i]; + } -/** - * @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); -} + check_move_direction_allowed(); + } -/** - * @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); -} + 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]}; + this->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) + { + 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 + 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::Subscription lidar_subscription; + rclcpp::Subscription odometry_subscription; -rclcpp::Client::SharedPtr trajectory_client; -rclcpp::Client::SharedPtr vehicle_control_client; + rclcpp::Client::SharedPtr trajectory_client; + rclcpp::Client::SharedPtr vehicle_control_client; -rclcpp::Service::SharedPtr move_position_service; + rclcpp::Service::SharedPtr move_position_service; -drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; -drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; + 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}; + 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)) + template + void wait_for_service(T client) { - if (!rclcpp::ok()) + while (!client->wait_for_service(1s)) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return 0; + 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_INFO(this->get_logger(), "service not available, waiting again..."); } -} -} +}; ; int main(int argc, char *argv[]) {