Add exact path following in rpp for feasible plans

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -585,59 +585,6 @@ inline void savitskyGolayFilter(
     control_sequence.wz(offset)};
 }
 
-/**
- * @brief Find the iterator of the first pose at which there is an inversion on the path,
- * @param path to check for inversion
- * @return the first point after the inversion found in the path
- */
-inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
-{
-  // At least 3 poses for a possible inversion
-  if (path.poses.size() < 3) {
-    return path.poses.size();
-  }
-
-  // Iterating through the path to determine the position of the path inversion
-  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.
-    float oa_x = path.poses[idx].pose.position.x -
-      path.poses[idx - 1].pose.position.x;
-    float oa_y = path.poses[idx].pose.position.y -
-      path.poses[idx - 1].pose.position.y;
-    float ab_x = path.poses[idx + 1].pose.position.x -
-      path.poses[idx].pose.position.x;
-    float 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.
-    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0f) {
-      return idx + 1;
-    }
-  }
-
-  return path.poses.size();
-}
-
-/**
- * @brief Find and remove poses after the first inversion in the path
- * @param path to check for inversion
- * @return The location of the inversion, return 0 if none exist
- */
-inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
-{
-  nav_msgs::msg::Path cropped_path = path;
-  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
-  if (first_after_inversion == path.poses.size()) {
-    return 0u;
-  }
-
-  cropped_path.poses.erase(
-    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
-  path = cropped_path;
-  return first_after_inversion;
-}
-
 /**
  * @brief Compare to trajectory points to find closest path point along integrated distances
  * @param vec Vect to check

nav2_mppi_controller/src/path_handler.cpp

@@ -18,6 +18,7 @@
 #include "nav2_mppi_controller/tools/utils.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 
 namespace mppi
 {
@@ -135,7 +136,7 @@ nav_msgs::msg::Path PathHandler::transformPath(
     if (isWithinInversionTolerances(global_pose)) {
       prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
       global_plan_up_to_inversion_ = global_plan_;
-      inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+      inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
     }
   }
 
@@ -158,7 +159,7 @@ void PathHandler::setPath(const nav_msgs::msg::Path & plan)
   global_plan_ = plan;
   global_plan_up_to_inversion_ = global_plan_;
   if (enforce_path_inversion_) {
-    inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+    inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
   }
 }
 

nav2_mppi_controller/test/utils_test.cpp

@@ -390,69 +390,6 @@ TEST(UtilsTests, SmootherTest)
   EXPECT_LT(smoothed_val, original_val);
 }
 
-TEST(UtilsTests, FindPathInversionTest)
-{
-  // Straight path, no inversions to be found
-  nav_msgs::msg::Path path;
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(utils::findFirstPathInversion(path), 10u);
-
-  // To short to process
-  path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
-  EXPECT_EQ(utils::findFirstPathInversion(path), 3u);
-
-  // Has inversion at index 10, so should return 11 for the first point afterwards
-  // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
-  path.poses.clear();
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = 10 - i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(utils::findFirstPathInversion(path), 11u);
-}
-
-TEST(UtilsTests, RemovePosesAfterPathInversionTest)
-{
-  nav_msgs::msg::Path path;
-  // straight path
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
-
-  // try empty path
-  path.poses.clear();
-  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
-
-  // cusping path
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = 10 - i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u);
-  // Check to see if removed
-  EXPECT_EQ(path.poses.size(), 11u);
-  EXPECT_EQ(path.poses.back().pose.position.x, 10);
-}
-
 TEST(UtilsTests, ShiftColumnsByOnePlaceTest)
 {
   // Try with scalar value

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -63,6 +63,9 @@ struct Parameters
   bool use_collision_detection;
   double transform_tolerance;
   bool stateful;
+  bool enforce_path_inversion;
+  float inversion_xy_tolerance;
+  float inversion_yaw_tolerance;
 };
 
 /**

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp

@@ -27,10 +27,12 @@
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_core/controller_exceptions.hpp"
+#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
+using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
 
 /**
  * @class nav2_regulated_pure_pursuit_controller::PathHandler
@@ -43,7 +45,7 @@ class PathHandler
    * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler
    */
   PathHandler(
-    double transform_tolerance,
+    Parameters * params,
     std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
 
@@ -65,10 +67,24 @@ class PathHandler
     const geometry_msgs::msg::PoseStamped & pose,
     double max_robot_pose_search_dist, bool reject_unit_path = false);
 
-  void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;}
+  void setPlan(const nav_msgs::msg::Path & path);
 
   nav_msgs::msg::Path getPlan() {return global_plan_;}
 
+  /**
+    * @brief Check if the robot pose is within the set inversion tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the set inversion tolerances
+    */
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  /**
+    * @brief Prune a path to only interesting portions
+    * @param plan Plan to prune
+    * @param end Final path iterator
+    */
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
+
 protected:
   /**
    * Get the greatest extent of the costmap in meters from the center.
@@ -77,10 +93,12 @@ class PathHandler
   double getCostmapMaxExtent() const;
 
   rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")};
-  double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
+  unsigned int inversion_locale_{0u};
+  Parameters * params_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -174,13 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double & pose_cost, const nav_msgs::msg::Path & path,
     double & linear_vel, double & sign);
 
-  /**
-   * @brief checks for the cusp position
-   * @param pose Pose input to determine the cusp position
-   * @return robot distance from the cusp
-   */
-  double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan);
-
   nav2::LifecycleNode::WeakPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -105,7 +105,13 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".use_collision_detection",
     rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
-      node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
+    node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".enforce_path_inversion", rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".inversion_xy_tolerance", rclcpp::ParameterValue(0.2));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".inversion_yaw_tolerance", rclcpp::ParameterValue(0.4));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
   params_.base_desired_linear_vel = params_.desired_linear_vel;
@@ -190,6 +196,10 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
   node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
+  node->get_parameter(plugin_name_ + ".enforce_path_inversion", params_.enforce_path_inversion);
+  node->get_parameter(plugin_name_ + ".inversion_xy_tolerance", params_.inversion_xy_tolerance);
+  node->get_parameter(plugin_name_ + ".inversion_yaw_tolerance", params_.inversion_yaw_tolerance);
+
 
   if (params_.inflation_cost_scaling_factor <= 0.0) {
     RCLCPP_WARN(
@@ -311,6 +321,10 @@ ParameterHandler::updateParametersCallback(
         params_.transform_tolerance = parameter.as_double();
       } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") {
         params_.max_robot_pose_search_dist = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".inversion_xy_tolerance") {
+        params_.inversion_xy_tolerance = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".inversion_yaw_tolerance") {
+        params_.inversion_yaw_tolerance = parameter.as_double();
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
@@ -333,6 +347,8 @@ ParameterHandler::updateParametersCallback(
         params_.allow_reversing = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
         params_.interpolate_curvature_after_goal = parameter.as_bool();
+      } else if (param_name == plugin_name_ + ".enforce_path_inversion") {
+        params_.enforce_path_inversion = parameter.as_bool();
       }
     }
   }