From cfabf7e06e2bf638652a1a39c12aa42242970484 Mon Sep 17 00:00:00 2001 From: Revert Date: Thu, 18 Dec 2025 15:00:33 -0800 Subject: [PATCH 01/10] merged symmetric yaw tolerance from humble --- .../plugins/simple_goal_checker.hpp | 1 + .../plugins/simple_goal_checker.cpp | 28 ++++++- nav2_controller/plugins/test/goal_checker.cpp | 61 ++++++++++++++ nav2_mppi_controller/README.md | 1 + .../critics/goal_angle_critic.hpp | 1 + .../src/critics/goal_angle_critic.cpp | 37 +++++++-- nav2_mppi_controller/test/critics_tests.cpp | 82 +++++++++++++++++++ 7 files changed, 199 insertions(+), 12 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 76feabf256f..ec0f445c1b3 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -74,6 +74,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker protected: double xy_goal_tolerance_, yaw_goal_tolerance_; bool stateful_, check_xy_; + bool symmetric_yaw_tolerance_; // Cached squared xy_goal_tolerance_ double xy_goal_tolerance_sq_; // Dynamic parameters handler diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index ebdc8665c08..3ba05d1bfae 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -57,6 +57,7 @@ SimpleGoalChecker::SimpleGoalChecker() yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), + symmetric_yaw_tolerance_(false), xy_goal_tolerance_sq_(0.0625) { } @@ -72,6 +73,7 @@ void SimpleGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); + symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + ".symmetric_yaw_tolerance", false); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -101,10 +103,26 @@ bool SimpleGoalChecker::isGoalReached( check_xy_ = false; } } - double dyaw = angles::shortest_angular_distance( - tf2::getYaw(query_pose.orientation), - tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) <= yaw_goal_tolerance_; + + double query_yaw = tf2::getYaw(query_pose.orientation); + double goal_yaw = tf2::getYaw(goal_pose.orientation); + + if (symmetric_yaw_tolerance_) { + // For symmetric robots: accept either goal orientation or goal + 180° + double dyaw_forward = angles::shortest_angular_distance(query_yaw, goal_yaw); + double dyaw_backward = angles::shortest_angular_distance( + query_yaw, + angles::normalize_angle(goal_yaw + M_PI)); + + bool forward_match = fabs(dyaw_forward) <= yaw_goal_tolerance_; + bool backward_match = fabs(dyaw_backward) <= yaw_goal_tolerance_; + + return forward_match || backward_match; + } else { + // Standard behavior: only accept the specified goal orientation + double dyaw = angles::shortest_angular_distance(query_yaw, goal_yaw); + return fabs(dyaw) <= yaw_goal_tolerance_; + } } bool SimpleGoalChecker::getTolerances( @@ -150,6 +168,8 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".symmetric_yaw_tolerance") { + symmetric_yaw_tolerance_ = parameter.as_bool(); } } } diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 1aba1ba80bb..a84e9d61533 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -340,6 +340,67 @@ TEST(StoppedGoalChecker, is_reached) EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); } +TEST(SimpleGoalChecker, symmetric_yaw_tolerance_disabled) +{ + auto x = std::make_shared("goal_checker_symmetric"); + + SimpleGoalChecker gc; + auto costmap = std::make_shared("test_costmap"); + + gc.initialize(x, "symmetric_test", costmap); + + // Test that with symmetric_yaw_tolerance disabled (default), + // robot at 0 rad and goal at PI rad is NOT reached + // (only exact goal orientation or close to it is accepted) + checkMacro(gc, 0, 0, 0, 0, 0, 3.14159, 0, 0, 0, false); +} + +TEST(SimpleGoalChecker, symmetric_yaw_tolerance_enabled) +{ + auto x = std::make_shared("goal_checker_symmetric_enabled"); + + SimpleGoalChecker gc; + auto costmap = std::make_shared("test_costmap"); + + gc.initialize(x, "symmetric_test2", costmap); + + // Enable symmetric_yaw_tolerance + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("symmetric_test2.symmetric_yaw_tolerance", true), + rclcpp::Parameter("symmetric_test2.yaw_goal_tolerance", 0.3)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("symmetric_test2.symmetric_yaw_tolerance").as_bool(), true); + + // Test that with symmetric_yaw_tolerance enabled, + // robot at 0 rad and goal at PI rad IS reached + // (because robot can face backward, which is PI rad) + checkMacro(gc, 0, 0, 0, 0, 0, 3.14159, 0, 0, 0, true); + + // Test that forward orientation still works + checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); + + // Test that intermediate angles work with tolerance + checkMacro(gc, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, true); + checkMacro(gc, 0, 0, -0.1, 0, 0, 0, 0, 0, 0, true); + + // Test that intermediate angles work for backward (PI) orientation + checkMacro(gc, 0, 0, 3.14159 + 0.1, 0, 0, 3.14159, 0, 0, 0, true); + checkMacro(gc, 0, 0, 3.14159 - 0.1, 0, 0, 3.14159, 0, 0, 0, true); + + // Test that angles outside tolerance are not reached + checkMacro(gc, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, false); + checkMacro(gc, 0, 0, 1.5, 0, 0, 3.14159, 0, 0, 0, false); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 63db3f0aa03..2d09ab095ac 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -95,6 +95,7 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | + | symmetric_yaw_tolerance | bool | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty. See [Symmetric Yaw Tolerance Guide](./symmetric_goal_critic_README.md) for details. | #### Goal Critic | Parameter | Type | Definition | 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 08ec354cf3b..0eeb98063a9 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 @@ -43,6 +43,7 @@ class GoalAngleCritic : public CriticFunction void score(CriticData & data) override; protected: + bool symmetric_yaw_tolerance_{false}; float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index a7e272b4a47..747442cb60c 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" +#include "angles/angles.h" namespace mppi::critics { @@ -24,12 +25,14 @@ void GoalAngleCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); + getParam(symmetric_yaw_tolerance_, "symmetric_yaw_tolerance", false); RCLCPP_INFO( logger_, - "GoalAngleCritic instantiated with %d power, %f weight, and %f " - "angular threshold.", - power_, weight_, threshold_to_consider_); + "GoalAngleCritic instantiated with %d power, %f weight, %f " + "angular threshold, and symmetric_yaw_tolerance %s.", + power_, weight_, threshold_to_consider_, + symmetric_yaw_tolerance_ ? "enabled" : "disabled"); } void GoalAngleCritic::score(CriticData & data) @@ -42,12 +45,30 @@ void GoalAngleCritic::score(CriticData & data) double goal_yaw = tf2::getYaw(goal.orientation); - if (power_ > 1u) { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).pow(power_).eval(); + if (symmetric_yaw_tolerance_) { + // For symmetric robots: use minimum distance to either goal orientation or goal + 180° + const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); + + auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs(); + auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw_flipped).abs(); + + // Use the minimum distance + auto min_distance = distance_to_goal.cwiseMin(distance_to_flipped); + + if (power_ > 1u) { + data.costs += (min_distance.rowwise().mean() * weight_).pow(power_).eval(); + } else { + data.costs += (min_distance.rowwise().mean() * weight_).eval(); + } } else { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).eval(); + // Standard behavior: only consider the specified goal orientation + if (power_ > 1u) { + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_).pow(power_).eval(); + } else { + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_).eval(); + } } } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 51ad7897fb0..9cb9d8c74ac 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -269,6 +269,88 @@ TEST(CriticTests, GoalAngleCritic) EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight } +TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap"); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + // Set parameters for critic with symmetric disabled + node->declare_parameter("critic_sym_disabled.cost_power", 1); + node->declare_parameter("critic_sym_disabled.cost_weight", 3.0); + node->declare_parameter("critic_sym_disabled.threshold_to_consider", 0.5); + node->declare_parameter("critic_sym_disabled.symmetric_yaw_tolerance", false); + + // Set parameters for critic with symmetric enabled + node->declare_parameter("critic_sym_enabled.cost_power", 1); + node->declare_parameter("critic_sym_enabled.cost_weight", 3.0); + node->declare_parameter("critic_sym_enabled.threshold_to_consider", 0.5); + node->declare_parameter("critic_sym_enabled.symmetric_yaw_tolerance", true); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + + // Setup goal angle critic with symmetric_yaw_tolerance disabled (default) + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic_sym_disabled", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic_sym_disabled"); + + // Setup goal angle critic with symmetric_yaw_tolerance enabled + GoalAngleCritic critic_symmetric; + critic_symmetric.on_configure(node, "mppi", "critic_sym_enabled", costmap_ros, ¶m_handler); + EXPECT_EQ(critic_symmetric.getName(), "critic_sym_enabled"); + + // Setup state and path: robot at origin, goal at (10, 0) with yaw of PI + state.pose.pose.position.x = 9.7; // Within threshold_to_consider (0.5) of goal at 10.0 + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14159; // Goal at PI + + // Test 1: Trajectory at 0 rad (forward) with symmetric disabled + // Should have high cost since it's not aligned with goal at PI + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + data1.motion_model = std::make_shared(); + critic.score(data1); + float cost_forward_without_symmetric = costs(0); + EXPECT_GT(cost_forward_without_symmetric, 9.4); // Should have significant cost + + // Test 2: Trajectory at PI rad (backward) with symmetric disabled + // Should have zero/low cost since backward matches goal orientation PI + generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + data2.motion_model = std::make_shared(); + critic.score(data2); + float cost_backward_without_symmetric = costs(0); + EXPECT_LT(cost_backward_without_symmetric, 0.0001); // Should be nearly zero + + // Test 3: Trajectory at 0 rad (forward) with symmetric ENABLED + // Should have LOWER cost than Test 1 because with symmetric enabled, + // 0 rad is aligned with goal + PI (backward), which equals goal orientation + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + data3.motion_model = std::make_shared(); + critic_symmetric.score(data3); + float cost_forward_with_symmetric = costs(0); + EXPECT_LT(cost_forward_with_symmetric, 0.0001); // Should be nearly zero + EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); // Should be lower than without symmetric +} + TEST(CriticTests, GoalCritic) { // Standard preamble From 01ee1d234df7f4f697a5c4cf06355279f64cdcec Mon Sep 17 00:00:00 2001 From: Revert Date: Fri, 19 Dec 2025 15:03:15 -0800 Subject: [PATCH 02/10] fixed lint issues Signed-off-by: Revert --- nav2_controller/plugins/simple_goal_checker.cpp | 3 ++- .../src/critics/goal_angle_critic.cpp | 6 ++++-- nav2_mppi_controller/test/critics_tests.cpp | 12 ++++++++---- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 3ba05d1bfae..0050744bb34 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -73,7 +73,8 @@ void SimpleGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); - symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + ".symmetric_yaw_tolerance", false); + symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + + ".symmetric_yaw_tolerance", false); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 747442cb60c..af675827584 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -49,8 +49,10 @@ void GoalAngleCritic::score(CriticData & data) // For symmetric robots: use minimum distance to either goal orientation or goal + 180° const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); - auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs(); - auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw_flipped).abs(); + auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, + goal_yaw).abs(); + auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, + goal_yaw_flipped).abs(); // Use the minimum distance auto min_distance = distance_to_goal.cwiseMin(distance_to_flipped); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 9cb9d8c74ac..f272a757d8c 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -322,7 +322,8 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // Should have high cost since it's not aligned with goal at PI generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); - CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; data1.motion_model = std::make_shared(); critic.score(data1); float cost_forward_without_symmetric = costs(0); @@ -332,7 +333,8 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // Should have zero/low cost since backward matches goal orientation PI generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f); costs = Eigen::ArrayXf::Zero(1000); - CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; data2.motion_model = std::make_shared(); critic.score(data2); float cost_backward_without_symmetric = costs(0); @@ -343,12 +345,14 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // 0 rad is aligned with goal + PI (backward), which equals goal orientation generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); - CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; data3.motion_model = std::make_shared(); critic_symmetric.score(data3); float cost_forward_with_symmetric = costs(0); EXPECT_LT(cost_forward_with_symmetric, 0.0001); // Should be nearly zero - EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); // Should be lower than without symmetric + // Should be lower than without symmetric + EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); } TEST(CriticTests, GoalCritic) From acebc0ab575fcd7f7dbd6cf76601c09f8e60d31c Mon Sep 17 00:00:00 2001 From: Revert Date: Fri, 19 Dec 2025 15:09:35 -0800 Subject: [PATCH 03/10] fixed more lint issues Signed-off-by: Revert --- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 4 ++-- nav2_mppi_controller/test/critics_tests.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index af675827584..f9001b9993d 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -49,9 +49,9 @@ void GoalAngleCritic::score(CriticData & data) // For symmetric robots: use minimum distance to either goal orientation or goal + 180° const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); - auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, + auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs(); - auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, + auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw_flipped).abs(); // Use the minimum distance diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f272a757d8c..75b0a317a5a 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -322,7 +322,7 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // Should have high cost since it's not aligned with goal at PI generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); - CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data1.motion_model = std::make_shared(); critic.score(data1); @@ -333,7 +333,7 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // Should have zero/low cost since backward matches goal orientation PI generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f); costs = Eigen::ArrayXf::Zero(1000); - CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data2.motion_model = std::make_shared(); critic.score(data2); @@ -345,14 +345,14 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) // 0 rad is aligned with goal + PI (backward), which equals goal orientation generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); - CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data3.motion_model = std::make_shared(); critic_symmetric.score(data3); float cost_forward_with_symmetric = costs(0); EXPECT_LT(cost_forward_with_symmetric, 0.0001); // Should be nearly zero // Should be lower than without symmetric - EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); + EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); } TEST(CriticTests, GoalCritic) From 443bf5ce88857dbdfbd6352decb10140f7c61e59 Mon Sep 17 00:00:00 2001 From: Revert Date: Fri, 19 Dec 2025 15:13:23 -0800 Subject: [PATCH 04/10] fixed yet more lint issues Signed-off-by: Revert --- nav2_controller/plugins/simple_goal_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 0050744bb34..857cff9321d 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -73,7 +73,7 @@ void SimpleGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); - symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + + symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + ".symmetric_yaw_tolerance", false); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; From 2e765597cf75b8bf64d82c320b9b3413199c02c0 Mon Sep 17 00:00:00 2001 From: Revert Date: Sun, 21 Dec 2025 10:13:31 -0800 Subject: [PATCH 05/10] fixed code formatting Signed-off-by: Revert --- nav2_mppi_controller/test/critics_tests.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 75b0a317a5a..880788ebb63 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -323,7 +323,7 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, - nullptr, std::nullopt, std::nullopt}; + nullptr, std::nullopt, std::nullopt}; data1.motion_model = std::make_shared(); critic.score(data1); float cost_forward_without_symmetric = costs(0); @@ -334,7 +334,7 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f); costs = Eigen::ArrayXf::Zero(1000); CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, - nullptr, std::nullopt, std::nullopt}; + nullptr, std::nullopt, std::nullopt}; data2.motion_model = std::make_shared(); critic.score(data2); float cost_backward_without_symmetric = costs(0); @@ -346,7 +346,7 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); costs = Eigen::ArrayXf::Zero(1000); CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, - nullptr, std::nullopt, std::nullopt}; + nullptr, std::nullopt, std::nullopt}; data3.motion_model = std::make_shared(); critic_symmetric.score(data3); float cost_forward_with_symmetric = costs(0); From cd941b5d3b1465a10e006be0b17171d99c85ec97 Mon Sep 17 00:00:00 2001 From: Revert Date: Tue, 23 Dec 2025 10:23:21 -0800 Subject: [PATCH 06/10] added test to cover power for goal_angle_critic --- nav2_mppi_controller/test/critics_tests.cpp | 86 +++++++++++++++++++++ 1 file changed, 86 insertions(+) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 880788ebb63..75a760b61f9 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -355,6 +355,92 @@ TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance) EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); } +TEST(CriticTests, GoalAngleCriticSymmetricYawTolerancePow) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap"); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + // Set parameters for critic with symmetric disabled + node->declare_parameter("critic_sym_disabled.cost_power", 2); + node->declare_parameter("critic_sym_disabled.cost_weight", 3.0); + node->declare_parameter("critic_sym_disabled.threshold_to_consider", 0.5); + node->declare_parameter("critic_sym_disabled.symmetric_yaw_tolerance", false); + + // Set parameters for critic with symmetric enabled + node->declare_parameter("critic_sym_enabled.cost_power", 2); + node->declare_parameter("critic_sym_enabled.cost_weight", 3.0); + node->declare_parameter("critic_sym_enabled.threshold_to_consider", 0.5); + node->declare_parameter("critic_sym_enabled.symmetric_yaw_tolerance", true); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + + // Setup goal angle critic with symmetric_yaw_tolerance disabled (default) + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic_sym_disabled", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic_sym_disabled"); + + // Setup goal angle critic with symmetric_yaw_tolerance enabled + GoalAngleCritic critic_symmetric; + critic_symmetric.on_configure(node, "mppi", "critic_sym_enabled", costmap_ros, ¶m_handler); + EXPECT_EQ(critic_symmetric.getName(), "critic_sym_enabled"); + + // Setup state and path: robot at origin, goal at (10, 0) with yaw of PI + state.pose.pose.position.x = 9.7; // Within threshold_to_consider (0.5) of goal at 10.0 + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14159; // Goal at PI + + // Test 1: Trajectory at 0 rad (forward) with symmetric disabled + // Should have high cost since it's not aligned with goal at PI + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; + data1.motion_model = std::make_shared(); + critic.score(data1); + float cost_forward_without_symmetric = costs(0); + EXPECT_GT(cost_forward_without_symmetric, 88.8); // Should have significant cost + + // Test 2: Trajectory at PI rad (backward) with symmetric disabled + // Should have zero/low cost since backward matches goal orientation PI + generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; + data2.motion_model = std::make_shared(); + critic.score(data2); + float cost_backward_without_symmetric = costs(0); + EXPECT_LT(cost_backward_without_symmetric, 0.0001); // Should be nearly zero + + // Test 3: Trajectory at 0 rad (forward) with symmetric ENABLED + // Should have LOWER cost than Test 1 because with symmetric enabled, + // 0 rad is aligned with goal + PI (backward), which equals goal orientation + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30); + costs = Eigen::ArrayXf::Zero(1000); + CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; + data3.motion_model = std::make_shared(); + critic_symmetric.score(data3); + float cost_forward_with_symmetric = costs(0); + EXPECT_LT(cost_forward_with_symmetric, 1e-10); // Should be nearly zero + // Should be lower than without symmetric + EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric); +} + TEST(CriticTests, GoalCritic) { // Standard preamble From a09233e05d6d4f163e6a32431f6facd8613d1df3 Mon Sep 17 00:00:00 2001 From: Revert Date: Wed, 24 Dec 2025 09:22:53 -0800 Subject: [PATCH 07/10] refactored code to check for power_ once --- .../src/critics/goal_angle_critic.cpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index f9001b9993d..5ad747ba67b 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -44,7 +44,8 @@ void GoalAngleCritic::score(CriticData & data) geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); double goal_yaw = tf2::getYaw(goal.orientation); - + auto min_distance = ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()).eval(); if (symmetric_yaw_tolerance_) { // For symmetric robots: use minimum distance to either goal orientation or goal + 180° const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); @@ -55,25 +56,16 @@ void GoalAngleCritic::score(CriticData & data) goal_yaw_flipped).abs(); // Use the minimum distance - auto min_distance = distance_to_goal.cwiseMin(distance_to_flipped); - - if (power_ > 1u) { - data.costs += (min_distance.rowwise().mean() * weight_).pow(power_).eval(); - } else { - data.costs += (min_distance.rowwise().mean() * weight_).eval(); - } + min_distance = distance_to_goal.cwiseMin(distance_to_flipped).rowwise().mean().eval(); + } + if (power_ > 1u) { + data.costs += (min_distance * weight_).pow(power_).eval(); } else { - // Standard behavior: only consider the specified goal orientation - if (power_ > 1u) { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).pow(power_).eval(); - } else { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).eval(); - } + data.costs += (min_distance * weight_).eval(); } } + } // namespace mppi::critics #include From 45d1ed6e6764f8c87d0024bca3ce2de07b4261d8 Mon Sep 17 00:00:00 2001 From: Revert Date: Wed, 24 Dec 2025 09:32:12 -0800 Subject: [PATCH 08/10] fixed lint & uncrustify problems --- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 5ad747ba67b..7f0a825691a 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -45,7 +45,7 @@ void GoalAngleCritic::score(CriticData & data) double goal_yaw = tf2::getYaw(goal.orientation); auto min_distance = ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()).eval(); + rowwise().mean()).eval(); if (symmetric_yaw_tolerance_) { // For symmetric robots: use minimum distance to either goal orientation or goal + 180° const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); @@ -57,7 +57,7 @@ void GoalAngleCritic::score(CriticData & data) // Use the minimum distance min_distance = distance_to_goal.cwiseMin(distance_to_flipped).rowwise().mean().eval(); - } + } if (power_ > 1u) { data.costs += (min_distance * weight_).pow(power_).eval(); } else { From ef0fbac371ae99d38077cc571f37460061afca51 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Wed, 24 Dec 2025 09:22:53 -0800 Subject: [PATCH 09/10] refactored code to check for power_ once Signed-off-by: Sandeep Dutta --- nav2_mppi_controller/README.md | 2 +- nav2_mppi_controller/src/critics/goal_angle_critic.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 2d09ab095ac..6af092b4bc7 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -95,7 +95,7 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | - | symmetric_yaw_tolerance | bool | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty. See [Symmetric Yaw Tolerance Guide](./symmetric_goal_critic_README.md) for details. | + | symmetric_yaw_tolerance | bool | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty.| #### Goal Critic | Parameter | Type | Definition | diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 7f0a825691a..d23679b05ef 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -50,13 +50,11 @@ void GoalAngleCritic::score(CriticData & data) // For symmetric robots: use minimum distance to either goal orientation or goal + 180° const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); - auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, - goal_yaw).abs(); auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw_flipped).abs(); // Use the minimum distance - min_distance = distance_to_goal.cwiseMin(distance_to_flipped).rowwise().mean().eval(); + min_distance = min_distance.cwiseMin(distance_to_flipped).rowwise().mean().eval(); } if (power_ > 1u) { data.costs += (min_distance * weight_).pow(power_).eval(); @@ -65,7 +63,6 @@ void GoalAngleCritic::score(CriticData & data) } } - } // namespace mppi::critics #include From f180de77f2924c4e1b212a40c0cb00bbc2b6a3a2 Mon Sep 17 00:00:00 2001 From: Revert Date: Tue, 30 Dec 2025 09:49:38 -0800 Subject: [PATCH 10/10] formatted simple_goal_checker.cpp Signed-off-by: Revert --- nav2_controller/plugins/simple_goal_checker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 857cff9321d..42841408298 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -73,8 +73,8 @@ void SimpleGoalChecker::initialize( xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25); yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25); stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true); - symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + - ".symmetric_yaw_tolerance", false); + symmetric_yaw_tolerance_ = node->declare_or_get_parameter( + plugin_name + ".symmetric_yaw_tolerance", false); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;