From cf05320315710e08d28279cb62f981e399443feb Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 25 Jan 2024 23:54:10 +0000 Subject: [PATCH] [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../nav2_costmap_2d/inflation_layer.hpp | 22 +++++++++ .../nav2_smac_planner/collision_checker.hpp | 13 ++++- .../include/nav2_smac_planner/node_2d.hpp | 4 +- .../include/nav2_smac_planner/node_hybrid.hpp | 20 ++++++-- .../nav2_smac_planner/node_lattice.hpp | 8 ++-- nav2_smac_planner/src/a_star.cpp | 5 +- nav2_smac_planner/src/analytic_expansion.cpp | 6 +-- nav2_smac_planner/src/collision_checker.cpp | 8 +++- nav2_smac_planner/src/node_2d.cpp | 3 +- nav2_smac_planner/src/node_hybrid.cpp | 47 ++++++++++++++++--- nav2_smac_planner/src/node_lattice.cpp | 3 +- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 5 +- .../src/smac_planner_lattice.cpp | 2 +- nav2_smac_planner/test/test_a_star.cpp | 32 +++++++++++-- .../test/test_collision_checker.cpp | 42 ++++++++++++++--- nav2_smac_planner/test/test_node2d.cpp | 20 ++++++-- nav2_smac_planner/test/test_nodehybrid.cpp | 35 +++++++++++--- nav2_smac_planner/test/test_nodelattice.cpp | 27 +++++++++-- nav2_smac_planner/test/test_smoother.cpp | 9 +++- 20 files changed, 254 insertions(+), 59 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 0ce4b008d9..b83010c62d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -41,10 +41,13 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" namespace nav2_costmap_2d { @@ -161,6 +164,25 @@ class InflationLayer : public Layer return cost; } + static std::shared_ptr getInflationLayer( + std::shared_ptr & costmap_ros, + const std::string layer_name = "") + { + const auto layered_costmap = costmap_ros->getLayeredCostmap(); + for (auto layer = layered_costmap->getPlugins()->begin(); + layer != layered_costmap->getPlugins()->end(); + ++layer) + { + auto inflation_layer = std::dynamic_pointer_cast(*layer); + if (inflation_layer) { + if (layer_name.empty() || inflation_layer->getName() == layer_name) { + return inflation_layer; + } + } + } + return nullptr; + } + // Provide a typedef to ease future code maintenance typedef std::recursive_mutex mutex_t; diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index c1878a8863..53e019a3ae 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -11,8 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. Reserved. + #include +#include + #include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_smac_planner/constants.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -39,7 +43,7 @@ class GridCollisionChecker * orientations for to speed up collision checking */ GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap, + std::shared_ptr costmap, unsigned int num_quantizations, rclcpp_lifecycle::LifecycleNode::SharedPtr node); @@ -103,6 +107,12 @@ class GridCollisionChecker return angles_; } + /** + * @brief Get costmap ros object for inflation layer params + * @return Costmap ros + */ + std::shared_ptr getCostmapROS() {return costmap_ros_;} + private: /** * @brief Check if value outside the range @@ -114,6 +124,7 @@ class GridCollisionChecker bool outsideRange(const unsigned int & max, const float & value); protected: + std::shared_ptr costmap_ros_; std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; float footprint_cost_; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 392e325fac..10a975577a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -223,13 +223,11 @@ class Node2D * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new - * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates, - const nav2_costmap_2d::Costmap2D * costmap); + const Coordinates & goal_coordinates); /** * @brief Initialize the neighborhood to be used in A* diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 8ba117e80e..c46a947186 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -31,6 +31,8 @@ #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" namespace nav2_smac_planner { @@ -360,13 +362,11 @@ class NodeHybrid * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new - * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates, - const nav2_costmap_2d::Costmap2D * costmap); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models @@ -423,14 +423,22 @@ class NodeHybrid /** * @brief reset the obstacle heuristic state - * @param costmap Costmap to use + * @param costmap_ros Costmap to use * @param goal_coords Coordinates to start heuristic expansion at */ static void resetObstacleHeuristic( - nav2_costmap_2d::Costmap2D * costmap, + std::shared_ptr costmap_ros, const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y); + /** + * @brief Using the inflation layer, find the footprint's adjusted cost + * if the robot is non-circular + * @param cost Cost to adjust + * @return float Cost adjusted + */ + static float adjustedFootprintCost(const float & cost); + /** * @brief Retrieve all valid neighbors of a node. * @param validity_checker Functor for state validity checking @@ -462,6 +470,8 @@ class NodeHybrid static ObstacleHeuristicQueue obstacle_heuristic_queue; static nav2_costmap_2d::Costmap2D * sampled_costmap; + static std::shared_ptr costmap_ros; + static std::shared_ptr inflation_layer; static CostmapDownsampler downsampler; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 807e068c88..99f50d6a92 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -312,13 +312,11 @@ class NodeLattice * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new - * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates, - const nav2_costmap_2d::Costmap2D * costmap); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models @@ -355,12 +353,12 @@ class NodeLattice * @param goal_coords Coordinates to start heuristic expansion at */ static void resetObstacleHeuristic( - nav2_costmap_2d::Costmap2D * costmap, + std::shared_ptr costmap_ros, const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y) { // State Lattice and Hybrid-A* share this heuristics - NodeHybrid::resetObstacleHeuristic(costmap, start_x, start_y, goal_x, goal_y); + NodeHybrid::resetObstacleHeuristic(costmap_ros, start_x, start_y, goal_x, goal_y); } /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index e57db9952a..ebade0f4aa 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -207,7 +207,8 @@ void AStarAlgorithm::setGoal( throw std::runtime_error("Start must be set before goal."); } - NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my); + NodeT::resetObstacleHeuristic( + _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } _goal_coordinates = goal_coords; @@ -403,7 +404,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates, _costmap); + node_coords, _goal_coordinates); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 10cb3bdd5a..5dbe852a3b 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -57,12 +57,12 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic _motion_model == MotionModel::STATE_LATTICE) { // See if we are closer and should be expanding more often - auto costmap = _collision_checker->getCostmap(); const Coordinates node_coords = - NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size); + NodeT::getCoords( + current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose, costmap))); + static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index c31c836e64..044e4a8a31 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -18,16 +18,20 @@ namespace nav2_smac_planner { GridCollisionChecker::GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap, + std::shared_ptr costmap_ros, unsigned int num_quantizations, rclcpp_lifecycle::LifecycleNode::SharedPtr node) -: FootprintCollisionChecker(costmap) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) { if (node) { clock_ = node->get_clock(); logger_ = node->get_logger(); } + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } + // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(num_quantizations); angles_.reserve(num_quantizations); diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 14a901aded..07e16dda64 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -81,8 +81,7 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates, - const nav2_costmap_2d::Costmap2D * /*costmap*/) + const Coordinates & goal_coordinates) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 24db0e5fc7..e63ce5cddc 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -41,6 +41,9 @@ HybridMotionTable NodeHybrid::motion_table; float NodeHybrid::size_lookup = 25; LookupTable NodeHybrid::dist_heuristic_lookup_table; nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr; +std::shared_ptr NodeHybrid::costmap_ros = nullptr; +std::shared_ptr NodeHybrid::inflation_layer = nullptr; + CostmapDownsampler NodeHybrid::downsampler; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; @@ -434,8 +437,7 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords, - const nav2_costmap_2d::Costmap2D * /*costmap*/) + const Coordinates & goal_coords) { const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); @@ -478,7 +480,7 @@ inline float distanceHeuristic2D( } void NodeHybrid::resetObstacleHeuristic( - nav2_costmap_2d::Costmap2D * costmap, + std::shared_ptr costmap_ros_i, const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y) { @@ -486,10 +488,12 @@ void NodeHybrid::resetObstacleHeuristic( // the planner considerably to search through 75% less cells with no detectable // erosion of path quality after even modest smoothing. The error would be no more // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality - sampled_costmap = costmap; + costmap_ros = costmap_ros_i; + inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros); + sampled_costmap = costmap_ros->getCostmap(); if (motion_table.downsample_obstacle_heuristic) { std::weak_ptr ptr; - downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0, true); + downsampler.on_configure(ptr, "fake_frame", "fake_topic", sampled_costmap, 2.0, true); downsampler.on_activate(); sampled_costmap = downsampler.downsample(2.0); } @@ -530,6 +534,29 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } +float NodeHybrid::adjustedFootprintCost(const float & cost) +{ + if (!inflation_layer) { + return cost; + } + + const auto layered_costmap = costmap_ros->getLayeredCostmap(); + const float scale_factor = inflation_layer->getCostScalingFactor(); + const float min_radius = layered_costmap->getInscribedRadius(); + float dist_to_obj = (scale_factor * min_radius - log(cost) + log(253.0f)) / scale_factor; + + // Subtract minimum radius for edge cost + dist_to_obj -= min_radius; + if (dist_to_obj < 0.0f) { + dist_to_obj = 0.0f; + } + + // Compute cost at this value + return static_cast( + inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); +} + + float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, const Coordinates & goal_coords, @@ -537,6 +564,7 @@ float NodeHybrid::getObstacleHeuristic( { // If already expanded, return the cost const unsigned int size_x = sampled_costmap->getSizeInCellsX(); + const bool is_circular = costmap_ros->getUseRadius(); // Divided by 2 due to downsampled costmap. unsigned int start_y, start_x; @@ -606,7 +634,14 @@ float NodeHybrid::getObstacleHeuristic( // if neighbor path is better and non-lethal, set new cost and add to queue if (new_idx < size_x * size_y) { cost = static_cast(sampled_costmap->getCost(new_idx)); - if (cost >= INSCRIBED) { + + if (!is_circular) { + // Adjust cost value if using SE2 footprint checks + cost = adjustedFootprintCost(cost); + if (cost >= OCCUPIED) { + continue; + } + } else if (cost >= INSCRIBED) { continue; } diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 0076979c44..c3303e0a11 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -327,8 +327,7 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords, - const nav2_costmap_2d::Costmap2D * /*costmap*/) + const Coordinates & goal_coords) { // get obstacle heuristic value const float obstacle_heuristic = getObstacleHeuristic( diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 77650a61f4..70f8852f28 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -108,7 +108,7 @@ void SmacPlanner2D::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node); + _collision_checker = GridCollisionChecker(costmap_ros, 1 /*for 2D, most be 1*/, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), true /*for 2D, most use radius*/, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 12d4c2976f..b0012c9e2b 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -36,6 +36,7 @@ SmacPlannerHybrid::SmacPlannerHybrid() _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), + _costmap_ros(nullptr), _costmap_downsampler(nullptr) { } @@ -215,7 +216,7 @@ void SmacPlannerHybrid::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); + _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -679,7 +680,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize collision checker if (reinit_collision_checker) { - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); + _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index a5f16bcdda..854ec3b7b9 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -181,7 +181,7 @@ void SmacPlannerLattice::configure( // increments causing "wobbly" checks that could cause larger robots to virtually show collisions // in valid configurations. This approximation helps to bound orientation error for all checks // in exchange for slight inaccuracies in the collision headings in terminal search states. - _collision_checker = GridCollisionChecker(_costmap, 72u, node); + _collision_checker = GridCollisionChecker(_costmap_ros, 72u, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius(), diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 730193f802..13c2c80298 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -61,9 +61,15 @@ TEST(AStarTest, test_a_star_2d) } } + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + // functional case testing std::unique_ptr checker = - std::make_unique(costmapA, 1, lnode); + std::make_unique(costmap_ros, 1, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); @@ -149,8 +155,14 @@ TEST(AStarTest, test_a_star_se2) } } + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, size_theta, lnode); + std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -217,8 +229,14 @@ TEST(AStarTest, test_a_star_lattice) } } + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, size_theta, lnode); + std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -268,8 +286,14 @@ TEST(AStarTest, test_se2_single_pose_path) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, size_theta, lnode); + std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 84f2d5b39d..dd4e032a51 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -51,7 +51,13 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmap_; + + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -64,7 +70,13 @@ TEST(collision_footprint, test_point_cost) auto node = std::make_shared("testB"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmap_; + + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); @@ -79,7 +91,13 @@ TEST(collision_footprint, test_world_to_map) auto node = std::make_shared("testC"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmap_; + + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); @@ -92,7 +110,7 @@ TEST(collision_footprint, test_world_to_map) EXPECT_NEAR(cost, 0.0, 0.001); - costmap_->setCost(50, 50, 200); + costmap->setCost(50, 50, 200); collision_checker.worldToMap(5.0, 5.0, x, y); collision_checker.inCollision(x, y, 0.0, false); @@ -126,7 +144,13 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmap_; + + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); @@ -167,7 +191,13 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmap_; + + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index eede15fa84..3671594855 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -36,8 +36,15 @@ TEST(Node2DTest, test_node_2d) { auto node = std::make_shared("test"); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = costmapA; + std::unique_ptr checker = - std::make_unique(&costmapA, 72, node); + std::make_unique(costmap_ros, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -69,7 +76,7 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; @@ -123,8 +130,15 @@ TEST(Node2DTest, test_node_2d_neighbors) EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = costmapA; + std::unique_ptr checker = - std::make_unique(&costmapA, 72, lnode); + std::make_unique(costmap_ros, 72, lnode); unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 81fc62a236..e0b6ceef0a 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -58,8 +58,15 @@ TEST(NodeHybridTest, test_node_hybrid) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, 72, node); + std::make_unique(costmap_ros, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -178,8 +185,15 @@ TEST(NodeHybridTest, test_obstacle_heuristic) costmapA->setCost(i, j, 254); } } + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, 72, node); + std::make_unique(costmap_ros, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid testA(0); @@ -194,10 +208,10 @@ TEST(NodeHybridTest, test_obstacle_heuristic) // first block the high-cost passage to make sure the cost spreads through the better path for (unsigned int j = 61; j <= 70; ++j) { - costmapA->setCost(50, j, 254); + costmap->setCost(50, j, 254); } nav2_smac_planner::NodeHybrid::resetObstacleHeuristic( - costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y); + costmap_ros, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y); float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic( testA.pose, testB.pose, @@ -209,10 +223,10 @@ TEST(NodeHybridTest, test_obstacle_heuristic) // (it should, since the unblocked narrow path will have higher cost than the wide one // and thus lower bound of the path cost should be unchanged) for (unsigned int j = 61; j <= 70; ++j) { - costmapA->setCost(50, j, 250); + costmap->setCost(50, j, 250); } nav2_smac_planner::NodeHybrid::resetObstacleHeuristic( - costmapA, + costmap_ros, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y); float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic( testA.pose, @@ -336,8 +350,15 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01); nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = costmapA; + std::unique_ptr checker = - std::make_unique(&costmapA, 72, lnode); + std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); std::function neighborGetter = diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index f64f022f71..b3eadab5f9 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -221,8 +221,15 @@ TEST(NodeLatticeTest, test_node_lattice) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, 72, node); + std::make_unique(costmap_ros, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test node valid and cost @@ -285,8 +292,15 @@ TEST(NodeLatticeTest, test_get_neighbors) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + std::unique_ptr checker = - std::make_unique(costmapA, 72, lnode); + std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); std::function neighborGetter = @@ -336,8 +350,15 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D( 40, 40, 0.05, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmapi = costmap_ros->getCostmap(); + *costmapi = *costmap; + std::unique_ptr checker = - std::make_unique(costmap, 72, lnode); + std::make_unique(costmap_ros, 72, lnode); // Make some custom asymmetrical footprint nav2_costmap_2d::Footprint footprint; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index acf709c282..3844923498 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -94,8 +94,15 @@ TEST(SmootherTest, test_full_smoother) a_star.initialize( false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmapi = costmap_ros->getCostmap(); + *costmapi = *costmap; + std::unique_ptr checker = - std::make_unique(costmap, size_theta, node); + std::make_unique(costmap_ros, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // Create A* search to smooth