|
| 1 | +// Copyright (c) 2025, Polymath Robotics |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef NAV2_ROUTE__CORNER_SMOOTHING_HPP_ |
| 16 | +#define NAV2_ROUTE__CORNER_SMOOTHING_HPP_ |
| 17 | + |
| 18 | +#include <vector> |
| 19 | +#include <cmath> |
| 20 | +#include <algorithm> |
| 21 | + |
| 22 | +#include "nav2_route/types.hpp" |
| 23 | +#include "nav2_route/utils.hpp" |
| 24 | + |
| 25 | +namespace nav2_route |
| 26 | +{ |
| 27 | +/** |
| 28 | + * @class nav2_route::CornerArc |
| 29 | + * @brief A class used to smooth corners defined by the edges and nodes |
| 30 | + * of the route graph. Used with path converter to output a corner smoothed plan |
| 31 | + */ |
| 32 | +class CornerArc |
| 33 | +{ |
| 34 | +public: |
| 35 | + /** |
| 36 | + * @brief A constructor for nav2_route::CornerArc |
| 37 | + * @param start start coordinate of corner to be smoothed |
| 38 | + * @param corner corner coordinate of corner to be smoothed |
| 39 | + * @param end end coordinate of corner to be smoothed |
| 40 | + * @param minimum_radius smoothing radius to fit to the corner |
| 41 | + */ |
| 42 | + CornerArc( |
| 43 | + const Coordinates & start, const Coordinates & corner, const Coordinates & end, |
| 44 | + float minimum_radius) |
| 45 | + { |
| 46 | + start_edge_length_ = hypotf(corner.x - start.x, corner.y - start.y); |
| 47 | + end_edge_length_ = hypotf(end.x - corner.x, end.y - corner.y); |
| 48 | + |
| 49 | + // invalid scenario would cause equations to blow up |
| 50 | + if (start_edge_length_ == 0.0 || end_edge_length_ == 0.0) {return;} |
| 51 | + |
| 52 | + float angle = getAngleBetweenEdges(start, corner, end); |
| 53 | + |
| 54 | + // cannot smooth a 0 (back and forth) or 180 (straight line) angle |
| 55 | + if (std::abs(angle) < 1E-6 || std::abs(angle - M_PI) < 1E-6) {return;} |
| 56 | + |
| 57 | + float tangent_length = minimum_radius / (std::tan(std::fabs(angle) / 2)); |
| 58 | + |
| 59 | + if (tangent_length < start_edge_length_ && tangent_length < end_edge_length_) { |
| 60 | + std::vector<float> start_edge_unit_tangent = |
| 61 | + {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_}; |
| 62 | + std::vector<float> end_edge_unit_tangent = |
| 63 | + {(end.x - corner.x) / end_edge_length_, (end.y - corner.y) / end_edge_length_}; |
| 64 | + |
| 65 | + float bisector_x = start_edge_unit_tangent[0] + end_edge_unit_tangent[0]; |
| 66 | + float bisector_y = start_edge_unit_tangent[1] + end_edge_unit_tangent[1]; |
| 67 | + float bisector_magnitude = std::sqrt(bisector_x * bisector_x + bisector_y * bisector_y); |
| 68 | + |
| 69 | + std::vector<float> unit_bisector = |
| 70 | + {bisector_x / bisector_magnitude, bisector_y / bisector_magnitude}; |
| 71 | + |
| 72 | + start_coordinate_.x = corner.x + start_edge_unit_tangent[0] * tangent_length; |
| 73 | + start_coordinate_.y = corner.y + start_edge_unit_tangent[1] * tangent_length; |
| 74 | + |
| 75 | + end_coordinate_.x = corner.x + end_edge_unit_tangent[0] * tangent_length; |
| 76 | + end_coordinate_.y = corner.y + end_edge_unit_tangent[1] * tangent_length; |
| 77 | + |
| 78 | + float bisector_length = minimum_radius / std::sin(angle / 2); |
| 79 | + |
| 80 | + circle_center_coordinate_.x = corner.x + unit_bisector[0] * bisector_length; |
| 81 | + circle_center_coordinate_.y = corner.y + unit_bisector[1] * bisector_length; |
| 82 | + |
| 83 | + valid_corner_ = true; |
| 84 | + } |
| 85 | + } |
| 86 | + |
| 87 | + /** |
| 88 | + * @brief A destructor for nav2_route::CornerArc |
| 89 | + */ |
| 90 | + ~CornerArc() = default; |
| 91 | + |
| 92 | + /** |
| 93 | + * @brief interpolates the arc for a path of certain density |
| 94 | + * @param max_angle_resolution Resolution to interpolate path to |
| 95 | + * @param poses Pose output |
| 96 | + */ |
| 97 | + void interpolateArc( |
| 98 | + const float & max_angle_resolution, |
| 99 | + std::vector<geometry_msgs::msg::PoseStamped> & poses) |
| 100 | + { |
| 101 | + std::vector<float> r_start{start_coordinate_.x - circle_center_coordinate_.x, |
| 102 | + start_coordinate_.y - circle_center_coordinate_.y}; |
| 103 | + std::vector<float> r_end{end_coordinate_.x - circle_center_coordinate_.x, |
| 104 | + end_coordinate_.y - circle_center_coordinate_.y}; |
| 105 | + float cross = r_start[0] * r_end[1] - r_start[1] * r_end[0]; |
| 106 | + float dot = r_start[0] * r_end[0] + r_start[1] * r_end[1]; |
| 107 | + float signed_angle = std::atan2(cross, dot); |
| 108 | + // lower limit for N is 1 to protect against divide by 0 |
| 109 | + int N = std::max(1, static_cast<int>(std::ceil(std::abs(signed_angle) / max_angle_resolution))); |
| 110 | + float angle_resolution = signed_angle / N; |
| 111 | + |
| 112 | + float x, y; |
| 113 | + for (int i = 0; i <= N; i++) { |
| 114 | + float angle = i * angle_resolution; |
| 115 | + x = circle_center_coordinate_.x + |
| 116 | + (r_start[0] * std::cos(angle) - r_start[1] * std::sin(angle)); |
| 117 | + y = circle_center_coordinate_.y + |
| 118 | + (r_start[0] * std::sin(angle) + r_start[1] * std::cos(angle)); |
| 119 | + poses.push_back(utils::toMsg(x, y)); |
| 120 | + } |
| 121 | + } |
| 122 | + |
| 123 | + /** |
| 124 | + * @brief return if a valid corner arc (one that doesn't overrun the edge lengths) is generated |
| 125 | + * @return if the a corner was able to be generated |
| 126 | + */ |
| 127 | + bool isCornerValid() const {return valid_corner_;} |
| 128 | + |
| 129 | + /** |
| 130 | + * @brief return the start coordinate of the corner arc |
| 131 | + * @return start coordinate of the arc |
| 132 | + */ |
| 133 | + Coordinates getCornerStart() const {return start_coordinate_;} |
| 134 | + |
| 135 | + /** |
| 136 | + * @brief return the end coordinate of the corner arc |
| 137 | + * @return end coordinate of the arc |
| 138 | + */ |
| 139 | + Coordinates getCornerEnd() const {return end_coordinate_;} |
| 140 | + |
| 141 | +protected: |
| 142 | + /** |
| 143 | + * @brief find the signed angle between a corner generated by 3 points |
| 144 | + * @param start start coordinate of corner to be smoothed |
| 145 | + * @param corner corner coordinate of corner to be smoothed |
| 146 | + * @param end end coordinate of corner to be smoothed |
| 147 | + * @return signed angle of the corner |
| 148 | + */ |
| 149 | + float getAngleBetweenEdges( |
| 150 | + const Coordinates & start, const Coordinates & corner, |
| 151 | + const Coordinates & end) |
| 152 | + { |
| 153 | + float start_dx = start.x - corner.x; |
| 154 | + float start_dy = start.y - corner.y; |
| 155 | + |
| 156 | + float end_dx = end.x - corner.x; |
| 157 | + float end_dy = end.y - corner.y; |
| 158 | + |
| 159 | + float angle = |
| 160 | + acos((start_dx * end_dx + start_dy * end_dy) / (start_edge_length_ * end_edge_length_)); |
| 161 | + |
| 162 | + return angle; |
| 163 | + } |
| 164 | + |
| 165 | +private: |
| 166 | + bool valid_corner_{false}; |
| 167 | + float start_edge_length_; |
| 168 | + float end_edge_length_; |
| 169 | + Coordinates start_coordinate_; |
| 170 | + Coordinates end_coordinate_; |
| 171 | + Coordinates circle_center_coordinate_; |
| 172 | +}; |
| 173 | + |
| 174 | +} // namespace nav2_route |
| 175 | + |
| 176 | +#endif // NAV2_ROUTE__CORNER_SMOOTHING_HPP_ |
0 commit comments