From 4d7284517dbf5d35ec1b1ae6bc1cc7c4919eba3b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 18:35:23 +0100 Subject: [PATCH 1/2] PR fixes Signed-off-by: Tony Najjar --- .../tools/path_handler.hpp | 8 +- .../nav2_mppi_controller/tools/utils.hpp | 82 +++++++++++++++++-- nav2_mppi_controller/src/path_handler.cpp | 31 +++++-- 3 files changed, 101 insertions(+), 20 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 4233c91ef28..e3873e13cf5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/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 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 4a3cc6b7c06..0f7c0dfd8db 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/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; } /** diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 55931f6bc62..026169257be 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/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 @@ -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 From 89956172c72594aa9d4d1c1601a7f58c1629111a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 18:35:23 +0100 Subject: [PATCH 2/2] refactor Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/tools/utils.hpp | 141 ++++++++---------- 1 file changed, 63 insertions(+), 78 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 0f7c0dfd8db..e8a5e7eb2a8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -511,90 +511,83 @@ 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 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 + * @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 findFirstPathRotation( - const nav_msgs::msg::Path & path, - float rotation_threshold) +inline unsigned int findFirstPathInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) { 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 + 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++; } - end_idx++; - } - return end_idx; + first_constraint = end_idx; + break; + } } } } - 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 - * @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; + // 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; } /** @@ -609,15 +602,7 @@ inline unsigned int removePosesAfterFirstInversion( { 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); - first_constraint = std::min(first_constraint, first_after_inversion); + const unsigned int first_constraint = findFirstPathInversion(cropped_path, rotation_threshold); if (first_constraint == path.poses.size()) { return 0u; // No constraint found