completing shortcutting fix

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};

nav2_smac_planner/src/analytic_expansion.cpp

@@ -111,28 +111,25 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<float>::max();
-          }
+            if (expansion.size() < 2) {
+              return std::numeric_limits<float>::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<float>::max();
@@ -208,6 +205,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   float angle = 0.0;
   Coordinates proposed_coordinates;
   bool failure = false;
+  std::vector<float> 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<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
       if (next->isNodeValid(_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<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
     }
   }
 
+  if (!failure) {
+    // We found 'a' valid expansion. Now to tell if its a quality option...
+    const float max_cost = _search_info.analytic_expansion_max_cost;
+    if (*std::max_element(node_costs.begin(), node_costs.end()) > 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;

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<float>(costmap_->getCost(
-      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
+        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(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<float>(costmap_->getCost(
-      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
+        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;

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<ompl::base::DubinsStateSpace>(
       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<ompl::base::ReedsSheppStateSpace>(
       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);

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<rclcpp::Parameter> para
         reinit_a_star = true;
         _search_info.analytic_expansion_max_length =
           static_cast<float>(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<float>(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == _name + ".downsample_costmap") {
@@ -552,6 +564,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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") {

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<rclcpp::Parameter> par
         reinit_a_star = true;
         _search_info.analytic_expansion_max_length =
           static_cast<float>(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<float>(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == _name + ".allow_unknown") {
@@ -405,6 +417,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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") {