From 0589b6badefb3c794e6194f83dcd43924600d335 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 3 Nov 2023 15:32:50 -0700 Subject: [PATCH 1/8] a potential solution to smac shortcutting --- .../nav2_smac_planner/analytic_expansion.hpp | 3 +- .../nav2_smac_planner/node_lattice.hpp | 1 + nav2_smac_planner/src/analytic_expansion.cpp | 57 ++++++++++++++++--- nav2_smac_planner/src/node_hybrid.cpp | 8 +-- nav2_smac_planner/src/node_lattice.cpp | 9 +-- 5 files changed, 61 insertions(+), 17 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index fa1b2f1fdd6..9dc317dd8a4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -95,11 +95,12 @@ class AnalyticExpansion * @param node The node to start the analytic path from * @param goal The goal node to plan to * @param getter The function object that gets valid nodes from the graph + * @param state_space State space to use for computing analytic expansions * @return A set of analytically expanded nodes to the goal from current node, if possible */ AnalyticExpansionNodes getAnalyticPath( const NodePtr & node, const NodePtr & goal, - const NodeGetter & getter); + const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); /** * @brief Takes final analytic expansion and appends to current expanded node 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 7f6a82d31c7..b0b5ce6315e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -104,6 +104,7 @@ struct LatticeMotionTable float reverse_penalty; float travel_distance_reward; float rotation_penalty; + float min_turning_radius; bool allow_reverse_expansion; std::vector> motion_primitives; ompl::base::StateSpacePtr state_space; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 831bccbd9e0..c71d97cc277 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -80,7 +80,8 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_node, getter); + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); if (!analytic_nodes.empty()) { // If we have a valid path, attempt to refine it NodePtr node = current_node; @@ -94,7 +95,8 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic test_node->parent->parent->parent->parent->parent) { test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = getAnalyticPath(test_node, goal_node, getter); + refined_analytic_nodes = + getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); if (refined_analytic_nodes.empty()) { break; } @@ -105,6 +107,44 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } } + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible curvature + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traveral cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + score += distance * (1.0 + weight * normalized_cost); // TODO quadratic option, add `* normalized_cost` + } + return score; + }; + + float initial_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + float step = 0.10; // 10cm TODO make proportional to costmap resolution or a parameter? + while (true) { + min_turn_rad += step; + auto state_space = std::make_shared(min_turn_rad); // TODO set based on motion model + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= initial_score) { + analytic_nodes = refined_analytic_nodes; + } else { + break; + } + } + return setAnalyticPath(node, goal_node, analytic_nodes); } } @@ -120,10 +160,10 @@ template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, const NodePtr & goal, - const NodeGetter & node_getter) + const NodeGetter & node_getter, + const ompl::base::StateSpacePtr & state_space) { - static ompl::base::ScopedState<> from(node->motion_table.state_space), to( - node->motion_table.state_space), s(node->motion_table.state_space); + static ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); from[0] = node->pose.x; from[1] = node->pose.y; from[2] = node->motion_table.getAngleFromBin(node->pose.theta); @@ -131,7 +171,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionpose.y; to[2] = node->motion_table.getAngleFromBin(goal->pose.theta); - float d = node->motion_table.state_space->distance(from(), to()); + float d = state_space->distance(from(), to()); // If the length is too far, exit. This prevents unsafe shortcutting of paths // into higher cost areas far out from the goal itself, let search to the work of getting @@ -162,7 +202,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionmotion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + state_space->interpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); // Make sure in range [0, 2PI) theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; @@ -256,7 +296,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion(min_turning_radius); + state_space = std::make_shared(min_turning_radius); // Precompute projection deltas delta_xs.resize(projections.size()); @@ -260,7 +260,7 @@ void HybridMotionTable::initReedsShepp( } // Create the correct OMPL state space - state_space = std::make_unique(min_turning_radius); + state_space = std::make_shared(min_turning_radius); // Precompute projection deltas delta_xs.resize(projections.size()); @@ -747,10 +747,10 @@ void NodeHybrid::precomputeDistanceHeuristic( { // Dubin or Reeds-Shepp shortest distances if (motion_model == MotionModel::DUBIN) { - motion_table.state_space = std::make_unique( + motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); } else if (motion_model == MotionModel::REEDS_SHEPP) { - motion_table.state_space = std::make_unique( + motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); } else { throw std::runtime_error( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c852a96dd32..dc6309edd11 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -64,6 +64,7 @@ void LatticeMotionTable::initMotionModel( current_lattice_filepath = search_info.lattice_filepath; allow_reverse_expansion = search_info.allow_reverse_expansion; rotation_penalty = search_info.rotation_penalty; + min_turning_radius = search_info.minimum_turning_radius; // Get the metadata about this minimum control set lattice_metadata = getLatticeMetadata(current_lattice_filepath); @@ -77,10 +78,10 @@ void LatticeMotionTable::initMotionModel( if (!state_space) { if (!allow_reverse_expansion) { - state_space = std::make_unique( + state_space = std::make_shared( lattice_metadata.min_turning_radius); } else { - state_space = std::make_unique( + state_space = std::make_shared( lattice_metadata.min_turning_radius); } } @@ -421,10 +422,10 @@ void NodeLattice::precomputeDistanceHeuristic( { // Dubin or Reeds-Shepp shortest distances if (!search_info.allow_reverse_expansion) { - motion_table.state_space = std::make_unique( + motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); } else { - motion_table.state_space = std::make_unique( + motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); } motion_table.lattice_metadata = From b759a70fc076ddefa6b8fcb2da5da25cc0c37d53 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 3 Nov 2023 15:36:28 -0700 Subject: [PATCH 2/8] costmap reoslution --- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index c71d97cc277..1d9b1b1558f 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -132,7 +132,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic float initial_score = scoringFn(analytic_nodes); float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; - float step = 0.10; // 10cm TODO make proportional to costmap resolution or a parameter? + float step = 0.05; // 5cm TODO make proportional to costmap resolution or a parameter? while (true) { min_turn_rad += step; auto state_space = std::make_shared(min_turn_rad); // TODO set based on motion model From 172bcdc8a2882c32980cc115b58b0288942d27da Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 3 Nov 2023 16:37:03 -0700 Subject: [PATCH 3/8] some fixes --- nav2_smac_planner/src/analytic_expansion.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 1d9b1b1558f..23e94cadcd3 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -124,25 +124,30 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const float & weight = expansion[0].node->motion_table.cost_penalty; for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { normalized_cost = iter->node->getCost() / 252.0f; + // H-cost method score += distance * (1.0 + weight * normalized_cost); // TODO quadratic option, add `* normalized_cost` + // Cost-only method + // score += normalized_cost; } return score; + // return score / expansion.size(); }; - float initial_score = scoringFn(analytic_nodes); + float best_score = scoringFn(analytic_nodes); float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; - float step = 0.05; // 5cm TODO make proportional to costmap resolution or a parameter? - while (true) { + const float step = 0.2; // TODO make proportional to resolution or parameter? In grid coords + unsigned int i = 0; + while (i < 20) { // TODO what max to check? min_turn_rad += step; auto state_space = std::make_shared(min_turn_rad); // TODO set based on motion model - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); // TODO other types of splines? score = scoringFn(refined_analytic_nodes); - if (score <= initial_score) { + if (score <= best_score) { analytic_nodes = refined_analytic_nodes; - } else { - break; + best_score = score; } + i++; } return setAnalyticPath(node, goal_node, analytic_nodes); From ee9b38bae0aa4a32a7796af3d89c143651ddd3b4 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 8 Nov 2023 13:50:48 -0800 Subject: [PATCH 4/8] completed prototype --- .../nav2_smac_planner/node_lattice.hpp | 1 + nav2_smac_planner/src/analytic_expansion.cpp | 30 +++++++++++-------- nav2_smac_planner/src/node_lattice.cpp | 4 +++ 3 files changed, 22 insertions(+), 13 deletions(-) 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 b0b5ce6315e..6763b52f30f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -111,6 +111,7 @@ struct LatticeMotionTable std::vector trig_values; std::string current_lattice_filepath; LatticeMetadata lattice_metadata; + MotionModel motion_model = MotionModel::UNKNOWN; }; /** diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 23e94cadcd3..2a0acab61e6 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -108,9 +108,12 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible curvature + // So, we can attempt to refine it more by increasing the possible radius // higher than the minimum turning radius and use the best solution based on // a scoring function similar to that used in traveral cost estimation. + + // TODO continue to search and compare? + // TODO take original and patch up: curve fitting path minimize curvature? auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { if (expansion.size() < 2) { return std::numeric_limits::max(); @@ -118,36 +121,37 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic float score = 0.0; float normalized_cost = 0.0; + // Analytic expansions are consistently spaced const float distance = hypotf( expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); const float & weight = expansion[0].node->motion_table.cost_penalty; for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { normalized_cost = iter->node->getCost() / 252.0f; - // H-cost method - score += distance * (1.0 + weight * normalized_cost); // TODO quadratic option, add `* normalized_cost` - // Cost-only method - // score += normalized_cost; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); } return score; - // return score / expansion.size(); }; float best_score = scoringFn(analytic_nodes); float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; - const float step = 0.2; // TODO make proportional to resolution or parameter? In grid coords - unsigned int i = 0; - while (i < 20) { // TODO what max to check? - min_turn_rad += step; - auto state_space = std::make_shared(min_turn_rad); // TODO set based on motion model - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); // TODO other types of splines? + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); score = scoringFn(refined_analytic_nodes); if (score <= best_score) { analytic_nodes = refined_analytic_nodes; best_score = score; } - i++; } return setAnalyticPath(node, goal_node, analytic_nodes); diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index dc6309edd11..d34ffc8552f 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -80,9 +80,11 @@ void LatticeMotionTable::initMotionModel( if (!allow_reverse_expansion) { state_space = std::make_shared( lattice_metadata.min_turning_radius); + motion_model = MotionModel::DUBIN; } else { state_space = std::make_shared( lattice_metadata.min_turning_radius); + motion_model = MotionModel::REEDS_SHEPP; } } @@ -424,9 +426,11 @@ void NodeLattice::precomputeDistanceHeuristic( if (!search_info.allow_reverse_expansion) { motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); + motion_table.motion_model = MotionModel::DUBIN; } else { motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); + motion_table.motion_model = MotionModel::REEDS_SHEPP; } motion_table.lattice_metadata = LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath); From 44348980b449617fc06684fb18635ec8997704bc Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 9 Nov 2023 18:34:17 -0800 Subject: [PATCH 5/8] some fixes for collision detection and performance --- .../nav2_smac_planner/analytic_expansion.hpp | 2 +- .../nav2_smac_planner/collision_checker.hpp | 4 ++-- .../include/nav2_smac_planner/node_2d.hpp | 8 ++++---- .../include/nav2_smac_planner/node_hybrid.hpp | 8 ++++---- .../include/nav2_smac_planner/node_lattice.hpp | 8 ++++---- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 6 +++--- nav2_smac_planner/src/collision_checker.cpp | 14 +++++++------- 8 files changed, 26 insertions(+), 26 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 9dc317dd8a4..10c4224a077 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -73,7 +73,7 @@ class AnalyticExpansion * @brief Sets the collision checker and costmap to use in expansion validation * @param collision_checker Collision checker to use */ - void setCollisionChecker(GridCollisionChecker * collision_checker); + void setCollisionChecker(GridCollisionChecker * & collision_checker); /** * @brief Attempt an analytic path completion 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 5933a6dc92f..c1878a88639 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -116,10 +116,10 @@ class GridCollisionChecker protected: std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; - double footprint_cost_; + float footprint_cost_; bool footprint_is_radius_; std::vector angles_; - double possible_inscribed_cost_{-1}; + float possible_inscribed_cost_{-1}; rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; rclcpp::Clock::SharedPtr clock_; }; 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 4150deaee7a..392e325fac5 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -87,7 +87,7 @@ class Node2D * @brief Gets the accumulated cost at this node * @return accumulated cost */ - inline float & getAccumulatedCost() + inline float getAccumulatedCost() { return _accumulated_cost; } @@ -105,7 +105,7 @@ class Node2D * @brief Gets the costmap cost at this node * @return costmap cost */ - inline float & getCost() + inline float getCost() { return _cell_cost; } @@ -123,7 +123,7 @@ class Node2D * @brief Gets if cell has been visited in search * @param If cell was visited */ - inline bool & wasVisited() + inline bool wasVisited() { return _was_visited; } @@ -158,7 +158,7 @@ class Node2D * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int & getIndex() + inline unsigned int getIndex() { return _index; } 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 cf9d341e548..ed932a057e9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -217,7 +217,7 @@ class NodeHybrid * @brief Gets the accumulated cost at this node * @return accumulated cost */ - inline float & getAccumulatedCost() + inline float getAccumulatedCost() { return _accumulated_cost; } @@ -263,7 +263,7 @@ class NodeHybrid * @brief Gets the costmap cost at this node * @return costmap cost */ - inline float & getCost() + inline float getCost() { return _cell_cost; } @@ -272,7 +272,7 @@ class NodeHybrid * @brief Gets if cell has been visited in search * @param If cell was visited */ - inline bool & wasVisited() + inline bool wasVisited() { return _was_visited; } @@ -289,7 +289,7 @@ class NodeHybrid * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int & getIndex() + inline unsigned int getIndex() { return _index; } 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 6763b52f30f..c74c3a8d179 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -184,7 +184,7 @@ class NodeLattice * @brief Gets the accumulated cost at this node * @return accumulated cost */ - inline float & getAccumulatedCost() + inline float getAccumulatedCost() { return _accumulated_cost; } @@ -202,7 +202,7 @@ class NodeLattice * @brief Gets the costmap cost at this node * @return costmap cost */ - inline float & getCost() + inline float getCost() { return _cell_cost; } @@ -211,7 +211,7 @@ class NodeLattice * @brief Gets if cell has been visited in search * @param If cell was visited */ - inline bool & wasVisited() + inline bool wasVisited() { return _was_visited; } @@ -228,7 +228,7 @@ class NodeLattice * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int & getIndex() + inline unsigned int getIndex() { return _index; } diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d6c9db69bb3..c2a38154574 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -110,7 +110,7 @@ void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision _y_size = y_size; NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); } - _expander->setCollisionChecker(collision_checker); + _expander->setCollisionChecker(_collision_checker); } template diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 2a0acab61e6..1b955005496 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -41,7 +41,7 @@ AnalyticExpansion::AnalyticExpansion( template void AnalyticExpansion::setCollisionChecker( - GridCollisionChecker * collision_checker) + GridCollisionChecker * & collision_checker) { _collision_checker = collision_checker; } @@ -191,8 +191,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion(std::floor(d / sqrt_2)); AnalyticExpansionNodes possible_nodes; // When "from" and "to" are zero or one cell away, diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 20d288809da..4a1cf963d80 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -49,7 +49,7 @@ void GridCollisionChecker::setFootprint( const bool & radius, const double & possible_inscribed_cost) { - possible_inscribed_cost_ = possible_inscribed_cost; + possible_inscribed_cost_ = static_cast(possible_inscribed_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -106,8 +106,8 @@ bool GridCollisionChecker::inCollision( if (!footprint_is_radius_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision - footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); + footprint_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5), static_cast(y + 0.5))); if (footprint_cost_ < possible_inscribed_cost_) { if (possible_inscribed_cost_ > 0) { @@ -146,7 +146,7 @@ bool GridCollisionChecker::inCollision( current_footprint.push_back(new_pt); } - footprint_cost_ = footprintCost(current_footprint); + footprint_cost_ = static_cast(footprintCost(current_footprint)); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; @@ -156,15 +156,15 @@ bool GridCollisionChecker::inCollision( return footprint_cost_ >= OCCUPIED; } else { // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); + footprint_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5), static_cast(y + 0.5))); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return static_cast(footprint_cost_) >= INSCRIBED; + return footprint_cost_ >= INSCRIBED; } } From 0816f5faa7b317d5c68173ad9db99b3f55b08c17 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 13 Nov 2023 17:53:41 -0800 Subject: [PATCH 6/8] completing shortcutting fix --- .../include/nav2_smac_planner/types.hpp | 2 + nav2_smac_planner/src/analytic_expansion.cpp | 72 +++++++++++++------ nav2_smac_planner/src/collision_checker.cpp | 4 +- nav2_smac_planner/src/node_lattice.cpp | 4 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 15 ++++ .../src/smac_planner_lattice.cpp | 15 ++++ 6 files changed, 88 insertions(+), 24 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index d199c677753..6baf51f4a4d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -43,6 +43,8 @@ struct SearchInfo float rotation_penalty{5.0}; float analytic_expansion_ratio{3.5}; float analytic_expansion_max_length{60.0}; + float analytic_expansion_max_cost{200.0}; + bool analytic_expansion_max_cost_override{false}; std::string lattice_filepath; bool cache_obstacle_heuristic{false}; bool allow_reverse_expansion{false}; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 1b955005496..f2a216f446c 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -111,28 +111,25 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // So, we can attempt to refine it more by increasing the possible radius // higher than the minimum turning radius and use the best solution based on // a scoring function similar to that used in traveral cost estimation. - - // TODO continue to search and compare? - // TODO take original and patch up: curve fitting path minimize curvature? auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); - } + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); - } - return score; - }; + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; float best_score = scoringFn(analytic_nodes); float score = std::numeric_limits::max(); @@ -208,6 +205,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion node_costs; + node_costs.reserve(num_intervals); // Check intermediary poses (non-goal, non-start) for (float i = 1; i < num_intervals; i++) { @@ -231,6 +230,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionisNodeValid(_traverse_unknown, _collision_checker) && next != prev) { // Save the node, and its previous coordinates in case we need to abort possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + node_costs.emplace_back(next->getCost()); prev = next; } else { // Abort @@ -245,6 +245,38 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion max_cost) { + // If any element is above the comfortable cost limit, check edge cases: + // (1) Check if goal is in greater than max_cost space requiring + // entering it, but only entering it on final approach, not in-and-out + // (2) Checks if goal is in normal space, but enters costed space unnecessarily + // mid-way through, skirting obstacle or in non-globally confined space + bool cost_exit_high_cost_region = false; + for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) { + const float & curr_cost = *iter; + if (curr_cost <= max_cost) { + cost_exit_high_cost_region = true; + } else if (curr_cost > max_cost && cost_exit_high_cost_region) { + failure = true; + break; + } + } + + // (3) Handle exception: there may be no other option close to goal + // if max cost is set too low (optional) + if (failure) { + if (d < 2.0f * M_PI * goal->motion_table.min_turning_radius && + _search_info.analytic_expansion_max_cost_override) + { + failure = false; + } + } + } + } + // Reset to initial poses to not impact future searches for (const auto & node_pose : possible_nodes) { const auto & n = node_pose.node; diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 4a1cf963d80..5fded662e16 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -107,7 +107,7 @@ bool GridCollisionChecker::inCollision( // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5), static_cast(y + 0.5))); if (footprint_cost_ < possible_inscribed_cost_) { if (possible_inscribed_cost_ > 0) { @@ -157,7 +157,7 @@ bool GridCollisionChecker::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5), static_cast(y + 0.5))); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index d34ffc8552f..f630b51eca8 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -426,11 +426,11 @@ void NodeLattice::precomputeDistanceHeuristic( if (!search_info.allow_reverse_expansion) { motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); - motion_table.motion_model = MotionModel::DUBIN; + motion_table.motion_model = MotionModel::DUBIN; } else { motion_table.state_space = std::make_shared( search_info.minimum_turning_radius); - motion_table.motion_model = MotionModel::REEDS_SHEPP; + motion_table.motion_model = MotionModel::REEDS_SHEPP; } motion_table.lattice_metadata = LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index d623ab61107..440dcb47cd8 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -125,6 +125,15 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_max_cost", rclcpp::ParameterValue(200.0)); + node->get_parameter( + name + ".analytic_expansion_max_cost", _search_info.analytic_expansion_max_cost); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_max_cost_override", rclcpp::ParameterValue(false)); + node->get_parameter( + name + ".analytic_expansion_max_cost_override", + _search_info.analytic_expansion_max_cost_override); nav2_util::declare_parameter_if_not_declared( node, name + ".use_quadratic_cost_penalty", rclcpp::ParameterValue(false)); node->get_parameter( @@ -532,6 +541,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); + } else if (name == _name + ".analytic_expansion_max_cost") { + reinit_a_star = true; + _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); } } else if (type == ParameterType::PARAMETER_BOOL) { if (name == _name + ".downsample_costmap") { @@ -552,6 +564,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para } else { _smoother.reset(); } + } else if (name == _name + ".analytic_expansion_max_cost_override") { + _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); + reinit_a_star = true; } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == _name + ".downsampling_factor") { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 8b46fc3ae74..65847d4b3b3 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -111,6 +111,15 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_max_cost", rclcpp::ParameterValue(200.0)); + node->get_parameter( + name + ".analytic_expansion_max_cost", _search_info.analytic_expansion_max_cost); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_max_cost_override", rclcpp::ParameterValue(false)); + node->get_parameter( + name + ".analytic_expansion_max_cost_override", + _search_info.analytic_expansion_max_cost_override); nav2_util::declare_parameter_if_not_declared( node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0)); node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m); @@ -388,6 +397,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); + } else if (name == _name + ".analytic_expansion_max_cost") { + reinit_a_star = true; + _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); } } else if (type == ParameterType::PARAMETER_BOOL) { if (name == _name + ".allow_unknown") { @@ -405,6 +417,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else { _smoother.reset(); } + } else if (name == _name + ".analytic_expansion_max_cost_override") { + _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); + reinit_a_star = true; } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == _name + ".max_iterations") { From 98e3502b994afa2d1ada68066047b9f862764f94 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 16 Nov 2023 07:55:47 -0800 Subject: [PATCH 7/8] updating tests --- nav2_smac_planner/test/test_a_star.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index b6107b141db..873446a7a17 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -168,8 +168,8 @@ TEST(AStarTest, test_a_star_se2) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); // check path is the right size and collision free - EXPECT_EQ(num_it, 3186); - EXPECT_EQ(path.size(), 64u); + EXPECT_EQ(num_it, 3146); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -233,8 +233,8 @@ TEST(AStarTest, test_a_star_lattice) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 26); - EXPECT_GT(path.size(), 47u); + EXPECT_EQ(num_it, 22); + EXPECT_GT(path.size(), 46u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } From d34a2c8f5118e62a8bd3b45b8885c17781ba8e5e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 23 Jan 2024 14:06:30 -0800 Subject: [PATCH 8/8] adding readme --- nav2_smac_planner/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5eee580bc8e..71302532415 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -115,6 +115,8 @@ planner_server: angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius + analytic_expansion_max_cost: true # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) + analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0