Add rotation tolerance checks and path rotation handling in PathHandler

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

Author: tonynajjar

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -135,22 +135,36 @@ class PathHandler
     */
   bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
 
+  /**
+    * @brief Check if the robot pose is within the set rotation tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the set rotation tolerances
+    */
+  bool isWithinRotationTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
   std::string name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   ParametersHandler * parameters_handler_;
 
   nav_msgs::msg::Path global_plan_;
   nav_msgs::msg::Path global_plan_up_to_inversion_;
+  nav_msgs::msg::Path global_plan_up_to_rotation_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 
   double max_robot_pose_search_dist_{0};
   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};
   bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
   unsigned int inversion_locale_{0u};
+  unsigned int rotation_locale_{0u};
 };
 }  // namespace mppi
 

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -510,8 +510,71 @@ 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 translation_threshold Maximum translation per pose transition
+ * @param rotation_threshold Minimum total rotation to consider significant
+ * @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);
+
+      // If individual translation is above threshold, end of rotation sequence
+      if (next_translation >= translation_threshold) {
+        break;
+      }
+
+      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;
+    }
+  }
+
+  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,
+ * or where the robot rotates in place
  * @param path to check for inversion
  * @return the first point after the inversion found in the path
  */
@@ -563,6 +626,38 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
   return first_after_inversion;
 }
 
+/**
+ * @brief Find and remove poses after the first in-place rotation in the path
+ * @param path to check for rotation
+ * @param translation_threshold Maximum translation per pose transition
+ * @param rotation_threshold Minimum total rotation to consider significant
+ * @return The location of the rotation, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstRotation(
+  nav_msgs::msg::Path & path,
+  float translation_threshold,
+  float rotation_threshold)
+{
+  if (path.poses.size() < 2) {
+    return 0u;
+  }
+
+  nav_msgs::msg::Path cropped_path = path;
+  // Find first in-place rotation
+  const unsigned int first_after_rotation = findFirstPathRotation(
+    cropped_path, translation_threshold, rotation_threshold);
+  
+  if (first_after_rotation == path.poses.size()) {
+    return 0u;  // No rotation found
+  }
+
+  // Remove poses after the rotation
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_rotation, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_rotation;
+}
+
 /**
  * @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

@@ -44,6 +44,14 @@ void PathHandler::initialize(
     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;
+  }
 }
 
 std::pair<nav_msgs::msg::Path, PathIterator>
@@ -139,6 +147,15 @@ nav_msgs::msg::Path PathHandler::transformPath(
     }
   }
 
+  if (enforce_path_rotation_ && rotation_locale_ != 0u) {
+    if (isWithinRotationTolerances(global_pose)) {
+      // Robot has completed the rotation, unlock the rest of the path
+      global_plan_up_to_rotation_ = global_plan_;
+      global_plan_up_to_inversion_ = global_plan_;
+      rotation_locale_ = 0u;  // Reset since rotation is complete
+    }
+  }
+
   if (transformed_plan.poses.empty()) {
     throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
   }
@@ -156,10 +173,26 @@ double PathHandler::getMaxCostmapDist()
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;
-  global_plan_up_to_inversion_ = global_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(global_plan_up_to_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;
 }
 
 nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
@@ -202,4 +235,18 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam
   return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
 }
 
+bool PathHandler::isWithinRotationTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the rotation pose
+  const auto last_pose = global_plan_up_to_rotation_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+  return distance <= rotation_xy_tolerance_ && fabs(angle_distance) <= rotation_yaw_tolerance_;
+}
+
 }  // namespace mppi