Smoother feasbility

nav2_smac_planner/README.md

@@ -130,6 +130,7 @@ planner_server:
         w_smooth: 0.3
         w_data: 0.2
         tolerance: 1e-10
+        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
 ```
 
 ## Topics

nav2_smac_planner/include/nav2_smac_planner/smoother.hpp

@@ -29,6 +29,8 @@
 #include "nav_msgs/msg/path.hpp"
 #include "angles/angles.h"
 #include "tf2/utils.h"
+#include "ompl/base/StateSpace.h"
+#include "ompl/base/spaces/DubinsStateSpace.h"
 
 namespace nav2_smac_planner
 {
@@ -43,6 +45,41 @@ struct PathSegment
   unsigned int end;
 };
 
+/**
+ * @struct nav2_smac_planner::BoundaryPoints
+ * @brief Set of boundary condition points from expansion
+ */
+struct BoundaryPoints
+{
+  /**
+   * @brief A constructor for BoundaryPoints
+   */
+  BoundaryPoints(double & x_in, double & y_in, double & theta_in)
+  : x(x_in), y(y_in), theta(theta_in)
+  {}
+
+  double x;
+  double y;
+  double theta;
+};
+
+/**
+ * @struct nav2_smac_planner::BoundaryExpansion
+ * @brief Boundary expansion state
+ */
+struct BoundaryExpansion
+{
+  double path_end_idx{0.0};
+  double expansion_path_length{0.0};
+  double original_path_length{0.0};
+  std::vector<BoundaryPoints> pts;
+  bool in_collision{false};
+};
+
+typedef std::vector<BoundaryExpansion> BoundaryExpansions;
+typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
+typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
+
 /**
  * @class nav2_smac_planner::Smoother
  * @brief A path smoother implementation
@@ -63,8 +100,10 @@ class Smoother
   /**
    * @brief Initialization of the smoother
    * @param min_turning_radius Minimum turning radius (m)
+   * @param motion_model Motion model type
    */
-  void initialize(const double & min_turning_radius);
+  void initialize(
+    const double & min_turning_radius);
 
   /**
    * @brief Smoother API method
@@ -76,22 +115,22 @@ class Smoother
   bool smooth(
     nav_msgs::msg::Path & path,
     const nav2_costmap_2d::Costmap2D * costmap,
-    const double & max_time,
-    const bool do_refinement = true);
+    const double & max_time);
 
 protected:
   /**
    * @brief Smoother method - does the smoothing on a segment
    * @param path Reference to path
+   * @param reversing_segment Return if this is a reversing segment
    * @param costmap Pointer to minimal costmap
    * @param max_time Maximum time to compute, stop early if over limit
    * @return If smoothing was successful
    */
   bool smoothImpl(
     nav_msgs::msg::Path & path,
+    bool & reversing_segment,
     const nav2_costmap_2d::Costmap2D * costmap,
-    const double & max_time,
-    const bool do_refinement = true);
+    const double & max_time);
 
   /**
    * @brief Get the field value for a given dimension
@@ -122,22 +161,81 @@ class Smoother
   std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
 
   /**
-   * @brief Get the instantaneous curvature valud
-   * @param path Path to find curvature in
-   * @param i idx in path to find it for
-   * @return curvature
+   * @brief Enforced minimum curvature boundary conditions on plan output
+   * the robot is traveling in the same direction (e.g. forward vs reverse)
+   * @param start_pose Start pose of the feasible path to maintain
+   * @param path Path to modify for curvature constraints on start / end of path
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
    */
-  inline double getCurvature(const nav_msgs::msg::Path & path, const unsigned int i);
+  void enforceStartBoundaryConditions(
+    const geometry_msgs::msg::Pose & start_pose,
+    nav_msgs::msg::Path & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const bool & reversing_segment);
+
+  /**
+   * @brief Enforced minimum curvature boundary conditions on plan output
+   * the robot is traveling in the same direction (e.g. forward vs reverse)
+   * @param end_pose End pose of the feasible path to maintain
+   * @param path Path to modify for curvature constraints on start / end of path
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
+   */
+  void enforceEndBoundaryConditions(
+    const geometry_msgs::msg::Pose & end_pose,
+    nav_msgs::msg::Path & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const bool & reversing_segment);
+
+  /**
+   * @brief Given a set of boundary expansion, find the one which is shortest
+   * such that it is least likely to contain a loop-de-loop when working with
+   * close-by primitive markers. Instead, select a further away marker which
+   * generates a shorter `
+   * @param boundary_expansions Set of boundary expansions
+   * @return Idx of the shorest boundary expansion option
+   */
+  unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions);
+
+  /**
+   * @brief Populate a motion model expansion from start->end into expansion
+   * @param start Start pose of the feasible path to maintain
+   * @param end End pose of the feasible path to maintain
+   * @param expansion Expansion object to populate
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
+   */
+  void findBoundaryExpansion(
+    const geometry_msgs::msg::Pose & start,
+    const geometry_msgs::msg::Pose & end,
+    BoundaryExpansion & expansion,
+    const nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @brief Generates boundary expansions with end idx at least strategic
+   * distances away, using either Reverse or (Forward) Path Iterators.
+   * @param start iterator to start search in path for
+   * @param end iterator to end search for
+   * @return Boundary expansions with end idxs populated
+   */
+  template<typename IteratorT>
+  BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end);
 
   /**
    * @brief For a given path, update the path point orientations based on smoothing
    * @param path Path to approximate the path orientation in
+   * @param reversing_segment Return if this is a reversing segment
    */
-  inline void updateApproximatePathOrientations(nav_msgs::msg::Path & path);
+  inline void updateApproximatePathOrientations(
+    nav_msgs::msg::Path & path,
+    bool & reversing_segment);
 
   double min_turning_rad_, tolerance_, data_w_, smooth_w_;
-  int max_its_;
-  bool is_holonomic_;
+  int max_its_, refinement_ctr_;
+  bool is_holonomic_, do_refinement_;
+  MotionModel motion_model_;
+  ompl::base::StateSpacePtr state_space_;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -82,13 +82,17 @@ struct SmootherParams
     nav2_util::declare_parameter_if_not_declared(
       node, local_name + "w_smooth", rclcpp::ParameterValue(0.3));
     node->get_parameter(local_name + "w_smooth", w_smooth_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "do_refinement", rclcpp::ParameterValue(true));
+    node->get_parameter(local_name + "do_refinement", do_refinement_);
   }
 
   double tolerance_;
   int max_its_;
   double w_data_;
   double w_smooth_;
   bool holonomic_;
+  bool do_refinement_;
 };
 
 /**

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -308,9 +308,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
 #endif
 
   // Smooth plan
-  if (plan.poses.size() > 6) {
-    _smoother->smooth(plan, costmap, time_remaining);
-  }
+  _smoother->smooth(plan, costmap, time_remaining);
 
   // If use_final_approach_orientation=true, interpolate the last pose orientation from the
   // previous pose to set the orientation to the 'final approach' orientation of the robot so

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -357,7 +357,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 #endif
 
   // Smooth plan
-  if (_smoother && num_iterations > 1 && plan.poses.size() > 6) {
+  if (_smoother && num_iterations > 1) {
     _smoother->smooth(plan, costmap, time_remaining);
   }
 

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -311,7 +311,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
 #endif
 
   // Smooth plan
-  if (_smoother && num_iterations > 1 && plan.poses.size() > 6) {
+  if (_smoother && num_iterations > 1) {
     _smoother->smooth(plan, _costmap, time_remaining);
   }