From 38e74df88362ac4e15785910eaee90524a1fd7a5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 14:07:22 +0200 Subject: [PATCH 01/19] Implement integral distance for critics Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/models/path.hpp | 2 ++ .../nav2_mppi_controller/optimizer.hpp | 7 ++++++ .../tools/path_handler.hpp | 14 +++++++++++ .../nav2_mppi_controller/tools/utils.hpp | 11 ++++++++ nav2_mppi_controller/src/controller.cpp | 2 +- .../src/critics/cost_critic.cpp | 4 +-- .../src/critics/goal_angle_critic.cpp | 5 ++-- .../src/critics/goal_critic.cpp | 8 +++--- .../src/critics/obstacles_critic.cpp | 4 +-- .../src/critics/path_align_critic.cpp | 7 ++---- .../src/critics/path_angle_critic.cpp | 6 ++--- .../src/critics/path_follow_critic.cpp | 6 ++--- .../src/critics/prefer_forward_critic.cpp | 8 +++--- .../src/critics/twirling_critic.cpp | 10 +++++--- nav2_mppi_controller/src/optimizer.cpp | 8 +++++- nav2_mppi_controller/src/path_handler.cpp | 25 +++++++++++++++++++ 16 files changed, 93 insertions(+), 34 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index bc969796f7e..5c5ab5c96e4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -29,6 +29,8 @@ struct Path Eigen::ArrayXf x; Eigen::ArrayXf y; Eigen::ArrayXf yaws; + float plan_length; + float plan_length_up_to_inversion; /** * @brief Reset path data diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e70cfcab786..ebed847eb5e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -90,6 +90,8 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track + * @param plan_length Length of the global plan from current robot position to the end + * @param plan_length_up_to_inversion Length of the plan from current robot position up to the inversion point (if any) * @param goal Given Goal pose to reach. * @param goal_checker Object to check if goal is completed * @return Tuple of [TwistStamped command, optimal trajectory] @@ -97,6 +99,7 @@ class Optimizer std::tuple evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const float & plan_length, const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** @@ -150,12 +153,16 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track + * @param plan_length Length of the global plan from current robot position to the end + * @param plan_length_up_to_inversion Length of the plan from current robot position up to the inversion point (if any) * @param goal_checker Object to check if goal is completed */ void prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const float & plan_length, + const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** 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..40e01be2a83 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 @@ -97,6 +97,18 @@ class PathHandler */ geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + /** + * @brief Get the length of the global plan up to inversion point + * @return Length of the global plan up to inversion point + */ + float getPlanLengthUpToInversion() const; + + /** + * @brief Get the total length of the global plan + * @return Total length of the global plan + */ + float getPlanLength() const; + protected: /** * @brief Get largest dimension of costmap (radially) @@ -141,7 +153,9 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; + float global_plan_length_{0}; nav_msgs::msg::Path global_plan_up_to_inversion_; + float global_plan_length_up_to_inversion_{0}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; 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 f518f16e169..25b83b72950 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -261,6 +261,17 @@ inline geometry_msgs::msg::Pose getCriticGoal( } } +inline float getCriticGoalPathDistance( + const CriticData & data, + bool enforce_path_inversion) +{ + if (enforce_path_inversion) { + return data.path.plan_length_up_to_inversion; + } else { + return data.path.plan_length; + } +} + /** * @brief Check if the robot pose is within the Goal Checker's tolerances to goal * @param global_checker Pointer to the goal checker diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a9fab10c85e..e8dff60707a 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -107,7 +107,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, path_handler_.getPlanLength(), path_handler_.getPlanLengthUpToInversion(), goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index dedf910187e..d8eb5197024 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -135,7 +135,7 @@ void CostCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); @@ -153,7 +153,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { + if (distance < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 8ac899140f8..c41e84a78d0 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -41,10 +41,9 @@ void GoalAngleCritic::score(CriticData & data) } geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (distance > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 927ef950de8..e1a5e0f4647 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -42,13 +42,15 @@ void GoalCritic::score(CriticData & data) } geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (distance > threshold_to_consider_) { return; } + // Use euclidean distance for cost calculation instead of integral path distance because it's faster + // When within threshold_to_consider_ of the goal, euclidean distance provides + // a computationally efficient approximation with negligible accuracy loss. auto goal_x = goal.position.x; auto goal_y = goal.position.y; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index db2972dfc9e..9f76d90abe6 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -130,11 +130,11 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { + if (distance < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 3ea13c641ab..50882c6b98d 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -46,12 +46,9 @@ void PathAlignCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); // Don't apply close to goal, let the goal critics take over - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (distance < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 96ceae1b93b..7b6e54371ed 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -66,11 +66,9 @@ void PathAngleCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (distance > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 4f8c1c70eec..13dfb41e2d8 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -40,11 +40,9 @@ void PathFollowCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (data.path.x.size() < 2 || - utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) + if (data.path.x.size() < 2 || distance < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 47c84321559..e6da34e0206 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -41,11 +41,9 @@ void PreferForwardCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); + + if (distance > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index c9cfc79233c..521b0b68460 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -39,11 +39,13 @@ void TwirlingCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (utils::withinPositionGoalTolerance( - data.goal_checker, data.state.pose.pose, goal)) - { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); + + if (distance < pose_tolerance.position.x) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 97a16249814..dbda2b0fd23 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -183,10 +183,12 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const float & plan_length, + const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, goal_checker); + prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; @@ -252,12 +254,16 @@ void Optimizer::prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const float & plan_length, + const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { state_.pose = robot_pose; state_.speed = robot_speed; path_ = utils::toTensor(plan); + path_.plan_length = plan_length; + path_.plan_length_up_to_inversion = plan_length_up_to_inversion; costs_.setZero(); goal_ = goal; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 55931f6bc62..1de4c911350 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -139,6 +139,21 @@ nav_msgs::msg::Path PathHandler::transformPath( } } + // Calculate distances using the original global plan + // Find the distance from lower_bound (closest point to robot) to the end of global_plan_up_to_inversion_ + auto start_idx = std::distance(global_plan_up_to_inversion_.poses.begin(), lower_bound); + global_plan_length_up_to_inversion_ = nav2_util::geometry_utils::calculate_path_length(global_plan_up_to_inversion_, start_idx); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + // For paths with inversion: length from closest point to robot to end of full path + auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), + global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), lower_bound)); + global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, full_plan_start_idx); + } + else { + global_plan_length_ = global_plan_length_up_to_inversion_; + } + if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } @@ -202,4 +217,14 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } +float PathHandler::getPlanLengthUpToInversion() const +{ + return global_plan_length_up_to_inversion_; +} + +float PathHandler::getPlanLength() const +{ + return global_plan_length_; +} + } // namespace mppi From 26baa2bc860dcc254ad98896f4b95b39b5c3ec24 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 14:48:54 +0200 Subject: [PATCH 02/19] . Signed-off-by: Tony Najjar --- .../tools/path_handler.hpp | 14 +++++ nav2_mppi_controller/src/path_handler.cpp | 55 ++++++++++++------- 2 files changed, 48 insertions(+), 21 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 40e01be2a83..19ce608aa86 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 @@ -133,6 +133,20 @@ class PathHandler std::pair getGlobalPlanConsideringBoundsInCostmapFrame( const geometry_msgs::msg::PoseStamped & global_pose); + /** + * @brief Extract path segments (closest point and pruned end) from global plan + * @param global_pose Robot pose in global plan frame + * @return pair of iterators marking closest point and pruned end + */ + std::pair extractPathSegments( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Calculate path lengths for both full path and path up to inversion + * @param closest_point Iterator to closest point on path + */ + void calculatePathLengths(const PathIterator & closest_point); + /** * @brief Prune a path to only interesting portions * @param plan Plan to prune diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 1de4c911350..faa0532c7b2 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -46,8 +46,7 @@ void PathHandler::initialize( } } -std::pair -PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( +std::pair PathHandler::extractPathSegments( const geometry_msgs::msg::PoseStamped & global_pose) { using nav2_util::geometry_utils::euclidean_distance; @@ -67,14 +66,43 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( return euclidean_distance(global_pose, ps); }); - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); - transformed_plan.header.stamp = global_pose.header.stamp; - auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + return {closest_point, pruned_plan_end}; +} + +void PathHandler::calculatePathLengths( + const PathIterator & closest_point) +{ + // Calculate path length from closest point to end of global_plan_up_to_inversion_ + auto start_idx_up_to_inversion = std::distance(global_plan_up_to_inversion_.poses.begin(), closest_point); + global_plan_length_up_to_inversion_ = nav2_util::geometry_utils::calculate_path_length( + global_plan_up_to_inversion_, start_idx_up_to_inversion); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + // For paths with inversion: calculate length from closest point to end of full path + auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), + global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), closest_point)); + global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, full_plan_start_idx); + } else { + global_plan_length_ = global_plan_length_up_to_inversion_; + } +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + // Extract path segments and calculate lengths first + auto [closest_point, pruned_plan_end] = extractPathSegments(global_pose); + calculatePathLengths(closest_point); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + unsigned int mx, my; // Find the furthest relevant pose on the path to consider within costmap // bounds @@ -139,21 +167,6 @@ nav_msgs::msg::Path PathHandler::transformPath( } } - // Calculate distances using the original global plan - // Find the distance from lower_bound (closest point to robot) to the end of global_plan_up_to_inversion_ - auto start_idx = std::distance(global_plan_up_to_inversion_.poses.begin(), lower_bound); - global_plan_length_up_to_inversion_ = nav2_util::geometry_utils::calculate_path_length(global_plan_up_to_inversion_, start_idx); - - if (enforce_path_inversion_ && inversion_locale_ != 0u) { - // For paths with inversion: length from closest point to robot to end of full path - auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), - global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), lower_bound)); - global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, full_plan_start_idx); - } - else { - global_plan_length_ = global_plan_length_up_to_inversion_; - } - if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } From 47ebb214e4adf127a96f411c1886f86efda7b010 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 15:02:08 +0200 Subject: [PATCH 03/19] linting Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/controller.cpp | 3 ++- .../src/critics/path_follow_critic.cpp | 3 +-- .../src/critics/prefer_forward_critic.cpp | 2 +- nav2_mppi_controller/src/optimizer.cpp | 3 ++- nav2_mppi_controller/src/path_handler.cpp | 13 ++++++++----- 5 files changed, 14 insertions(+), 10 deletions(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index e8dff60707a..b5067c7b705 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -107,7 +107,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, path_handler_.getPlanLength(), path_handler_.getPlanLengthUpToInversion(), goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, path_handler_.getPlanLength(), + path_handler_.getPlanLengthUpToInversion(), goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 13dfb41e2d8..541a6893fa8 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -42,8 +42,7 @@ void PathFollowCritic::score(CriticData & data) float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (data.path.x.size() < 2 || distance < threshold_to_consider_) - { + if (data.path.x.size() < 2 || distance < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index e6da34e0206..a46698551fd 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -42,7 +42,7 @@ void PreferForwardCritic::score(CriticData & data) } float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - + if (distance > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index dbda2b0fd23..10db776a77a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -188,7 +188,8 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, goal_checker); + prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, + goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index faa0532c7b2..677f9165c74 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -77,15 +77,18 @@ void PathHandler::calculatePathLengths( const PathIterator & closest_point) { // Calculate path length from closest point to end of global_plan_up_to_inversion_ - auto start_idx_up_to_inversion = std::distance(global_plan_up_to_inversion_.poses.begin(), closest_point); + auto start_idx_up_to_inversion = std::distance(global_plan_up_to_inversion_.poses.begin(), + closest_point); global_plan_length_up_to_inversion_ = nav2_util::geometry_utils::calculate_path_length( global_plan_up_to_inversion_, start_idx_up_to_inversion); - + if (enforce_path_inversion_ && inversion_locale_ != 0u) { // For paths with inversion: calculate length from closest point to end of full path - auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), - global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), closest_point)); - global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, full_plan_start_idx); + auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), + global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), + closest_point)); + global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, + full_plan_start_idx); } else { global_plan_length_ = global_plan_length_up_to_inversion_; } From 331b1c10e7bfa71e2a3623ae872342d0ea4a183b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 15:03:10 +0200 Subject: [PATCH 04/19] more linting Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/goal_critic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index e1a5e0f4647..1dbff55e73b 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -48,7 +48,7 @@ void GoalCritic::score(CriticData & data) return; } - // Use euclidean distance for cost calculation instead of integral path distance because it's faster + // Use euclidean distance for cost calculation instead of integral path (it's faster) // When within threshold_to_consider_ of the goal, euclidean distance provides // a computationally efficient approximation with negligible accuracy loss. auto goal_x = goal.position.x; From ba3156ac7c89203f6ccdb4eb2735d25d3504911d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 16:09:23 +0200 Subject: [PATCH 05/19] fix tests Signed-off-by: Tony Najjar --- .../benchmark/optimizer_benchmark.cpp | 7 ++++++- nav2_mppi_controller/src/critics/twirling_critic.cpp | 12 +++++++----- nav2_mppi_controller/test/optimizer_smoke_test.cpp | 6 +++++- nav2_mppi_controller/test/optimizer_unit_tests.cpp | 6 +++++- 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index afe8e83eea9..f7fe84b6050 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -29,6 +29,7 @@ #include #include #include "tf2_ros/buffer.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" @@ -91,8 +92,12 @@ void prepareAndRunBenchmark( auto path = getIncrementalDummyPath(node, path_settings); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; + float plan_length = nav2_util::geometry_utils::calculate_path_length(path); + float plan_length_up_to_inversion = plan_length; // assume no inversion + for (auto _ : state) { - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, path.poses.back().pose, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, + plan_length_up_to_inversion, path.poses.back().pose, dummy_goal_checker); } } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 521b0b68460..0cacf2352e3 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -41,12 +41,14 @@ void TwirlingCritic::score(CriticData & data) float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist velocity_tolerance; - data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); + if (data.goal_checker != nullptr) { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (distance < pose_tolerance.position.x) { - return; + if (distance < pose_tolerance.position.x) { + return; + } } if (power_ > 1u) { diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 733229b77df..be689b094be 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -22,6 +22,7 @@ #include #include #include "tf2_ros/buffer.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -79,7 +80,10 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker); + float plan_length = nav2_util::geometry_utils::calculate_path_length(path); + float plan_length_up_to_inversion = plan_length; // For test purposes, assume no inversion + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, + plan_length_up_to_inversion, goal, dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); } diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index eec9ea0ec57..4b904bec022 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/optimizer.hpp" +#include "nav2_util/geometry_utils.hpp" #include "tf2_ros/buffer.hpp" // Tests main optimizer functions @@ -119,7 +120,10 @@ class OptimizerTester : public Optimizer const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, goal_checker); + float plan_length = nav2_util::geometry_utils::calculate_path_length(plan); + float plan_length_up_to_inversion = plan_length; // For test purposes, assume no inversion + prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, + goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(costs_.sum(), 0, 1e-6); // should be reset From 694ccc97f0e00d5b76b1593eb16301d626494f18 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 16:24:20 +0200 Subject: [PATCH 06/19] reset path length Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/models/path.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index 5c5ab5c96e4..ae99243fdee 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -40,6 +40,8 @@ struct Path x.setZero(size); y.setZero(size); yaws.setZero(size); + plan_length = 0.0f; + plan_length_up_to_inversion = 0.0f; } }; From aabe91b904782001bcf338c52ba103e546aa0184 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 17:00:36 +0200 Subject: [PATCH 07/19] fix sign Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 2 +- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 7b6e54371ed..fa0b746113a 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -68,7 +68,7 @@ void PathAngleCritic::score(CriticData & data) float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (distance > threshold_to_consider_) { + if (distance < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index a46698551fd..7958b86c848 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -43,7 +43,7 @@ void PreferForwardCritic::score(CriticData & data) float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (distance > threshold_to_consider_) { + if (distance < threshold_to_consider_) { return; } From 47c7f3e8a76916455401a23950bfcf3c2c4c9af8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 17:17:17 +0200 Subject: [PATCH 08/19] fix return value Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index fa0b746113a..e4f6c483a9c 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -150,6 +150,7 @@ void PathAngleCritic::score(CriticData & data) return; } } + return true; } } // namespace mppi::critics From 86029fac4482ed4412369f71732dc38523cc8996 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Aug 2025 17:17:57 +0200 Subject: [PATCH 09/19] Revert "fix return value" This reverts commit 8b21e890de598ab6b35c4a0200c82ebfc4b0c9c8. Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index e4f6c483a9c..fa0b746113a 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -150,7 +150,6 @@ void PathAngleCritic::score(CriticData & data) return; } } - return true; } } // namespace mppi::critics From 2f66b45549bf9bb509bada2618cd0e4ee91700ef Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 12:28:32 +0200 Subject: [PATCH 10/19] remove extra variable Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/cost_critic.cpp | 4 +--- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 4 +--- nav2_mppi_controller/src/critics/goal_critic.cpp | 3 +-- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 4 +--- nav2_mppi_controller/src/critics/path_align_critic.cpp | 3 +-- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 3 +-- nav2_mppi_controller/src/critics/path_follow_critic.cpp | 6 +++--- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 4 +--- nav2_mppi_controller/src/critics/twirling_critic.cpp | 6 +++--- 9 files changed, 13 insertions(+), 24 deletions(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index d8eb5197024..168f17adfa3 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -135,8 +135,6 @@ void CostCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); auto * costmap = collision_checker_.getCostmap(); @@ -153,7 +151,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (distance < near_goal_distance_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index c41e84a78d0..ea28f269848 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -41,9 +41,7 @@ void GoalAngleCritic::score(CriticData & data) } geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - - if (distance > threshold_to_consider_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 1dbff55e73b..6009398c00d 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -42,9 +42,8 @@ void GoalCritic::score(CriticData & data) } geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (distance > threshold_to_consider_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 9f76d90abe6..d8a29713435 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -130,11 +130,9 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (distance < near_goal_distance_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 50882c6b98d..c1d43d9dde6 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -46,9 +46,8 @@ void PathAlignCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); // Don't apply close to goal, let the goal critics take over - if (distance < threshold_to_consider_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index fa0b746113a..55c4639b884 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -66,9 +66,8 @@ void PathAngleCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (distance < threshold_to_consider_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 541a6893fa8..64c0fbc6dde 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -40,9 +40,9 @@ void PathFollowCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - - if (data.path.x.size() < 2 || distance < threshold_to_consider_) { + if (data.path.x.size() < 2 || utils::getCriticGoalPathDistance(data, + enforce_path_inversion_) < threshold_to_consider_) + { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 7958b86c848..7fb54ea4aca 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -41,9 +41,7 @@ void PreferForwardCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - - if (distance < threshold_to_consider_) { + if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 0cacf2352e3..611e7bc2109 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -39,14 +39,14 @@ void TwirlingCritic::score(CriticData & data) return; } - float distance = utils::getCriticGoalPathDistance(data, enforce_path_inversion_); - if (data.goal_checker != nullptr) { geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist velocity_tolerance; data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (distance < pose_tolerance.position.x) { + if (utils::getCriticGoalPathDistance(data, + enforce_path_inversion_) < pose_tolerance.position.x) + { return; } } From 83982913dd9af5345826ed2ba84e48c014da6033 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 12:30:50 +0200 Subject: [PATCH 11/19] add doxygen Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/tools/utils.hpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 25b83b72950..4e95d8cf182 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -261,6 +261,12 @@ inline geometry_msgs::msg::Pose getCriticGoal( } } +/** + * @brief Get the distance from the robot to the critic goal + * @param data Data to use + * @param enforce_path_inversion True to use the path inversion point + * @return float Distance to the critic goal + */ inline float getCriticGoalPathDistance( const CriticData & data, bool enforce_path_inversion) From 105d634ec921b4d04ab133354edadf5b38e8120a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 12:31:24 +0200 Subject: [PATCH 12/19] rename getCriticGoalPathDistance Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- nav2_mppi_controller/src/critics/cost_critic.cpp | 2 +- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 2 +- nav2_mppi_controller/src/critics/goal_critic.cpp | 2 +- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 2 +- nav2_mppi_controller/src/critics/path_align_critic.cpp | 2 +- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 2 +- nav2_mppi_controller/src/critics/path_follow_critic.cpp | 2 +- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 2 +- nav2_mppi_controller/src/critics/twirling_critic.cpp | 2 +- 10 files changed, 10 insertions(+), 10 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 4e95d8cf182..0fa7ddb986d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -267,7 +267,7 @@ inline geometry_msgs::msg::Pose getCriticGoal( * @param enforce_path_inversion True to use the path inversion point * @return float Distance to the critic goal */ -inline float getCriticGoalPathDistance( +inline float getIntegratedPathDistanceToGoal( const CriticData & data, bool enforce_path_inversion) { diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 168f17adfa3..96cd3d254a8 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -151,7 +151,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < near_goal_distance_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index ea28f269848..89a4fe795e5 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -41,7 +41,7 @@ void GoalAngleCritic::score(CriticData & data) } geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) > threshold_to_consider_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 6009398c00d..1e7c92bc7b2 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -43,7 +43,7 @@ void GoalCritic::score(CriticData & data) geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) > threshold_to_consider_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) > threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index d8a29713435..6a81efba417 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -132,7 +132,7 @@ void ObstaclesCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < near_goal_distance_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index c1d43d9dde6..afc73e6c09c 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -47,7 +47,7 @@ void PathAlignCritic::score(CriticData & data) } // Don't apply close to goal, let the goal critics take over - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 55c4639b884..6ba2771a8a7 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -67,7 +67,7 @@ void PathAngleCritic::score(CriticData & data) } - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 64c0fbc6dde..944dc4b2e6b 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -40,7 +40,7 @@ void PathFollowCritic::score(CriticData & data) return; } - if (data.path.x.size() < 2 || utils::getCriticGoalPathDistance(data, + if (data.path.x.size() < 2 || utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { return; diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 7fb54ea4aca..91f46197d9d 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -41,7 +41,7 @@ void PreferForwardCritic::score(CriticData & data) return; } - if (utils::getCriticGoalPathDistance(data, enforce_path_inversion_) < threshold_to_consider_) { + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 611e7bc2109..8f5a566b8fa 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -44,7 +44,7 @@ void TwirlingCritic::score(CriticData & data) geometry_msgs::msg::Twist velocity_tolerance; data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (utils::getCriticGoalPathDistance(data, + if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < pose_tolerance.position.x) { return; From ea64f0934bdabab58f9df2d488d737546aaececa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 12:41:06 +0200 Subject: [PATCH 13/19] delete withinPositionGoalTolerance Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/tools/utils.hpp | 57 ------------------- nav2_mppi_controller/test/utils_test.cpp | 50 ---------------- 2 files changed, 107 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 0fa7ddb986d..806b0e96571 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -278,63 +278,6 @@ inline float getIntegratedPathDistanceToGoal( } } -/** - * @brief Check if the robot pose is within the Goal Checker's tolerances to goal - * @param global_checker Pointer to the goal checker - * @param robot Pose of robot - * @param goal Goal pose - * @return bool If robot is within goal checker tolerances to the goal - */ -inline bool withinPositionGoalTolerance( - nav2_core::GoalChecker * goal_checker, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - if (goal_checker) { - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist velocity_tolerance; - goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - - const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - - auto dx = robot.position.x - goal.position.x; - auto dy = robot.position.y - goal.position.y; - - auto dist_sq = dx * dx + dy * dy; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - } - - return false; -} - -/** - * @brief Check if the robot pose is within tolerance to the goal - * @param pose_tolerance Pose tolerance to use - * @param robot Pose of robot - * @param goal Goal pose - * @return bool If robot is within tolerance to the goal - */ -inline bool withinPositionGoalTolerance( - float pose_tolerance, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - const double & dist_sq = - std::pow(goal.position.x - robot.position.x, 2) + - std::pow(goal.position.y - robot.position.y, 2); - - const float pose_tolerance_sq = pose_tolerance * pose_tolerance; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - - return false; -} - /** * @brief normalize * Normalizes the angle to be -M_PIF circle to +M_PIF circle diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 6f70e19cfa5..db3e12d74f0 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -116,56 +116,6 @@ TEST(UtilsTests, ConversionTests) EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6); } -TEST(UtilsTests, WithTolTests) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 10.0; - pose.position.y = 1.0; - - nav2_core::GoalChecker * goal_checker = new TestGoalChecker; - - nav_msgs::msg::Path path; - path.poses.resize(2); - geometry_msgs::msg::Pose & goal = path.poses.back().pose; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_critic; - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_critic, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - // Test not in tolerance - goal.position.x = 0.0; - goal.position.y = 0.0; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal)); - - // Test in tolerance - goal.position.x = 9.8; - goal.position.y = 0.95; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 10.0; - goal.position.y = 0.76; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 9.76; - goal.position.y = 1.0; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - delete goal_checker; - goal_checker = nullptr; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); -} - TEST(UtilsTests, AnglesTests) { // Test angle normalization by creating insane angles From 62368c01183056042172e08e5a2b0be3114adebd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 13:40:08 +0200 Subject: [PATCH 14/19] use only one path distance Signed-off-by: Tony Najjar --- .../benchmark/optimizer_benchmark.cpp | 3 +- .../nav2_mppi_controller/models/path.hpp | 4 -- .../nav2_mppi_controller/models/state.hpp | 1 + .../nav2_mppi_controller/optimizer.hpp | 8 +-- .../tools/path_handler.hpp | 28 ---------- .../nav2_mppi_controller/tools/utils.hpp | 35 ------------ nav2_mppi_controller/src/controller.cpp | 3 +- .../src/critics/cost_critic.cpp | 2 +- .../src/critics/goal_angle_critic.cpp | 7 +-- .../src/critics/goal_critic.cpp | 8 +-- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/path_align_critic.cpp | 7 +-- .../src/critics/path_angle_critic.cpp | 7 +-- .../src/critics/path_follow_critic.cpp | 3 +- .../src/critics/prefer_forward_critic.cpp | 3 +- .../src/critics/twirling_critic.cpp | 3 +- nav2_mppi_controller/src/optimizer.cpp | 9 +-- nav2_mppi_controller/src/path_handler.cpp | 53 ++---------------- .../test/optimizer_smoke_test.cpp | 5 +- .../test/optimizer_unit_tests.cpp | 3 +- nav2_mppi_controller/test/utils_test.cpp | 55 ------------------- 21 files changed, 27 insertions(+), 222 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index f7fe84b6050..a9e57fce1c1 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -93,11 +93,10 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; float plan_length = nav2_util::geometry_utils::calculate_path_length(path); - float plan_length_up_to_inversion = plan_length; // assume no inversion for (auto _ : state) { auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, - plan_length_up_to_inversion, path.poses.back().pose, + path.poses.back().pose, dummy_goal_checker); } } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index ae99243fdee..bc969796f7e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -29,8 +29,6 @@ struct Path Eigen::ArrayXf x; Eigen::ArrayXf y; Eigen::ArrayXf yaws; - float plan_length; - float plan_length_up_to_inversion; /** * @brief Reset path data @@ -40,8 +38,6 @@ struct Path x.setZero(size); y.setZero(size); yaws.setZero(size); - plan_length = 0.0f; - plan_length_up_to_inversion = 0.0f; } }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index a8c81c3b07d..c28b0df0604 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -39,6 +39,7 @@ struct State Eigen::ArrayXXf cwz; geometry_msgs::msg::PoseStamped pose; + float local_path_length; geometry_msgs::msg::Twist speed; /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index ebed847eb5e..1914abde647 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -33,6 +33,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/motion_models.hpp" @@ -90,8 +91,6 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track - * @param plan_length Length of the global plan from current robot position to the end - * @param plan_length_up_to_inversion Length of the plan from current robot position up to the inversion point (if any) * @param goal Given Goal pose to reach. * @param goal_checker Object to check if goal is completed * @return Tuple of [TwistStamped command, optimal trajectory] @@ -99,7 +98,6 @@ class Optimizer std::tuple evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const float & plan_length, const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** @@ -153,16 +151,12 @@ class Optimizer * @param robot_pose Pose of the robot at given time * @param robot_speed Speed of the robot at given time * @param plan Path plan to track - * @param plan_length Length of the global plan from current robot position to the end - * @param plan_length_up_to_inversion Length of the plan from current robot position up to the inversion point (if any) * @param goal_checker Object to check if goal is completed */ void prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const float & plan_length, - const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** 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 19ce608aa86..4233c91ef28 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 @@ -97,18 +97,6 @@ class PathHandler */ geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); - /** - * @brief Get the length of the global plan up to inversion point - * @return Length of the global plan up to inversion point - */ - float getPlanLengthUpToInversion() const; - - /** - * @brief Get the total length of the global plan - * @return Total length of the global plan - */ - float getPlanLength() const; - protected: /** * @brief Get largest dimension of costmap (radially) @@ -133,20 +121,6 @@ class PathHandler std::pair getGlobalPlanConsideringBoundsInCostmapFrame( const geometry_msgs::msg::PoseStamped & global_pose); - /** - * @brief Extract path segments (closest point and pruned end) from global plan - * @param global_pose Robot pose in global plan frame - * @return pair of iterators marking closest point and pruned end - */ - std::pair extractPathSegments( - const geometry_msgs::msg::PoseStamped & global_pose); - - /** - * @brief Calculate path lengths for both full path and path up to inversion - * @param closest_point Iterator to closest point on path - */ - void calculatePathLengths(const PathIterator & closest_point); - /** * @brief Prune a path to only interesting portions * @param plan Plan to prune @@ -167,9 +141,7 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; - float global_plan_length_{0}; nav_msgs::msg::Path global_plan_up_to_inversion_; - float global_plan_length_up_to_inversion_{0}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; 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 806b0e96571..4a3cc6b7c06 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -243,41 +243,6 @@ inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) return pathPose; } -/** - * @brief Get the target pose to be evaluated by the critic - * @param data Data to use - * @param enforce_path_inversion True to return the cusp point (last pose of the path) - * instead of the original goal - * @return geometry_msgs::msg::Pose Target pose for the critic - */ -inline geometry_msgs::msg::Pose getCriticGoal( - const CriticData & data, - bool enforce_path_inversion) -{ - if (enforce_path_inversion) { - return getLastPathPose(data.path); - } else { - return data.goal; - } -} - -/** - * @brief Get the distance from the robot to the critic goal - * @param data Data to use - * @param enforce_path_inversion True to use the path inversion point - * @return float Distance to the critic goal - */ -inline float getIntegratedPathDistanceToGoal( - const CriticData & data, - bool enforce_path_inversion) -{ - if (enforce_path_inversion) { - return data.path.plan_length_up_to_inversion; - } else { - return data.path.plan_length; - } -} - /** * @brief normalize * Normalizes the angle to be -M_PIF circle to +M_PIF circle diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index b5067c7b705..a9fab10c85e 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -107,8 +107,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( std::unique_lock costmap_lock(*(costmap->getMutex())); auto [cmd, optimal_trajectory] = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, path_handler_.getPlanLength(), - path_handler_.getPlanLengthUpToInversion(), goal, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 96cd3d254a8..7f63c523bd7 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -151,7 +151,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < near_goal_distance_) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 89a4fe795e5..2b925fae46b 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -36,14 +36,11 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) > threshold_to_consider_) { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); double goal_yaw = tf2::getYaw(goal.orientation); diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 1e7c92bc7b2..eb3f11c72c0 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -37,15 +37,11 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) > threshold_to_consider_) { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); // Use euclidean distance for cost calculation instead of integral path (it's faster) // When within threshold_to_consider_ of the goal, euclidean distance provides diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 6a81efba417..bd85a854bff 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -132,7 +132,7 @@ void ObstaclesCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < near_goal_distance_) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index afc73e6c09c..73a6f59602d 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -42,12 +42,7 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - // Don't apply close to goal, let the goal critics take over - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 6ba2771a8a7..394f3285ca4 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -62,12 +62,7 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 944dc4b2e6b..da1600baaa9 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -40,8 +40,7 @@ void PathFollowCritic::score(CriticData & data) return; } - if (data.path.x.size() < 2 || utils::getIntegratedPathDistanceToGoal(data, - enforce_path_inversion_) < threshold_to_consider_) + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 91f46197d9d..421abb0aa5e 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -41,7 +41,8 @@ void PreferForwardCritic::score(CriticData & data) return; } - if (utils::getIntegratedPathDistanceToGoal(data, enforce_path_inversion_) < threshold_to_consider_) { + if (data.state.local_path_length < threshold_to_consider_) + { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 8f5a566b8fa..4e1347dc867 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -44,8 +44,7 @@ void TwirlingCritic::score(CriticData & data) geometry_msgs::msg::Twist velocity_tolerance; data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (utils::getIntegratedPathDistanceToGoal(data, - enforce_path_inversion_) < pose_tolerance.position.x) + if (data.state.local_path_length < pose_tolerance.position.x) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 10db776a77a..bc86aaebab0 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -183,12 +183,10 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const float & plan_length, - const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, + prepare(robot_pose, robot_speed, plan, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; @@ -255,16 +253,13 @@ void Optimizer::prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - const float & plan_length, - const float & plan_length_up_to_inversion, const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { state_.pose = robot_pose; state_.speed = robot_speed; + state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); path_ = utils::toTensor(plan); - path_.plan_length = plan_length; - path_.plan_length_up_to_inversion = plan_length_up_to_inversion; costs_.setZero(); goal_ = goal; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 677f9165c74..55931f6bc62 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -46,7 +46,8 @@ void PathHandler::initialize( } } -std::pair PathHandler::extractPathSegments( +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( const geometry_msgs::msg::PoseStamped & global_pose) { using nav2_util::geometry_utils::euclidean_distance; @@ -66,46 +67,14 @@ std::pair PathHandler::extractPathSegments( return euclidean_distance(global_pose, ps); }); - auto pruned_plan_end = - nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); - - return {closest_point, pruned_plan_end}; -} - -void PathHandler::calculatePathLengths( - const PathIterator & closest_point) -{ - // Calculate path length from closest point to end of global_plan_up_to_inversion_ - auto start_idx_up_to_inversion = std::distance(global_plan_up_to_inversion_.poses.begin(), - closest_point); - global_plan_length_up_to_inversion_ = nav2_util::geometry_utils::calculate_path_length( - global_plan_up_to_inversion_, start_idx_up_to_inversion); - - if (enforce_path_inversion_ && inversion_locale_ != 0u) { - // For paths with inversion: calculate length from closest point to end of full path - auto full_plan_start_idx = std::distance(global_plan_.poses.begin(), - global_plan_.poses.begin() + std::distance(global_plan_up_to_inversion_.poses.begin(), - closest_point)); - global_plan_length_ = nav2_util::geometry_utils::calculate_path_length(global_plan_, - full_plan_start_idx); - } else { - global_plan_length_ = global_plan_length_up_to_inversion_; - } -} - -std::pair -PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( - const geometry_msgs::msg::PoseStamped & global_pose) -{ - // Extract path segments and calculate lengths first - auto [closest_point, pruned_plan_end] = extractPathSegments(global_pose); - calculatePathLengths(closest_point); - nav_msgs::msg::Path transformed_plan; transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); transformed_plan.header.stamp = global_pose.header.stamp; + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + unsigned int mx, my; // Find the furthest relevant pose on the path to consider within costmap // bounds @@ -233,14 +202,4 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } -float PathHandler::getPlanLengthUpToInversion() const -{ - return global_plan_length_up_to_inversion_; -} - -float PathHandler::getPlanLength() const -{ - return global_plan_length_; -} - } // namespace mppi diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index be689b094be..0343c199b3f 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -81,9 +81,8 @@ TEST_P(OptimizerSuite, OptimizerTest) { nav2_core::GoalChecker * dummy_goal_checker{nullptr}; float plan_length = nav2_util::geometry_utils::calculate_path_length(path); - float plan_length_up_to_inversion = plan_length; // For test purposes, assume no inversion - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, - plan_length_up_to_inversion, goal, dummy_goal_checker); + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, goal, + dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); } diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 4b904bec022..fb9ce056949 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -121,8 +121,7 @@ class OptimizerTester : public Optimizer nav2_core::GoalChecker * goal_checker) { float plan_length = nav2_util::geometry_utils::calculate_path_length(plan); - float plan_length_up_to_inversion = plan_length; // For test purposes, assume no inversion - prepare(robot_pose, robot_speed, plan, plan_length, plan_length_up_to_inversion, goal, + prepare(robot_pose, robot_speed, plan, plan_length, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index db3e12d74f0..70733e37db4 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -600,61 +600,6 @@ TEST(UtilsTests, getLastPathPoseTest) EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); } -TEST(UtilsTests, getCriticGoalTest) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 10.0; - pose.position.y = 1.0; - - nav_msgs::msg::Path path; - path.poses.resize(10); - path.poses[9].pose.position.x = 5.0; - path.poses[9].pose.position.y = 50.0; - path.poses[9].pose.orientation.x = 0.0; - path.poses[9].pose.orientation.y = 0.0; - path.poses[9].pose.orientation.z = 1.0; - path.poses[9].pose.orientation.w = 0.0; - - geometry_msgs::msg::Pose goal; - goal.position.x = 6.0; - goal.position.y = 60.0; - goal.orientation.x = 0.0; - goal.orientation.y = 0.0; - goal.orientation.z = 0.0; - goal.orientation.w = 1.0; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_t = toTensor(path); - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_t, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - bool enforce_path_inversion = true; - geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 5); - EXPECT_EQ(target_goal.position.y, 50); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3); - - enforce_path_inversion = false; - target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 6); - EXPECT_EQ(target_goal.position.y, 60); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3); -} - int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); From b80fba3cb5384ffdd56b3eb9fcf76512975feceb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 13:46:12 +0200 Subject: [PATCH 15/19] cleanup Signed-off-by: Tony Najjar --- nav2_mppi_controller/benchmark/optimizer_benchmark.cpp | 4 +--- .../include/nav2_mppi_controller/critics/cost_critic.hpp | 1 - .../nav2_mppi_controller/critics/goal_angle_critic.hpp | 1 - .../include/nav2_mppi_controller/critics/goal_critic.hpp | 1 - .../include/nav2_mppi_controller/critics/obstacles_critic.hpp | 1 - .../nav2_mppi_controller/critics/path_align_critic.hpp | 1 - .../nav2_mppi_controller/critics/path_angle_critic.hpp | 1 - .../nav2_mppi_controller/critics/path_follow_critic.hpp | 1 - .../nav2_mppi_controller/critics/prefer_forward_critic.hpp | 1 - .../include/nav2_mppi_controller/critics/twirling_critic.hpp | 1 - nav2_mppi_controller/src/critics/cost_critic.cpp | 2 -- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 2 -- nav2_mppi_controller/src/critics/goal_critic.cpp | 3 --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 2 -- nav2_mppi_controller/src/critics/path_align_critic.cpp | 3 --- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 1 - nav2_mppi_controller/src/critics/path_follow_critic.cpp | 3 --- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 2 -- nav2_mppi_controller/src/critics/twirling_critic.cpp | 3 --- nav2_mppi_controller/test/optimizer_smoke_test.cpp | 3 +-- nav2_mppi_controller/test/optimizer_unit_tests.cpp | 3 +-- 21 files changed, 3 insertions(+), 37 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index a9e57fce1c1..1f810fd8c8b 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -92,10 +92,8 @@ void prepareAndRunBenchmark( auto path = getIncrementalDummyPath(node, path_settings); nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - float plan_length = nav2_util::geometry_utils::calculate_path_length(path); - for (auto _ : state) { - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, path.poses.back().pose, dummy_goal_checker); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 2646c98fd9e..c285a14b664 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -144,7 +144,6 @@ class CostCritic : public CriticFunction std::string inflation_layer_name_; unsigned int power_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 58c108b21cd..08ec354cf3b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -46,7 +46,6 @@ class GoalAngleCritic : public CriticFunction float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 6b248f29945..8fb8fb688ae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -46,7 +46,6 @@ class GoalCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 23fb3db2ca0..31b6a3d0df8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -97,7 +97,6 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; - bool enforce_path_inversion_{false}; std::string inflation_layer_name_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 133447a8774..f7ad2fda6a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -52,7 +52,6 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 85b9d48eb83..b137270aba9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -81,7 +81,6 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index aacec055ccf..1ccd08c32fe 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -53,7 +53,6 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp index 811ba9ae9a6..e17ad235e5c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -45,7 +45,6 @@ class PreferForwardCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp index bc40b194404..3e316a567e0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -44,7 +44,6 @@ class TwirlingCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 7f63c523bd7..6b15d992ca8 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -23,8 +23,6 @@ namespace mppi::critics void CostCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 2b925fae46b..8792df2aaa2 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -20,8 +20,6 @@ namespace mppi::critics void GoalAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index eb3f11c72c0..6973b11bae6 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void GoalCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index bd85a854bff..0577db0a76f 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -24,8 +24,6 @@ namespace mppi::critics void ObstaclesCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 73a6f59602d..befa9e42eb9 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -20,12 +20,9 @@ namespace mppi::critics void PathAlignCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 4); diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 394f3285ca4..6f42d33b042 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -23,7 +23,6 @@ namespace mppi::critics void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); float vx_min; getParentParam(vx_min, "vx_min", -0.35); if (fabs(vx_min) < 1e-6f) { // zero diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index da1600baaa9..e68f26036a3 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void PathFollowCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam( threshold_to_consider_, "threshold_to_consider", 1.4f); diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 421abb0aa5e..38280653403 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -22,8 +22,6 @@ namespace mppi::critics void PreferForwardCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 4e1347dc867..f46a2554fc7 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void TwirlingCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 0343c199b3f..c88422f3a84 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -80,8 +80,7 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - float plan_length = nav2_util::geometry_utils::calculate_path_length(path); - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, plan_length, goal, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index fb9ce056949..b56f98c02d7 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -120,8 +120,7 @@ class OptimizerTester : public Optimizer const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - float plan_length = nav2_util::geometry_utils::calculate_path_length(plan); - prepare(robot_pose, robot_speed, plan, plan_length, goal, + prepare(robot_pose, robot_speed, plan, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); From 4c0b7875a060c4fadb78a39506c40a23e8ec7e14 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 14:12:46 +0200 Subject: [PATCH 16/19] linting Signed-off-by: Tony Najjar --- nav2_mppi_controller/benchmark/optimizer_benchmark.cpp | 4 +--- nav2_mppi_controller/src/critics/path_follow_critic.cpp | 3 +-- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 3 +-- nav2_mppi_controller/src/critics/twirling_critic.cpp | 3 +-- 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 1f810fd8c8b..afe8e83eea9 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -29,7 +29,6 @@ #include #include #include "tf2_ros/buffer.hpp" -#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" @@ -93,8 +92,7 @@ void prepareAndRunBenchmark( nav2_core::GoalChecker * dummy_goal_checker{nullptr}; for (auto _ : state) { - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, - path.poses.back().pose, + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, path.poses.back().pose, dummy_goal_checker); } } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index e68f26036a3..240c49725d6 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -37,8 +37,7 @@ void PathFollowCritic::score(CriticData & data) return; } - if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) - { + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 38280653403..20c58324365 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -39,8 +39,7 @@ void PreferForwardCritic::score(CriticData & data) return; } - if (data.state.local_path_length < threshold_to_consider_) - { + if (data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index f46a2554fc7..2c6842c3a26 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -41,8 +41,7 @@ void TwirlingCritic::score(CriticData & data) geometry_msgs::msg::Twist velocity_tolerance; data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (data.state.local_path_length < pose_tolerance.position.x) - { + if (data.state.local_path_length < pose_tolerance.position.x) { return; } } From e807c1c27f24ef4177ab758ee35263d5d26b1744 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 14:23:48 +0200 Subject: [PATCH 17/19] reorder Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/models/state.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index c28b0df0604..50f9d400a91 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -39,8 +39,8 @@ struct State Eigen::ArrayXXf cwz; geometry_msgs::msg::PoseStamped pose; - float local_path_length; geometry_msgs::msg::Twist speed; + float local_path_length; /** * @brief Reset state data From 705421ac96241ed1bb85219d607db9af52b1959e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 16:54:05 +0200 Subject: [PATCH 18/19] format Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/goal_critic.cpp | 3 --- nav2_mppi_controller/src/optimizer.cpp | 3 +-- nav2_mppi_controller/test/optimizer_unit_tests.cpp | 4 +--- 3 files changed, 2 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 6973b11bae6..9889ef3f071 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -40,9 +40,6 @@ void GoalCritic::score(CriticData & data) geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); - // Use euclidean distance for cost calculation instead of integral path (it's faster) - // When within threshold_to_consider_ of the goal, euclidean distance provides - // a computationally efficient approximation with negligible accuracy loss. auto goal_x = goal.position.x; auto goal_y = goal.position.y; diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index bc86aaebab0..013b29de502 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -186,8 +186,7 @@ std::tuple Optimizer::evalCon const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, - goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; bool trajectory_valid = true; diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index b56f98c02d7..eec9ea0ec57 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -18,7 +18,6 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/optimizer.hpp" -#include "nav2_util/geometry_utils.hpp" #include "tf2_ros/buffer.hpp" // Tests main optimizer functions @@ -120,8 +119,7 @@ class OptimizerTester : public Optimizer const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal, - goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(costs_.sum(), 0, 1e-6); // should be reset From 614a26c95403b5d0486137e5a895a6af3112a90e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 29 Aug 2025 17:15:27 +0200 Subject: [PATCH 19/19] fix tests Signed-off-by: Tony Najjar --- nav2_mppi_controller/test/critics_tests.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 813cba14318..88e823019a7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -251,16 +251,19 @@ TEST(CriticTests, GoalAngleCritic) goal.orientation.y = 0.0; goal.orientation.z = 1.0; goal.orientation.w = 0.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Let's move it even closer, just to be sure it still doesn't trigger state.pose.pose.position.x = 9.2; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide state pose and path below `threshold_to_consider` to consider state.pose.pose.position.x = 9.7; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_GT(costs.sum(), 0); EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight @@ -305,6 +308,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 10.0; path.y(9) = 0.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 @@ -314,6 +318,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 0.5; path.y(9) = 0.0; goal.position.x = 0.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 @@ -360,12 +365,14 @@ TEST(CriticTests, PathAngleCritic) state.pose.pose.position.y = 0.0; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with less than PI/2 angular diff. path.x(9) = 0.95; goal.position.x = 0.95; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); data.furthest_reached_path_point = 2; // So it grabs the 2 + offset_from_furthest_ = 6th point path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_ path.y(6) = 0.0; @@ -478,12 +485,14 @@ TEST(CriticTests, PreferForwardCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.vx.setOnes(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); @@ -536,12 +545,14 @@ TEST(CriticTests, TwirlingCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.wz.setZero(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -601,6 +612,7 @@ TEST(CriticTests, PathFollowCritic) state.pose.pose.position.x = 2.0; path.x(5) = 1.8; goal.position.x = 1.8; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -608,6 +620,7 @@ TEST(CriticTests, PathFollowCritic) // pose differential is (0, 0) and (0.15, 0) path.x(5) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } @@ -653,6 +666,7 @@ TEST(CriticTests, PathAlignCritic) state.pose.pose.position.x = 1.0; path.x(9) = 0.85; goal.position.x = 0.85; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -660,6 +674,7 @@ TEST(CriticTests, PathAlignCritic) // but data furthest point reached is 0 and offset default is 20, so returns path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -668,6 +683,7 @@ TEST(CriticTests, PathAlignCritic) *data.furthest_reached_path_point = 21; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -699,6 +715,7 @@ TEST(CriticTests, PathAlignCritic) path.x(20) = 0.9; path.x(21) = 0.9; goal.position.x = 0.9; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); generated_trajectories.x.setConstant(0.66f); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -719,6 +736,7 @@ TEST(CriticTests, PathAlignCritic) path.x.setConstant(1.5f); path.y.setConstant(1.5f); goal.position.x = 1.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); }