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 <[email protected]>

Author: SteveMacenski

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

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

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -116,10 +116,10 @@ class GridCollisionChecker
 protected:
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
-  double footprint_cost_;
+  float footprint_cost_;
   bool footprint_is_radius_;
   std::vector<float> angles_;
-  double possible_inscribed_cost_{-1};
+  float possible_inscribed_cost_{-1};
   rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
   rclcpp::Clock::SharedPtr clock_;
 };

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

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

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<std::vector<MotionPrimitive>> motion_primitives;
   ompl::base::StateSpacePtr state_space;
   std::vector<TrigValues> 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;
   }

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/a_star.cpp

@@ -110,7 +110,7 @@ void AStarAlgorithm<NodeT>::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<typename NodeT>

nav2_smac_planner/src/analytic_expansion.cpp

@@ -41,7 +41,7 @@ AnalyticExpansion<NodeT>::AnalyticExpansion(
 
 template<typename NodeT>
 void AnalyticExpansion<NodeT>::setCollisionChecker(
-  GridCollisionChecker * collision_checker)
+  GridCollisionChecker * & collision_checker)
 {
   _collision_checker = collision_checker;
 }
@@ -80,7 +80,8 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<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 best_score = scoringFn(analytic_nodes);
+        float score = std::numeric_limits<float>::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<ompl::base::DubinsStateSpace>(min_turn_rad);
+          } else {
+            state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(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,18 +166,18 @@ template<typename NodeT>
 typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::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);
   to[0] = goal->pose.x;
   to[1] = goal->pose.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<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   }
 
   // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.);
-  unsigned int num_intervals = std::floor(d / sqrt_2);
+  static const float sqrt_2 = std::sqrt(2.0f);
+  unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));
 
   AnalyticExpansionNodes possible_nodes;
   // When "from" and "to" are zero or one cell away,
@@ -159,10 +205,12 @@ 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++) {
-    node->motion_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];
@@ -182,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
@@ -196,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;
@@ -256,7 +337,8 @@ typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Nod
 getAnalyticPath(
   const NodePtr & node,
   const NodePtr & goal,
-  const NodeGetter & node_getter)
+  const NodeGetter & node_getter,
+  const ompl::base::StateSpacePtr & state_space)
 {
   return AnalyticExpansionNodes();
 }