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..42841408298 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,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); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -101,10 +104,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 +169,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..6af092b4bc7 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.| #### 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..d23679b05ef 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) @@ -41,13 +44,22 @@ 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); + auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws, + goal_yaw_flipped).abs(); + + // Use the minimum distance + min_distance = min_distance.cwiseMin(distance_to_flipped).rowwise().mean().eval(); + } if (power_ > 1u) { - data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).pow(power_).eval(); + data.costs += (min_distance * 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(); } } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 51ad7897fb0..75a760b61f9 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -269,6 +269,178 @@ 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 + // Should be lower than without symmetric + 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