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);
 
@@ -156,11 +156,8 @@ class PathHandler
   double prune_distance_{0};
   double transform_tolerance_{0};
   float inversion_xy_tolerance_{0.2};
-  float inversion_yaw_tolerance{0.4};
-  float rotation_xy_tolerance_{0.2};
-  float rotation_yaw_tolerance_{0.4};
-  float rotation_translation_threshold_{0.02};
-  float rotation_rotation_threshold_{0.785};
+  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};

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -513,59 +513,50 @@ inline void savitskyGolayFilter(
 /**
  * @brief Find the first pose at which there is an in-place rotation in the path
  * @param path Path to check
- * @param translation_threshold Maximum translation per pose transition
- * @param rotation_threshold Minimum total rotation to consider significant
+ * @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 translation_threshold,
   float rotation_threshold)
 {
   if (path.poses.size() < 2) {
     return path.poses.size();
   }
 
-  // Iterate through path to find first rotation sequence
-  for (unsigned int start_idx = 0; start_idx < path.poses.size() - 1; ++start_idx) {
-    // Check initial translation
-    float ab_x = path.poses[start_idx + 1].pose.position.x -
-      path.poses[start_idx].pose.position.x;
-    float ab_y = path.poses[start_idx + 1].pose.position.y -
-      path.poses[start_idx].pose.position.y;
-    float initial_translation = sqrtf(ab_x * ab_x + ab_y * ab_y);
-
-    if (initial_translation >= translation_threshold) {
-      continue;  // Not a rotation sequence, try next pose
-    }
-
-    // Start of potential rotation sequence
-    float start_yaw = tf2::getYaw(path.poses[start_idx].pose.orientation);
-    unsigned int j = start_idx + 1;
-    unsigned int last_valid_j = j;  // Track the last pose in the rotation sequence
-
-    // Keep accumulating poses while translation remains small
-    while (j < path.poses.size() - 1) {
-      float next_x = path.poses[j + 1].pose.position.x - path.poses[j].pose.position.x;
-      float next_y = path.poses[j + 1].pose.position.y - path.poses[j].pose.position.y;
-      float next_translation = sqrtf(next_x * next_x + next_y * next_y);
+  // 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++;
+        }
 
-      // If individual translation is above threshold, end of rotation sequence
-      if (next_translation >= translation_threshold) {
-        break;
+        return end_idx;
       }
-
-      j++;
-      last_valid_j = j;  // Update the last valid pose in the sequence
-    }
-
-    // Check if we have accumulated enough rotation
-    float end_yaw = tf2::getYaw(path.poses[last_valid_j].pose.orientation);
-    float total_rotation = fabs(angles::shortest_angular_distance(start_yaw, end_yaw));
-
-    if (total_rotation > rotation_threshold) {
-      // Return the index after the last pose in the rotation sequence
-      return last_valid_j + 1;
     }
   }
 
@@ -608,22 +599,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,18 +39,16 @@ 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;
   }
-  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
   if (enforce_path_rotation_) {
-    getParam(rotation_xy_tolerance_, "rotation_xy_tolerance", 0.2);
-    getParam(rotation_yaw_tolerance_, "rotation_yaw_tolerance", 0.4);
-    getParam(rotation_translation_threshold_, "rotation_translation_threshold", 0.02);
-    getParam(rotation_rotation_threshold_, "rotation_rotation_threshold", 0.785);
-    rotation_locale_ = 0u;
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
   }
 }
 
@@ -139,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);
     }
   }
 
@@ -173,26 +176,12 @@ double PathHandler::getMaxCostmapDist()
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;
+  global_plan_up_to_inversion_ = plan;
 
-  // Start with the full plan and progressively apply restrictions
-  nav_msgs::msg::Path most_restrictive_plan = global_plan_;
-
-  // Check for rotation restrictions first
-  if (enforce_path_rotation_) {
-    rotation_locale_ = utils::removePosesAfterFirstRotation(
-      most_restrictive_plan,
-      rotation_translation_threshold_,
-      rotation_rotation_threshold_);
-  }
-
-  // Check for inversion restrictions (may further restrict the path)
-  if (enforce_path_inversion_) {
-    inversion_locale_ = utils::removePosesAfterFirstInversion(most_restrictive_plan);
-  }
-
-  // Set the working paths to the most restrictive version
-  global_plan_up_to_rotation_ = most_restrictive_plan;
-  global_plan_up_to_inversion_ = most_restrictive_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_;}
@@ -232,7 +221,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_;
 }
 
 bool PathHandler::isWithinRotationTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)