From 0816f5faa7b317d5c68173ad9db99b3f55b08c17 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 13 Nov 2023 17:53:41 -0800 Subject: [PATCH] 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 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/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 1b95500549..f2a216f446 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 4a1cf963d8..5fded662e1 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 d34ffc8552..f630b51eca 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 d623ab6110..440dcb47cd 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 8b46fc3ae7..65847d4b3b 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") {