From 847321124cd51150397e60c9a425382b0dd2e712 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 23 Jan 2024 22:07:31 +0000 Subject: [PATCH] Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski --- nav2_smac_planner/README.md | 2 + .../nav2_smac_planner/analytic_expansion.hpp | 5 +- .../nav2_smac_planner/collision_checker.hpp | 4 +- .../include/nav2_smac_planner/node_2d.hpp | 8 +- .../include/nav2_smac_planner/node_hybrid.hpp | 8 +- .../nav2_smac_planner/node_lattice.hpp | 10 +- .../include/nav2_smac_planner/types.hpp | 2 + nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 102 ++++++++++++++++-- nav2_smac_planner/src/collision_checker.cpp | 14 +-- nav2_smac_planner/src/node_hybrid.cpp | 8 +- nav2_smac_planner/src/node_lattice.cpp | 13 ++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 15 +++ .../src/smac_planner_lattice.cpp | 15 +++ 14 files changed, 166 insertions(+), 42 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index b769f1e4e5..cd7a20b1e2 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 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 fa1b2f1fdd..10c4224a07 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 @@ -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/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 5933a6dc92..c1878a8863 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 4150deaee7..392e325fac 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 e822663cf9..8ba117e80e 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 577cc28e19..807e068c88 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -105,12 +105,14 @@ 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; std::vector trig_values; std::string current_lattice_filepath; LatticeMetadata lattice_metadata; + MotionModel motion_model = MotionModel::UNKNOWN; }; /** @@ -183,7 +185,7 @@ class NodeLattice * @brief Gets the accumulated cost at this node * @return accumulated cost */ - inline float & getAccumulatedCost() + inline float getAccumulatedCost() { return _accumulated_cost; } @@ -201,7 +203,7 @@ class NodeLattice * @brief Gets the costmap cost at this node * @return costmap cost */ - inline float & getCost() + inline float getCost() { return _cell_cost; } @@ -210,7 +212,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; } @@ -227,7 +229,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/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index d199c67775..6baf51f4a4 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/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 55fe35e531..e57db9952a 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 d0e068c634..10cb3bdd5a 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; } @@ -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,50 @@ 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 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. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + 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 best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + 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; + } + } + return setAnalyticPath(node, goal_node, analytic_nodes); } } @@ -120,10 +166,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 +177,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 @@ -142,8 +188,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion(std::floor(d / sqrt_2)); AnalyticExpansionNodes possible_nodes; // When "from" and "to" are zero or one cell away, @@ -159,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++) { @@ -182,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 @@ -196,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; @@ -256,7 +337,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion(possible_inscribed_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -107,8 +107,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 + 0.5), static_cast(y + 0.5)); + 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) { @@ -147,7 +147,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; @@ -157,15 +157,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 + 0.5), static_cast(y + 0.5)); + 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; } } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 02b7bd2d13..24db0e5fc7 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -139,7 +139,7 @@ void HybridMotionTable::initDubin( } // 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()); @@ -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()); @@ -748,10 +748,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 dfdf47ff4b..0076979c44 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,11 +78,13 @@ 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); + motion_model = MotionModel::DUBIN; } else { - state_space = std::make_unique( + state_space = std::make_shared( lattice_metadata.min_turning_radius); + motion_model = MotionModel::REEDS_SHEPP; } } @@ -426,11 +429,13 @@ 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); + motion_table.motion_model = MotionModel::DUBIN; } else { - motion_table.state_space = std::make_unique( + 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); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index a56c3af3f8..12d4c2976f 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( @@ -548,6 +557,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") { @@ -568,6 +580,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 083ad1f952..a5f16bcdda 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); @@ -465,6 +474,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") { @@ -482,6 +494,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") {