diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index bbc1df4975..276e24b058 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -120,7 +120,7 @@ This process is then repeated a number of times and returns a converged solution | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | - | forward_preference | bool | Default true. Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains *no* particular preference one way or another. | + | mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. | #### Path Follow Critic 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 6bf412eb25..f7ba486a0c 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 @@ -15,13 +15,41 @@ #ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_ #define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_ +#include #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace mppi::critics { +/** + * @brief Enum type for different modes of operation + */ +enum class PathAngleMode +{ + FORWARD_PREFERENCE = 0, + NO_DIRECTIONAL_PREFERENCE = 1, + CONSIDER_FEASIBLE_PATH_ORIENTATIONS = 2 +}; + +/** + * @brief Method to convert mode enum to string for printing + */ +std::string modeToStr(const PathAngleMode & mode) +{ + if (mode == PathAngleMode::FORWARD_PREFERENCE) { + return "Forward Preference"; + } else if (mode == PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS) { + return "Consider Feasible Path Orientations"; + } else if (mode == PathAngleMode::NO_DIRECTIONAL_PREFERENCE) { + return "No Directional Preference"; + } else { + return "Invalid mode!"; + } +} + /** * @class mppi::critics::ConstraintCritic * @brief Critic objective function for aligning to path in cases of extreme misalignment @@ -49,7 +77,7 @@ class PathAngleCritic : public CriticFunction size_t offset_from_furthest_{0}; bool reversing_allowed_{true}; - bool forward_preference_{true}; + PathAngleMode mode_{0}; unsigned int power_{0}; float weight_{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 4d2295b626..77b801bf3e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -438,6 +438,31 @@ inline double posePointAngle( return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + double pose_x = pose.position.x; + double pose_y = pose.position.y; + double pose_yaw = tf2::getYaw(pose.orientation); + + double yaw = atan2(point_y - pose_y, point_x - pose_x); + + if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { + yaw = angles::normalize_angle(yaw + M_PI); + } + + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + /** * @brief Apply Savisky-Golay filter to optimal trajectory * @param control_sequence Sequence to apply filter to diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index f09554bf18..cd701ea093 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -20,6 +20,8 @@ namespace mppi::critics { +using xt::evaluation_strategy::immediate; + void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); @@ -41,43 +43,59 @@ void PathAngleCritic::initialize() getParam( max_angle_to_furthest_, "max_angle_to_furthest", 1.2); - getParam( - forward_preference_, - "forward_preference", true); - if (!reversing_allowed_) { - forward_preference_ = true; + int mode = 0; + getParam(mode, "mode", mode); + mode_ = static_cast(mode); + if (!reversing_allowed_ && mode_ == PathAngleMode::NO_DIRECTIONAL_PREFERENCE) { + mode_ = PathAngleMode::FORWARD_PREFERENCE; + RCLCPP_WARN( + logger_, + "Path angle mode set to no directional preference, but controller's settings " + "don't allow for reversing! Setting mode to forward preference."); } RCLCPP_INFO( logger_, - "PathAngleCritic instantiated with %d power and %f weight. Reversing %s", - power_, weight_, reversing_allowed_ ? "allowed." : "not allowed."); + "PathAngleCritic instantiated with %d power and %f weight. Mode set to: %s", + power_, weight_, modeToStr(mode_).c_str()); } void PathAngleCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } utils::setPathFurthestPointIfNotSet(data); - auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); const float goal_x = xt::view(data.path.x, offseted_idx); const float goal_y = xt::view(data.path.y, offseted_idx); - - if (utils::posePointAngle( - data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_) - { - return; + const float goal_yaw = xt::view(data.path.yaws, offseted_idx); + const geometry_msgs::msg::Pose & pose = data.state.pose.pose; + + switch (mode_) { + case PathAngleMode::FORWARD_PREFERENCE: + if (utils::posePointAngle(pose, goal_x, goal_y, true) < max_angle_to_furthest_) { + return; + } + break; + case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: + if (utils::posePointAngle(pose, goal_x, goal_y, false) < max_angle_to_furthest_) { + return; + } + break; + case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: + if (utils::posePointAngle(pose, goal_x, goal_y, goal_yaw) < max_angle_to_furthest_) { + return; + } + break; + default: + throw nav2_core::ControllerException("Invalid path angle mode!"); } auto yaws_between_points = xt::atan2( @@ -87,14 +105,31 @@ void PathAngleCritic::score(CriticData & data) auto yaws = xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); - if (reversing_allowed_ && !forward_preference_) { - const auto yaws_between_points_corrected = xt::where( - yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); - const auto corrected_yaws = xt::abs( - utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); - data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); - } else { - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + switch (mode_) { + case PathAngleMode::FORWARD_PREFERENCE: + { + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + return; + } + case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: + { + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + return; + } + case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: + { + const auto yaws_between_points_corrected = xt::where( + xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PI_2, + yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + return; + } } } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 6ba6cc95d1..ce2896122c 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -39,6 +40,20 @@ using namespace mppi::critics; // NOLINT using namespace mppi::utils; // NOLINT using xt::evaluation_strategy::immediate; +class PathAngleCriticWrapper : public PathAngleCritic +{ +public: + PathAngleCriticWrapper() + : PathAngleCritic() + { + } + + void setMode(int mode) + { + mode_ = static_cast(mode); + } +}; + TEST(CriticTests, ConstraintsCritic) { // Standard preamble @@ -239,7 +254,7 @@ TEST(CriticTests, PathAngleCritic) // Initialization testing // Make sure initializes correctly - PathAngleCritic critic; + PathAngleCriticWrapper critic; critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); EXPECT_EQ(critic.getName(), "critic"); @@ -267,6 +282,64 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight + + // Set mode to no directional preferences + reset costs + critic.setMode(1); + costs = xt::zeros({1000}); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.6516, 1e-2); + + // Set mode to consider path directionality + reset costs + critic.setMode(2); + costs = xt::zeros({1000}); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.6516, 1e-2); + + PathAngleMode mode; + mode = PathAngleMode::FORWARD_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("Forward Preference")); + mode = PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS; + EXPECT_EQ(modeToStr(mode), std::string("Consider Feasible Path Orientations")); + mode = PathAngleMode::NO_DIRECTIONAL_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("No Directional Preference")); + mode = static_cast(4); + EXPECT_EQ(modeToStr(mode), std::string("Invalid mode!")); } TEST(CriticTests, PreferForwardCritic) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 5afe1c1d88..4a77578fb0 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -203,6 +203,17 @@ TEST(UtilsTests, AnglesTests) EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); forward_preference = true; EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6); + + // Test point-pose angle with goal yaws + point_x = 1.0; + double point_yaw = 0.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-6); + point_yaw = M_PI; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-6); + point_yaw = 0.1; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-3); + point_yaw = 3.04159; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-3); } TEST(UtilsTests, FurthestAndClosestReachedPoint)