Refactor common functions in smoother and planner (#5537)

* Refactor common functions in planner and smoother

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Add unit tests for smoother utils

Signed-off-by: Maurice <mauricepurnawan@gmail.com>

* Update cmakelist and linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_util/include/nav2_util/smoother_utils.hpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_util/include/nav2_util/smoother_utils.hpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update unit tests

Signed-off-by: Maurice <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Signed-off-by: Maurice <mauricepurnawan@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: mini-1235

nav2_smac_planner/include/nav2_smac_planner/smoother.hpp

@@ -27,16 +27,6 @@
 namespace nav2_smac_planner
 {
 
-/**
- * @class nav2_smac_planner::PathSegment
- * @brief A segment of a path in start/end indices
- */
-struct PathSegment
-{
-  unsigned int start;
-  unsigned int end;
-};
-
 /**
  * @struct nav2_smac_planner::BoundaryPoints
  * @brief Set of boundary condition points from expansion
@@ -144,14 +134,6 @@ class Smoother
     geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
     const double & value);
 
-  /**
-   * @brief Finds the starting and end indices of path segments where
-   * the robot is traveling in the same direction (e.g. forward vs reverse)
-   * @param path Path in which to look for cusps
-   * @return Set of index pairs for each segment of the path in a given direction
-   */
-  std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
-
   /**
    * @brief Enforced minimum curvature boundary conditions on plan output
    * the robot is traveling in the same direction (e.g. forward vs reverse)
@@ -214,15 +196,6 @@ class Smoother
   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,
-    bool & reversing_segment);
-
   double min_turning_rad_, tolerance_, data_w_, smooth_w_;
   int max_its_, refinement_ctr_, refinement_num_;
   bool is_holonomic_, do_refinement_;

nav2_smac_planner/src/smoother.cpp

@@ -24,11 +24,13 @@
 #include "tf2/utils.hpp"
 
 #include "nav2_smac_planner/smoother.hpp"
+#include "nav2_util/smoother_utils.hpp"
 
 namespace nav2_smac_planner
 {
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
+using nav2_util::PathSegment;
 
 Smoother::Smoother(const SmootherParams & params)
 {
@@ -62,7 +64,8 @@ bool Smoother::smooth(
   bool success = true, reversing_segment;
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
-  std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
+  std::vector<PathSegment> path_segments = nav2_util::findDirectionalPathSegments(path,
+      is_holonomic_);
 
   for (unsigned int i = 0; i != path_segments.size(); i++) {
     if (path_segments[i].end - path_segments[i].start > 10) {
@@ -130,7 +133,7 @@ bool Smoother::smoothImpl(
         rclcpp::get_logger("SmacPlannerSmoother"),
         "Number of iterations has exceeded limit of %i.", max_its_);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
       return false;
     }
 
@@ -142,7 +145,7 @@ bool Smoother::smoothImpl(
         rclcpp::get_logger("SmacPlannerSmoother"),
         "Smoothing time exceeded allowed duration of %0.2f.", max_time);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
       return false;
     }
 
@@ -176,7 +179,7 @@ bool Smoother::smoothImpl(
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced.");
         path = last_path;
-        updateApproximatePathOrientations(path, reversing_segment);
+        nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
         return false;
       }
     }
@@ -191,7 +194,7 @@ bool Smoother::smoothImpl(
     smoothImpl(new_path, reversing_segment, costmap, max_time);
   }
 
-  updateApproximatePathOrientations(new_path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_);
   path = new_path;
   return true;
 }
@@ -221,92 +224,6 @@ void Smoother::setFieldByDim(
   }
 }
 
-std::vector<PathSegment> Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path)
-{
-  std::vector<PathSegment> segments;
-  PathSegment curr_segment;
-  curr_segment.start = 0;
-
-  // If holonomic, no directional changes and
-  // may have abrupt angular changes from naive grid search
-  if (is_holonomic_) {
-    curr_segment.end = path.poses.size() - 1;
-    segments.push_back(curr_segment);
-    return segments;
-  }
-
-  // Iterating through the path to determine the position of the cusp
-  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
-    // We have two vectors for the dot product OA and AB. Determining the vectors.
-    double oa_x = path.poses[idx].pose.position.x -
-      path.poses[idx - 1].pose.position.x;
-    double oa_y = path.poses[idx].pose.position.y -
-      path.poses[idx - 1].pose.position.y;
-    double ab_x = path.poses[idx + 1].pose.position.x -
-      path.poses[idx].pose.position.x;
-    double ab_y = path.poses[idx + 1].pose.position.y -
-      path.poses[idx].pose.position.y;
-
-    // Checking for the existence of cusp, in the path, using the dot product.
-    double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0) {
-      curr_segment.end = idx;
-      segments.push_back(curr_segment);
-      curr_segment.start = idx;
-    }
-
-    // Checking for the existence of a differential rotation in place.
-    double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
-    double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
-    double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
-    if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
-      curr_segment.end = idx;
-      segments.push_back(curr_segment);
-      curr_segment.start = idx;
-    }
-  }
-
-  curr_segment.end = path.poses.size() - 1;
-  segments.push_back(curr_segment);
-  return segments;
-}
-
-void Smoother::updateApproximatePathOrientations(
-  nav_msgs::msg::Path & path,
-  bool & reversing_segment)
-{
-  double dx, dy, theta, pt_yaw;
-  reversing_segment = false;
-
-  // Find if this path segment is in reverse
-  dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
-  dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
-  theta = atan2(dy, dx);
-  pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
-  if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
-    reversing_segment = true;
-  }
-
-  // Find the angle relative the path position vectors
-  for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
-    dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
-    dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
-    theta = atan2(dy, dx);
-
-    // If points are overlapping, pass
-    if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
-      continue;
-    }
-
-    // Flip the angle if this path segment is in reverse
-    if (reversing_segment) {
-      theta += M_PI;  // orientationAroundZAxis will normalize
-    }
-
-    path.poses[i].pose.orientation = orientationAroundZAxis(theta);
-  }
-}
-
 unsigned int Smoother::findShortestBoundaryExpansionIdx(
   const BoundaryExpansions & boundary_expansions)
 {

nav2_smac_planner/test/test_smoother.cpp

@@ -37,11 +37,6 @@ class SmootherWrapper : public nav2_smac_planner::Smoother
   explicit SmootherWrapper(const SmootherParams & params)
   : nav2_smac_planner::Smoother(params)
   {}
-
-  std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
-  {
-    return findDirectionalPathSegments(path);
-  }
 };
 
 TEST(SmootherTest, test_full_smoother)
@@ -133,10 +128,6 @@ TEST(SmootherTest, test_full_smoother)
     y_m = path[i].y;
   }
 
-  // Check that we accurately detect that this path has a reversing segment
-  auto path_segs = smoother->findDirectionalPathSegmentsWrapper(plan);
-  EXPECT_TRUE(path_segs.size() == 2u || path_segs.size() == 3u);
-
   // Test smoother, should succeed with same number of points
   // and shorter overall length, while still being collision free.
   auto path_size_in = plan.poses.size();

nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp

@@ -25,7 +25,7 @@
 #include <utility>
 
 #include "nav2_core/smoother.hpp"
-#include "nav2_smoother/smoother_utils.hpp"
+#include "nav2_util/smoother_utils.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"

nav2_smoother/include/nav2_smoother/simple_smoother.hpp

@@ -24,7 +24,7 @@
 #include <utility>
 
 #include "nav2_core/smoother.hpp"
-#include "nav2_smoother/smoother_utils.hpp"
+#include "nav2_util/smoother_utils.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"

nav2_smoother/src/savitzky_golay_smoother.cpp

@@ -19,11 +19,10 @@
 
 namespace nav2_smoother
 {
-
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
 using nav2::declare_parameter_if_not_declared;
+using nav2_util::PathSegment;
 
 void SavitzkyGolaySmoother::configure(
   const nav2::LifecycleNode::WeakPtr & parent,
@@ -89,7 +88,7 @@ bool SavitzkyGolaySmoother::smooth(
   std::vector<PathSegment> path_segments{
     PathSegment{0u, static_cast<unsigned int>(path.poses.size() - 1)}};
   if (enforce_path_inversion_) {
-    path_segments = findDirectionalPathSegments(path);
+    path_segments = nav2_util::findDirectionalPathSegments(path);
   }
 
   // Minimum point size to smooth is SG filter size + start + end
@@ -171,7 +170,7 @@ bool SavitzkyGolaySmoother::smoothImpl(
     }
   }
 
-  updateApproximatePathOrientations(path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(path, reversing_segment);
   return true;
 }
 

nav2_smoother/src/simple_smoother.cpp

@@ -19,10 +19,10 @@
 
 namespace nav2_smoother
 {
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
 using nav2::declare_parameter_if_not_declared;
+using nav2_util::PathSegment;
 
 void SimpleSmoother::configure(
   const nav2::LifecycleNode::WeakPtr & parent,
@@ -72,10 +72,10 @@ bool SimpleSmoother::smooth(
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
 
-  std::vector<PathSegment> path_segments{PathSegment{
+  std::vector<nav2_util::PathSegment> path_segments{PathSegment{
       0u, static_cast<unsigned int>(path.poses.size() - 1)}};
   if (enforce_path_inversion_) {
-    path_segments = findDirectionalPathSegments(path);
+    path_segments = nav2_util::findDirectionalPathSegments(path);
   }
 
   std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
@@ -137,7 +137,7 @@ void SimpleSmoother::smoothImpl(
         logger_,
         "Number of iterations has exceeded limit of %i.", max_its_);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment);
       return;
     }
 
@@ -149,7 +149,7 @@ void SimpleSmoother::smoothImpl(
         logger_,
         "Smoothing time exceeded allowed duration of %0.2f.", max_time);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment);
       throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration");
     }
 
@@ -183,7 +183,7 @@ void SimpleSmoother::smoothImpl(
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced.");
         path = last_path;
-        updateApproximatePathOrientations(path, reversing_segment);
+        nav2_util::updateApproximatePathOrientations(path, reversing_segment);
         return;
       }
     }
@@ -198,7 +198,7 @@ void SimpleSmoother::smoothImpl(
     smoothImpl(new_path, reversing_segment, costmap, max_time);
   }
 
-  updateApproximatePathOrientations(new_path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(new_path, reversing_segment);
   path = new_path;
 }
 

nav2_smoother/test/test_savitzky_golay_smoother.cpp

@@ -30,7 +30,6 @@
 #include "nav2_smoother/savitzky_golay_smoother.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_smoother;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT
 

nav2_smoother/test/test_simple_smoother.cpp

@@ -24,7 +24,6 @@
 #include "nav2_smoother/simple_smoother.hpp"
 #include "nav2_core/smoother_exceptions.hpp"
 
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_smoother;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT
 
@@ -36,11 +35,6 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother
   {
   }
 
-  std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
-  {
-    return findDirectionalPathSegments(path);
-  }
-
   void setMaxItsToInvalid()
   {
     max_its_ = 0;

nav2_util/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -61,6 +62,7 @@ ament_export_dependencies(
   tf2_ros
   pluginlib
   nav2_ros_common
+  angles
 )
 ament_export_targets(${library_name})