From 34ff819b2bb3f159548c185ff79f4b01f311657f Mon Sep 17 00:00:00 2001 From: Matthew LeMay Date: Mon, 19 Oct 2020 17:12:50 -0400 Subject: [PATCH] Made the costmap a unique pointer and fixed the costmap class naming --- README.md | 1 + include/CostMap2D.hpp | 4 ++-- include/RTTStar.hpp | 5 +++-- main.cpp | 12 +++++++----- src/CostMap2D.cpp | 8 ++++---- src/RTTStar.cpp | 12 +++++++----- 6 files changed, 24 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index e328781..cd5a734 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ meson compile ## Usage ``` cd /build* +chmod +x example ./example Enter the size of the boarder square: 100 Enter the step size (i.e. double): 5.0 diff --git a/include/CostMap2D.hpp b/include/CostMap2D.hpp index ee9549f..4b906d2 100644 --- a/include/CostMap2D.hpp +++ b/include/CostMap2D.hpp @@ -13,9 +13,9 @@ typedef std::tuple Point2D; * A class which represents a 2D cost map of booleans which represent whether or * a coordinate is occupied. */ -class Costmap2D { +class CostMap2D { public: - Costmap2D() noexcept = default; + CostMap2D() noexcept = default; std::optional update(const Point2D &point, const bool &value); std::optional get(const Point2D &point) const; [[nodiscard]] bool contains(const Point2D &point) const; diff --git a/include/RTTStar.hpp b/include/RTTStar.hpp index 8fef806..f290176 100644 --- a/include/RTTStar.hpp +++ b/include/RTTStar.hpp @@ -2,6 +2,7 @@ #include // NOLINT [build/c++11] #include +#include #include #include #include @@ -13,7 +14,7 @@ */ class RTTStar { public: - explicit RTTStar(const Costmap2D &cost_map, double_t epsilon, + explicit RTTStar(const std::unique_ptr cost_map, double_t epsilon, double_t radius) noexcept; std::vector initial_path(const Point2D &start, const Point2D &goal); @@ -29,7 +30,7 @@ class RTTStar { Point2D steer(const Point2D &x_nearest, const Point2D &x); bool obstacle_free(const Point2D &x_nearest, const Point2D &x_new); - const Costmap2D &cost_map; + const std::unique_ptr cost_map; std::optional start_point, end_point; std::map costs; std::map relations; diff --git a/main.cpp b/main.cpp index 8875000..603d2bd 100644 --- a/main.cpp +++ b/main.cpp @@ -1,7 +1,9 @@ #include #include +#include #include #include +#include #include #include "CostMap2D.hpp" @@ -42,8 +44,8 @@ Point2D prompt_point() { * @param border_size The size of the borders. * @return The populated cost map. */ -Costmap2D make_cost_map(const size_t &border_size) { - Costmap2D cost_map; +std::unique_ptr make_cost_map(const size_t &border_size) { + auto cost_map = std::make_unique(); std::vector indices(border_size); std::iota(indices.begin(), indices.end(), 0); @@ -58,7 +60,7 @@ Costmap2D make_cost_map(const size_t &border_size) { std::for_each(indices.begin(), indices.end(), [&cost_map, &distribution, &generator](const auto &i) { if (distribution(generator)) { - cost_map.update(std::make_tuple(i, 0), true); + cost_map->update(std::make_tuple(i, 0), true); } }); }); @@ -69,7 +71,7 @@ Costmap2D make_cost_map(const size_t &border_size) { int main() { const auto border_size = prompt("Enter the size of the boarder square: "); - const auto cost_map = make_cost_map(border_size); + auto cost_map = make_cost_map(border_size); const auto step_size = prompt("Enter the step size (i.e. double): "); const auto radius = prompt( @@ -78,7 +80,7 @@ int main() { "Enter the amount of time the search should continue for after getting " "an initial path (in milliseconds): "); - RTTStar rtt_star(cost_map, step_size, radius); + RTTStar rtt_star(std::move(cost_map), step_size, radius); std::cout << "Where should the algorithm search start point be?" << std::endl; const auto start_point = prompt_point(); diff --git a/src/CostMap2D.cpp b/src/CostMap2D.cpp index 0256e46..b1d11b3 100644 --- a/src/CostMap2D.cpp +++ b/src/CostMap2D.cpp @@ -10,7 +10,7 @@ * @param value The value to associate with a point. * @return Returns the old value if there is one. */ -std::optional Costmap2D::update(const Point2D &point, const bool &value) { +std::optional CostMap2D::update(const Point2D &point, const bool &value) { // If the minimum values was previously set if (this->max_y.has_value() && this->max_x.has_value() && this->min_y.has_value() && this->min_x.has_value()) { @@ -43,7 +43,7 @@ std::optional Costmap2D::update(const Point2D &point, const bool &value) { * @param point The point to get the value of. * @return The value associated with a point. */ -std::optional Costmap2D::get(const Point2D &point) const { +std::optional CostMap2D::get(const Point2D &point) const { const auto loc = this->sparse_costmap.find(point); return loc != this->sparse_costmap.end() ? std::optional{loc->second} : std::nullopt; @@ -56,7 +56,7 @@ std::optional Costmap2D::get(const Point2D &point) const { * @return The random normal distribution of x && y values. */ std::tuple, std::uniform_int_distribution<>> -Costmap2D::sample_space(const std::vector &waypoints) const { +CostMap2D::sample_space(const std::vector &waypoints) const { int64_t s_min_x, s_min_y, s_max_x, s_max_y; // If there are no values, the sample space should not be infinite @@ -97,6 +97,6 @@ Costmap2D::sample_space(const std::vector &waypoints) const { * @param point The point to check for. * @return true if the value is in the cost map; false otherwise. */ -bool Costmap2D::contains(const Point2D &point) const { +bool CostMap2D::contains(const Point2D &point) const { return this->sparse_costmap.contains(point); } diff --git a/src/RTTStar.cpp b/src/RTTStar.cpp index 4cae93a..b8b1e8e 100644 --- a/src/RTTStar.cpp +++ b/src/RTTStar.cpp @@ -2,7 +2,9 @@ #include #include +#include #include +#include /** * Constructs the RRT Star class. @@ -12,9 +14,9 @@ * nearest point. * @param radius The radius to select nearby points from. */ -RTTStar::RTTStar(const Costmap2D &cost_map, double_t epsilon, +RTTStar::RTTStar(std::unique_ptr cost_map, double_t epsilon, double_t radius) noexcept - : cost_map(cost_map), eps(epsilon), radius(radius) { + : cost_map(std::move(cost_map)), eps(epsilon), radius(radius) { this->gen = std::mt19937(std::random_device()()); } @@ -35,7 +37,7 @@ std::vector RTTStar::initial_path(const Point2D &start, this->costs.insert(std::make_pair(start, 0.0)); std::tie(this->x_sample_space, this->y_sample_space) = - this->cost_map.sample_space(std::vector{start, goal}); + this->cost_map->sample_space(std::vector{start, goal}); while (!this->costs.contains(goal) && !this->relations.contains(goal)) { this->iterate(); @@ -76,7 +78,7 @@ void RTTStar::iterate() { std::make_tuple(x_sample_space(this->gen), y_sample_space(this->gen)); // Check if the random point is valid || has already been selected - if (this->cost_map.get(x) == std::nullopt && !costs.contains(x)) { + if (this->cost_map->get(x) == std::nullopt && !costs.contains(x)) { // Get the point nearest to the new point const Point2D x_nearest = this->nearest(x); // Adjust the distance of the new point @@ -201,7 +203,7 @@ bool RTTStar::obstacle_free(const Point2D &x_nearest, const Point2D &x_new) { // Check that none of the points are in the cost map return std::all_of( traversed.begin(), traversed.end(), - [this](const auto &pt) { return !this->cost_map.contains(pt); }); + [this](const auto &pt) { return !this->cost_map->contains(pt); }); } /**