| 44 | } |
| 45 | |
| 46 | std::vector<nav2_costmap_2d::MapLocation> PolygonForCircle( |
| 47 | const nav2_costmap_2d::Costmap2D & costmap, |
| 48 | const Point & center, |
| 49 | const double & radius, |
| 50 | const std::size_t & resolution) |
| 51 | { |
| 52 | std::vector<nav2_costmap_2d::MapLocation> polygon; |
| 53 | |
| 54 | auto generate_point = [&, angle = 0.0]() mutable { |
| 55 | const double wx = (radius * std::cos(angle)) + center.x(); |
| 56 | const double wy = (radius * std::sin(angle)) + center.y(); |
| 57 | angle += (2 * M_PI) / resolution; |
| 58 | int mx = 0; |
| 59 | int my = 0; |
| 60 | costmap.worldToMapNoBounds(wx, wy, mx, my); |
| 61 | return nav2_costmap_2d::MapLocation{static_cast<unsigned int>(mx), |
| 62 | static_cast<unsigned int>(my)}; |
| 63 | }; |
| 64 | |
| 65 | std::generate_n(std::back_inserter(polygon), resolution, generate_point); |
| 66 | |
| 67 | return polygon; |
| 68 | } |
| 69 | |
| 70 | } // namespace astar_path_planner |