PR fixes

Signed-off-by: Tony Najjar <[email protected]>

Author: tonynajjar

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

@@ -510,6 +510,59 @@ inline void savitskyGolayFilter(
     control_sequence.wz(offset)};
 }
 
+/**
+ * @brief Find the first pose at which there is an in-place rotation in the path
+ * @param path Path to check
+ * @param rotation_threshold Minimum rotation angle to consider this an in-place rotation
+ * @return Index after the rotation sequence if found, path size if no rotation detected
+ */
+inline unsigned int findFirstPathRotation(
+  const nav_msgs::msg::Path & path,
+  float rotation_threshold)
+{
+  if (path.poses.size() < 2) {
+    return path.poses.size();
+  }
+
+  // Iterate through path to find first in-place rotation
+  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++;
+        }
+
+        return end_idx;
+      }
+    }
+  }
+
+  return path.poses.size();  // No significant rotation detected
+}
+
 /**
  * @brief Find the iterator of the first pose at which there is an inversion on the path,
  * @param path to check for inversion
@@ -545,22 +598,35 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
 }
 
 /**
- * @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;
+
+  // Check for in-place rotation first (if enabled)
+  unsigned int first_constraint = path.poses.size();
+  if (rotation_threshold > 0.0f) {
+    first_constraint = findFirstPathRotation(cropped_path, rotation_threshold);
+  }
+
+  // Check for inversion and take whichever comes first
   const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
-  if (first_after_inversion == path.poses.size()) {
-    return 0u;
+  first_constraint = std::min(first_constraint, first_after_inversion);
+
+  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