From 58270389a9cbc8b42beed5c5b3cb1133054ffd01 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 16 Jan 2024 10:59:14 +0000 Subject: [PATCH 1/7] prototype to test SE2 footprint H improvements --- nav2_smac_planner/CMakeLists.txt | 2 +- .../nav2_smac_planner/collision_checker.hpp | 10 +++- .../include/nav2_smac_planner/node_2d.hpp | 4 +- .../include/nav2_smac_planner/node_hybrid.hpp | 9 ++-- .../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 | 6 ++- nav2_smac_planner/src/node_2d.cpp | 3 +- nav2_smac_planner/src/node_hybrid.cpp | 52 +++++++++++++++++-- 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 | 4 +- .../src/smac_planner_lattice.cpp | 2 +- 14 files changed, 81 insertions(+), 35 deletions(-) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 4bbe5c55ca..edbb85a2e8 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -155,7 +155,7 @@ if(BUILD_TESTING) set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) + # add_subdirectory(test) endif() ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) 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 5933a6dc92..9ed263f45c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #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 +40,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 +104,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 +121,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_; double 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 4150deaee7..05f95d7fb6 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 cf9d341e54..37f1d98b38 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -360,13 +360,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,11 +421,11 @@ 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); @@ -462,6 +460,7 @@ class NodeHybrid static ObstacleHeuristicQueue obstacle_heuristic_queue; static nav2_costmap_2d::Costmap2D * sampled_costmap; + static std::shared_ptr costmap_ros; 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 577cc28e19..fcd0b7e7a4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -310,13 +310,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 @@ -353,12 +351,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 55fe35e531..b162804905 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 831bccbd9e..6323436bc4 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 20d288809d..207d569726 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -18,15 +18,17 @@ 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->getCostmap()) { if (node) { clock_ = node->get_clock(); logger_ = node->get_logger(); } + + costmap_ros_ = costmap_ros; // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(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 db2eba9f68..de801e6600 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -28,6 +28,7 @@ #include "ompl/base/spaces/ReedsSheppStateSpace.h" #include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" using namespace std::chrono; // NOLINT @@ -433,8 +434,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); @@ -477,7 +477,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) { @@ -485,10 +485,11 @@ 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; + sampled_costmap = costmap_ros->getCostmap(); + costmap_ros = costmap_ros_i; 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); } @@ -529,6 +530,43 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } +float adjustedFootprintCost( + const float & cost, std::shared_ptr costmap_ros) +{ + // TODO efficiency: have radius check to know if needed at H cost start, find + store inflation layer + // TODO architecture: abstract out object in utils to share with MPPI + // TODO reenable and fix tests + if (!costmap_ros->getUseRadius()) { + return cost; + } + + 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) { + continue; + } + + 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; + + // Compute cost at this value + return static_cast( + inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); + } + + // Didn't find an inflation layer; returning normal cost + return cost; +} + + float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, const Coordinates & goal_coords, @@ -605,6 +643,10 @@ 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)); + + // Adjust cost value if using SE2 footprint checks + cost = adjustedFootprintCost(cost, costmap_ros); + if (cost >= INSCRIBED) { continue; } diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index dfdf47ff4b..f4daea2cb4 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -324,8 +324,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 fc016ac545..540f8e10b0 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -206,7 +206,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(), @@ -654,7 +654,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 392c75d619..f3fd737e75 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -172,7 +172,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(), From bf175d059ad8a11849733a1d71fb1922ca928f75 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 16 Jan 2024 11:54:04 +0000 Subject: [PATCH 2/7] some fixes --- nav2_smac_planner/src/node_hybrid.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index de801e6600..c23a5973eb 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -42,6 +42,8 @@ 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; + CostmapDownsampler NodeHybrid::downsampler; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; @@ -485,8 +487,8 @@ 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_ros->getCostmap(); costmap_ros = costmap_ros_i; + sampled_costmap = costmap_ros->getCostmap(); if (motion_table.downsample_obstacle_heuristic) { std::weak_ptr ptr; downsampler.on_configure(ptr, "fake_frame", "fake_topic", sampled_costmap, 2.0, true); @@ -530,7 +532,7 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } -float adjustedFootprintCost( +inline float adjustedFootprintCost( const float & cost, std::shared_ptr costmap_ros) { // TODO efficiency: have radius check to know if needed at H cost start, find + store inflation layer From 626679154d162a35312de1ce692c943cbe294147 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 16 Jan 2024 12:20:12 +0000 Subject: [PATCH 3/7] fixed --- nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp | 2 ++ nav2_smac_planner/src/collision_checker.cpp | 6 ++++-- nav2_smac_planner/src/node_hybrid.cpp | 1 - nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 + 4 files changed, 7 insertions(+), 3 deletions(-) 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 37f1d98b38..cb7c6e75e9 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 { diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 207d569726..d7e4b24fdb 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -21,14 +21,16 @@ GridCollisionChecker::GridCollisionChecker( std::shared_ptr costmap_ros, unsigned int num_quantizations, rclcpp_lifecycle::LifecycleNode::SharedPtr node) -: FootprintCollisionChecker(costmap_ros->getCostmap()) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) { if (node) { clock_ = node->get_clock(); logger_ = node->get_logger(); } - costmap_ros_ = costmap_ros; + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(num_quantizations); diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index c23a5973eb..2f571e0bd9 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -28,7 +28,6 @@ #include "ompl/base/spaces/ReedsSheppStateSpace.h" #include "nav2_smac_planner/node_hybrid.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" using namespace std::chrono; // NOLINT diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 540f8e10b0..f30c3a28f8 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) { } From 916e8a8b0700fe46fa5963beb3f8cd231564f553 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 16 Jan 2024 13:00:14 +0000 Subject: [PATCH 4/7] invert logic --- nav2_smac_planner/src/node_hybrid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 2f571e0bd9..5c04c6637e 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -537,7 +537,7 @@ inline float adjustedFootprintCost( // TODO efficiency: have radius check to know if needed at H cost start, find + store inflation layer // TODO architecture: abstract out object in utils to share with MPPI // TODO reenable and fix tests - if (!costmap_ros->getUseRadius()) { + if (costmap_ros->getUseRadius()) { return cost; } From f68faf70431c3d46defa99c3ecd7edbfe3f083bc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Jan 2024 18:04:09 -0800 Subject: [PATCH 5/7] Working final prototype to be tested --- .../nav2_costmap_2d/inflation_layer.hpp | 20 +++++++ nav2_smac_planner/CMakeLists.txt | 2 +- .../nav2_smac_planner/collision_checker.hpp | 3 ++ .../include/nav2_smac_planner/node_hybrid.hpp | 9 ++++ nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 2 +- nav2_smac_planner/src/node_hybrid.cpp | 52 ++++++++----------- nav2_smac_planner/test/test_a_star.cpp | 32 ++++++++++-- .../test/test_collision_checker.cpp | 42 ++++++++++++--- nav2_smac_planner/test/test_node2d.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 18 ++++++- nav2_smac_planner/test/test_nodelattice.cpp | 27 ++++++++-- nav2_smac_planner/test/test_smoother.cpp | 9 +++- 13 files changed, 170 insertions(+), 50 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 aa0e0f13c2..f691b2c3d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -45,6 +45,7 @@ #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 +162,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/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index edbb85a2e8..4bbe5c55ca 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -155,7 +155,7 @@ if(BUILD_TESTING) set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(test) + add_subdirectory(test) endif() ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) 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 9ed263f45c..619cfa1b5c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -11,7 +11,10 @@ // 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" 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 cb7c6e75e9..540fff8673 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -431,6 +431,14 @@ class NodeHybrid 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 @@ -463,6 +471,7 @@ class NodeHybrid 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/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 6323436bc4..227f6a3b21 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -59,7 +59,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // See if we are closer and should be expanding more often const Coordinates node_coords = NodeT::getCoords( - current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + 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))); diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index d7e4b24fdb..eb8edea9f3 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -27,7 +27,7 @@ GridCollisionChecker::GridCollisionChecker( clock_ = node->get_clock(); logger_ = node->get_logger(); } - + if (costmap_ros) { costmap_ros_ = costmap_ros; } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 5c04c6637e..9f5df4c97d 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -42,6 +42,7 @@ 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; @@ -487,6 +488,7 @@ void NodeHybrid::resetObstacleHeuristic( // 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 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; @@ -531,40 +533,26 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } -inline float adjustedFootprintCost( - const float & cost, std::shared_ptr costmap_ros) +float NodeHybrid::adjustedFootprintCost(const float & cost) { - // TODO efficiency: have radius check to know if needed at H cost start, find + store inflation layer - // TODO architecture: abstract out object in utils to share with MPPI - // TODO reenable and fix tests - if (costmap_ros->getUseRadius()) { + if (!inflation_layer) { return cost; } 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) { - continue; - } - - 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; + 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; - // Compute cost at this value - return static_cast( - inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); + // Subtract minimum radius for edge cost + dist_to_obj -= min_radius; + if (dist_to_obj < 0.0f) { + dist_to_obj = 0.0f; } - // Didn't find an inflation layer; returning normal cost - return cost; + // Compute cost at this value + return static_cast( + inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); } @@ -575,6 +563,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; @@ -645,10 +634,13 @@ float NodeHybrid::getObstacleHeuristic( if (new_idx < size_x * size_y) { cost = static_cast(sampled_costmap->getCost(new_idx)); - // Adjust cost value if using SE2 footprint checks - cost = adjustedFootprintCost(cost, costmap_ros); - - 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/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index f048cc5550..a21de303db 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..5f26d773d7 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -69,7 +69,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'; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index c31dc0253b..866d04eadf 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -177,8 +177,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); @@ -335,8 +342,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 From 2c789deec44912f03d574053c2fd065b03dfd33a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Jan 2024 18:09:17 -0800 Subject: [PATCH 6/7] complete unit test conversions --- nav2_smac_planner/test/test_node2d.cpp | 18 ++++++++++++++++-- nav2_smac_planner/test/test_nodehybrid.cpp | 17 ++++++++++++----- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 5f26d773d7..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 @@ -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 866d04eadf..7ed61c4ec4 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -57,8 +57,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 @@ -200,10 +207,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, @@ -215,10 +222,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, From 07f81d1609fa4d97db335f150238d264408a5548 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Jan 2024 18:24:36 -0800 Subject: [PATCH 7/7] Update inflation_layer.hpp Signed-off-by: Steve Macenski --- nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 f691b2c3d3..bc422751f9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp"