Prototype solving ros-navigation#5192 Issue 2: Reeds-Shepp reduce small reverse expansions (ros-navigation#5207)

* prototype solving 5192 issue 2

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Removing unnecessary variable

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

Author: SteveMacenski

nav2_bringup/launch/tb3_loopback_simulation_launch.py

@@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription:
             'use_composition': use_composition,
             'use_respawn': use_respawn,
             'use_localization': 'False',  # Don't use SLAM, AMCL
+            'use_keepout_zones': 'False',
+            'use_speed_zones': 'False',
         }.items(),
     )
 

nav2_bringup/launch/tb3_simulation_launch.py

@@ -195,6 +195,8 @@ def generate_launch_description() -> LaunchDescription:
             'autostart': autostart,
             'use_composition': use_composition,
             'use_respawn': use_respawn,
+            'use_keepout_zones': 'False',
+            'use_speed_zones': 'False',
         }.items(),
     )
     # The SDF file for the world is a xacro file because we wanted to

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -15,6 +15,10 @@
 #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/base/spaces/ReedsSheppStateSpace.h>
+
 #include <functional>
 #include <list>
 #include <memory>
@@ -61,7 +65,33 @@ class AnalyticExpansion
     Coordinates proposed_coords;
   };
 
-  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
+  /**
+   * @struct AnalyticExpansionNodes
+   * @brief Analytic expansion nodes and associated metadata
+   *
+   * This structure holds a collection of analytic expansion nodes and the number of direction
+   * changes encountered during the expansion.
+   */
+  struct AnalyticExpansionNodes
+  {
+    AnalyticExpansionNodes() = default;
+
+    void add(
+      NodePtr & node,
+      Coordinates & initial_coords,
+      Coordinates & proposed_coords)
+    {
+      nodes.emplace_back(node, initial_coords, proposed_coords);
+    }
+
+    void setDirectionChanges(int changes)
+    {
+      direction_changes = changes;
+    }
+
+    std::vector<AnalyticExpansionNode> nodes;
+    int direction_changes{0};
+  };
 
   /**
    * @brief Constructor for analytic expansion object
@@ -136,6 +166,13 @@ class AnalyticExpansion
     const NodePtr & node, const NodePtr & goal,
     const AnalyticExpansionNodes & expanded_nodes);
 
+  /**
+    * @brief Counts the number of direction changes in a Reeds-Shepp path
+    * @param path The Reeds-Shepp path to count direction changes in
+    * @return The number of direction changes in the path
+    */
+  int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path);
+
   /**
    * @brief Takes an expanded nodes to clean up, if necessary, of any state
    * information that may be polluting it from a prior search iteration

nav2_smac_planner/src/analytic_expansion.cpp

@@ -12,10 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#include <ompl/base/ScopedState.h>
-#include <ompl/base/spaces/DubinsStateSpace.h>
-#include <ompl/base/spaces/ReedsSheppStateSpace.h>
-
 #include <algorithm>
 #include <vector>
 #include <memory>
@@ -64,7 +60,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
       NodeT::getCoords(
       current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
 
-    AnalyticExpansionNodes current_best_analytic_nodes = {};
+    AnalyticExpansionNodes current_best_analytic_nodes;
     NodePtr current_best_goal = nullptr;
     NodePtr current_best_node = nullptr;
     float current_best_score = std::numeric_limits<float>::max();
@@ -96,7 +92,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           getAnalyticPath(
           current_node, current_goal_node, getter,
           current_node->motion_table.state_space);
-        if (!analytic_nodes.empty()) {
+        if (!analytic_nodes.nodes.empty()) {
           found_valid_expansion = true;
           NodePtr node = current_node;
           float score = refineAnalyticPath(
@@ -118,7 +114,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             getAnalyticPath(
             current_node, current_goal_node, getter,
             current_node->motion_table.state_space);
-          if (!analytic_nodes.empty()) {
+          if (!analytic_nodes.nodes.empty()) {
             NodePtr node = current_node;
             float score = refineAnalyticPath(
               node, current_goal_node, getter, analytic_nodes);
@@ -134,7 +130,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
       }
     }
 
-    if (!current_best_analytic_nodes.empty()) {
+    if (!current_best_analytic_nodes.nodes.empty()) {
       return setAnalyticPath(
         current_best_node, current_best_goal,
         current_best_analytic_nodes);
@@ -146,6 +142,28 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
   return NodePtr(nullptr);
 }
 
+template<typename NodeT>
+int AnalyticExpansion<NodeT>::countDirectionChanges(
+  const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path)
+{
+  const double * lengths = path.length_;
+  int changes = 0;
+  int last_dir = 0;
+  for (int i = 0; i < 5; ++i) {
+    if (lengths[i] == 0.0) {
+      continue;
+    }
+
+    int currentDirection = (lengths[i] > 0.0) ? 1 : -1;
+    if (last_dir != 0 && currentDirection != last_dir) {
+      ++changes;
+    }
+    last_dir = currentDirection;
+  }
+
+  return changes;
+}
+
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
   const NodePtr & node,
@@ -163,6 +181,12 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
   float d = state_space->distance(from(), to());
 
+  auto rs_state_space = dynamic_cast<ompl::base::ReedsSheppStateSpace *>(state_space.get());
+  int direction_changes = 0;
+  if (rs_state_space) {
+    direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get()));
+  }
+
   // A move of sqrt(2) is guaranteed to be in a new cell
   static const float sqrt_2 = sqrtf(2.0f);
 
@@ -179,7 +203,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   AnalyticExpansionNodes possible_nodes;
   // When "from" and "to" are zero or one cell away,
   // num_intervals == 0
-  possible_nodes.reserve(num_intervals);  // We won't store this node or the goal
+  possible_nodes.nodes.reserve(num_intervals);  // We won't store this node or the goal
   std::vector<double> reals;
   double theta;
 
@@ -214,7 +238,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
       next->setPose(proposed_coordinates);
       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);
+        possible_nodes.add(next, initial_node_coords, proposed_coordinates);
         node_costs.emplace_back(next->getCost());
         prev = next;
       } else {
@@ -264,7 +288,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   }
 
   // Reset to initial poses to not impact future searches
-  for (const auto & node_pose : possible_nodes) {
+  for (const auto & node_pose : possible_nodes.nodes) {
     const auto & n = node_pose.node;
     n->setPose(node_pose.initial_coords);
   }
@@ -273,6 +297,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
     return AnalyticExpansionNodes();
   }
 
+  possible_nodes.setDirectionChanges(direction_changes);
   return possible_nodes;
 }
 
@@ -299,9 +324,13 @@ float AnalyticExpansion<NodeT>::refineAnalyticPath(
         getAnalyticPath(
         test_node, goal_node, getter,
         test_node->motion_table.state_space);
-      if (refined_analytic_nodes.empty()) {
+      if (refined_analytic_nodes.nodes.empty()) {
         break;
       }
+      if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) {
+        // If the direction changes are worse, we don't want to use this path
+        continue;
+      }
       analytic_nodes = refined_analytic_nodes;
       node = test_node;
     } else {
@@ -314,26 +343,27 @@ float AnalyticExpansion<NodeT>::refineAnalyticPath(
   // higher than the minimum turning radius and use the best solution based on
   // a scoring function similar to that used in traversal cost estimation.
   auto scoringFn = [&](const AnalyticExpansionNodes & expansion) {
-      if (expansion.size() < 2) {
+      if (expansion.nodes.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) {
+      expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x,
+      expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y);
+      const float & weight = expansion.nodes[0].node->motion_table.cost_penalty;
+      for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) {
         normalized_cost = iter->node->getCost() / 252.0f;
-      // Search's Traversal Cost Function
+        // Search's Traversal Cost Function
         score += distance * (1.0 + weight * normalized_cost);
       }
       return score;
     };
 
-  float best_score = scoringFn(analytic_nodes);
+  float original_score = scoringFn(analytic_nodes);
+  float best_score = original_score;
   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
@@ -347,7 +377,21 @@ float AnalyticExpansion<NodeT>::refineAnalyticPath(
     }
     refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space);
     score = scoringFn(refined_analytic_nodes);
-    if (score <= best_score) {
+
+    // Normal scoring: prioritize lower cost as long as not more directional changes
+    if (score <= best_score &&
+      refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes)
+    {
+      analytic_nodes = refined_analytic_nodes;
+      best_score = score;
+      continue;
+    }
+
+    // Special case: If we have a better score than original (only) and less directional changes
+    // the path quality is still better than the original and is less operationally complex
+    if (score <= original_score &&
+      refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes)
+    {
       analytic_nodes = refined_analytic_nodes;
       best_score = score;
     }
@@ -365,7 +409,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalytic
   _detached_nodes.clear();
   // Legitimate final path - set the parent relationships, states, and poses
   NodePtr prev = node;
-  for (const auto & node_pose : expanded_nodes) {
+  for (const auto & node_pose : expanded_nodes.nodes) {
     auto n = node_pose.node;
     cleanNode(n);
     if (n->getIndex() != goal_node->getIndex()) {

nav2_smac_planner/test/test_a_star.cpp

@@ -147,7 +147,7 @@ TEST(AStarTest, test_a_star_2d)
     analytic_expansion_nodes), std::numeric_limits<float>::max());
   nav2_smac_planner::AnalyticExpansion<nav2_smac_planner::Node2D>::AnalyticExpansionNodes
     expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr);
-  EXPECT_EQ(expected_nodes.size(), 0);
+  EXPECT_EQ(expected_nodes.nodes.size(), 0);
 
   delete costmapA;
 }