MPPI: Clip path up to in-place rotation

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -129,9 +129,9 @@ class PathHandler
   void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
 
   /**
-    * @brief Check if the robot pose is within the set inversion tolerances
+    * @brief Check if the robot pose is within the set inversion or rotation tolerances
     * @param robot_pose Robot's current pose to check
-    * @return bool If the robot pose is within the set inversion tolerances
+    * @return bool If the robot pose is within the tolerances for the next path constraint
     */
   bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
 
@@ -148,8 +148,10 @@ class PathHandler
   double prune_distance_{0};
   double transform_tolerance_{0};
   float inversion_xy_tolerance_{0.2};
-  float inversion_yaw_tolerance{0.4};
+  float inversion_yaw_tolerance_{0.4};
+  float minimum_rotation_angle_{0.785};
   bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
   unsigned int inversion_locale_{0u};
 };
 }  // namespace mppi

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -511,56 +511,107 @@ inline void savitskyGolayFilter(
 }
 
 /**
- * @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
+ * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path
+ * @param path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return the first point after the inversion or rotation found in the path
  */
-inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+inline unsigned int findFirstPathInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
 {
-  // At least 3 poses for a possible inversion
-  if (path.poses.size() < 3) {
+  if (path.poses.size() < 2) {
     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;
+  unsigned int first_constraint = path.poses.size();
+
+  // Check for in-place rotation first (if enabled)
+  if (rotation_threshold > 0.0f) {
+    for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) {
+      float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x;
+      float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y;
+      float translation = sqrtf(dx * dx + dy * dy);
+
+      // Check if poses are at roughly the same location
+      if (translation < 1e-3) {
+        float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation);
+        float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+        float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2));
+
+        // Check if this meets the minimum rotation threshold
+        if (rotation > rotation_threshold) {
+          // Found start of in-place rotation, now find where it ends
+          unsigned int end_idx = idx + 1;
+
+          // Continue while we have minimal translation (still rotating in place)
+          while (end_idx < path.poses.size() - 1) {
+            float next_dx = path.poses[end_idx + 1].pose.position.x -
+              path.poses[end_idx].pose.position.x;
+            float next_dy = path.poses[end_idx + 1].pose.position.y -
+              path.poses[end_idx].pose.position.y;
+            float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy);
+
+            if (next_translation >= 1e-3) {
+              break;  // End of in-place rotation sequence
+            }
+            end_idx++;
+          }
+
+          first_constraint = end_idx;
+          break;
+        }
+      }
+    }
+  }
+
+  // Check for inversion (at least 3 poses needed)
+  if (path.poses.size() >= 3) {
+    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) {
+        first_constraint = std::min(first_constraint, idx + 1);
+        break;
+      }
     }
   }
 
-  return path.poses.size();
+  return first_constraint;
 }
 
 /**
- * @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
+ * @brief Find and remove poses after the first inversion or in-place rotation in the path
+ * @param path Path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return The location of the inversion/rotation, return 0 if none exist
  */
-inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+inline unsigned int removePosesAfterFirstInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
 {
   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;
+
+  const unsigned int first_constraint = findFirstPathInversion(cropped_path, rotation_threshold);
+
+  if (first_constraint == path.poses.size()) {
+    return 0u;  // No constraint found
   }
 
   cropped_path.poses.erase(
-    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
+    cropped_path.poses.begin() + first_constraint, cropped_path.poses.end());
   path = cropped_path;
-  return first_after_inversion;
+  return first_constraint;
 }
 
 /**

nav2_mppi_controller/src/path_handler.cpp

@@ -39,11 +39,17 @@ void PathHandler::initialize(
   getParam(prune_distance_, "prune_distance", 1.5);
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
   getParam(enforce_path_inversion_, "enforce_path_inversion", false);
-  if (enforce_path_inversion_) {
+  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
+  if (enforce_path_inversion_ || enforce_path_rotation_) {
     getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
-    getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
+    getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4);
     inversion_locale_ = 0u;
   }
+
+  if (enforce_path_rotation_) {
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
+  }
 }
 
 std::pair<nav_msgs::msg::Path, PathIterator>
@@ -131,11 +137,16 @@ nav_msgs::msg::Path PathHandler::transformPath(
 
   prunePlan(global_plan_up_to_inversion_, lower_bound);
 
-  if (enforce_path_inversion_ && inversion_locale_ != 0u) {
+  if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) {
     if (isWithinInversionTolerances(global_pose)) {
+      // Robot has reached the inversion/rotation point, unlock the rest of the path
       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_);
+
+      // Recompute locale on the updated path
+      float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_check);
     }
   }
 
@@ -156,10 +167,12 @@ double PathHandler::getMaxCostmapDist()
 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_);
-  }
+  global_plan_up_to_inversion_ = plan;
+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_check);
 }
 
 nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
@@ -199,7 +212,7 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam
     tf2::getYaw(robot_pose.pose.orientation),
     tf2::getYaw(last_pose.pose.orientation));
 
-  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
 }
 
 }  // namespace mppi