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 e9091db9ae8..a6df71b1490 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 2dd6afde449..834df44a2a0 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) { } @@ -78,10 +79,14 @@ void SimpleGoalChecker::initialize( nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".stateful", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".symmetric_yaw_tolerance", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); node->get_parameter(plugin_name + ".stateful", stateful_); + node->get_parameter(plugin_name + ".symmetric_yaw_tolerance", symmetric_yaw_tolerance_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -111,10 +116,23 @@ 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 { + double dyaw = angles::shortest_angular_distance(query_yaw, goal_yaw); + return fabs(dyaw) <= yaw_goal_tolerance_; + } } bool SimpleGoalChecker::getTolerances( @@ -158,6 +176,8 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); + } else if (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 d1d337a5b5f..546355068f3 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -39,6 +39,7 @@ #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "eigen3/Eigen/Geometry" @@ -217,7 +218,8 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), - rclcpp::Parameter("test2.stateful", true)}); + rclcpp::Parameter("test2.stateful", true), + rclcpp::Parameter("test2.symmetric_yaw_tolerance", true)}); rclcpp::spin_until_future_complete( x->get_node_base_interface(), @@ -226,6 +228,7 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); + EXPECT_EQ(x->get_parameter("test2.symmetric_yaw_tolerance").as_bool(), true); // Test the dynamic parameters impacted the tolerances EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); @@ -338,6 +341,24 @@ TEST(StoppedGoalChecker, is_reached) velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25 + M_PI); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + + 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("test2.symmetric_yaw_tolerance", true), + rclcpp::Parameter("test.symmetric_yaw_tolerance", true)}); + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); } int main(int argc, char ** argv) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index d061958135c..bdee6abc1de 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -88,6 +88,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 if either is valid and wish the controller to select the easiest one itself. | #### 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 c8636e8b78c..31885245d95 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" - +#include "angles/angles.h" namespace mppi::critics { @@ -25,12 +25,13 @@ void GoalAngleCritic::initialize() 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) @@ -44,13 +45,23 @@ void GoalAngleCritic::score(CriticData & data) const auto goal_idx = data.path.x.shape(0) - 1; const float goal_yaw = data.path.yaws(goal_idx); + auto angular_distances = + xt::eval(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))); + + if (symmetric_yaw_tolerance_) { + // For symmetric robots: use minimum distance to either goal orientation or goal + 180° + const float symmetric_goal_yaw = angles::normalize_angle(goal_yaw + M_PI); + auto symmetric_distances = + xt::eval(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, + symmetric_goal_yaw))); + angular_distances = xt::eval(xt::minimum(angular_distances, symmetric_distances)); + } + if (power_ > 1u) { data.costs += xt::pow( - xt::mean(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * - weight_, power_); + xt::mean(angular_distances, {1}) * weight_, power_); } else { - data.costs += xt::mean( - xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_; + data.costs += xt::mean(angular_distances, {1}) * weight_; } } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 114755a5220..c6f54cdb351 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -258,6 +258,57 @@ TEST(CriticTests, GoalAngleCritic) EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight } +TEST(CriticTests, GoalAngleSymmetricCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + auto getParam = param_handler.getParamGetter("critic"); + bool symmetric_yaw_tolerance = true; + getParam(symmetric_yaw_tolerance, "symmetric_yaw_tolerance", true); + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path too far from `threshold_to_consider` to consider + state.pose.pose.position.x = 9.7; + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14; + goal.position.x = 10.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_NEAR(costs(0), 0, 0.02); // Should be zero cost due to symmetry + + path.yaws(9) = 0.0; + critic.score(data); + EXPECT_NEAR(costs(0), 0, 0.02); // (0.0 - 0.0) * 3.0 weight +} + TEST(CriticTests, GoalCritic) { // Standard preamble