diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index f025075d17b..edcf9a751d6 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -116,7 +116,6 @@ This process is then repeated a number of times and returns a converged solution | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | - #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -125,6 +124,8 @@ This process is then repeated a number of times and returns a converged solution | threshold_to_consider | double | Default 0.4. 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. | + #### Path Follow Critic | Parameter | Type | Definition | @@ -227,6 +228,7 @@ controller_server: offset_from_furthest: 4 threshold_to_consider: 0.40 max_angle_to_furthest: 1.0 + forward_preference: true # TwirlingCritic: # enabled: true # twirling_cost_power: 1 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 92130dceb36..6bf412eb258 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 @@ -48,6 +48,8 @@ class PathAngleCritic : public CriticFunction float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; + bool reversing_allowed_{true}; + bool forward_preference_{true}; 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 7319e318f0d..818d1fe357b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -416,15 +416,25 @@ inline void setPathCostsIfNotSet( * @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 forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { 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 no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + abs(angles::shortest_angular_distance(yaw, pose_yaw)), + abs(angles::shortest_angular_distance(yaw, pose_yaw + M_PI))); + } + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index fc26f89d118..4dc2bcf1deb 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -28,15 +28,14 @@ void ConstraintCritic::initialize() logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); - float vx_max, vy_max, vx_min, vy_min; + float vx_max, vy_max, vx_min; getParentParam(vx_max, "vx_max", 0.5); getParentParam(vy_max, "vy_max", 0.0); getParentParam(vx_min, "vx_min", -0.35); - getParentParam(vy_min, "vy_min", 0.0); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a588bbe2913..1225ccde83f 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.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. @@ -21,6 +22,15 @@ namespace mppi::critics void PathAngleCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + float vx_min; + getParentParam(vx_min, "vx_min", -0.35); + if (fabs(vx_min) < 1e-6) { // zero + reversing_allowed_ = false; + } else if (vx_min < 0.0) { // reversing possible + reversing_allowed_ = true; + } + auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); @@ -31,12 +41,18 @@ 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; + } RCLCPP_INFO( logger_, - "PathAngleCritic instantiated with %d power and %f weight.", - power_, weight_); + "PathAngleCritic instantiated with %d power and %f weight. Reversing %s", + power_, weight_, reversing_allowed_ ? "allowed." : "not allowed."); } void PathAngleCritic::score(CriticData & data) @@ -58,17 +74,27 @@ void PathAngleCritic::score(CriticData & data) 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) < max_angle_to_furthest_) { + if (utils::posePointAngle( + data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_) + { return; } - const auto yaws_between_points = xt::atan2( + auto yaws_between_points = xt::atan2( goal_y - data.trajectories.y, goal_x - data.trajectories.x); - const auto yaws = + + auto yaws = xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + if (reversing_allowed_ && !forward_preference_) { + data.costs += xt::pow( + xt::mean( + xt::where(yaws < M_PI_2, yaws, utils::normalize_angles(yaws + M_PI)), + {1}, immediate) * weight_, power_); + } else { + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + } } } // namespace mppi::critics diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 37acd78e876..924f51aed68 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -195,7 +195,14 @@ TEST(UtilsTests, AnglesTests) pose.position.y = 0.0; pose.orientation.w = 1.0; double point_x = 1.0, point_y = 0.0; - EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); + bool forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = false; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + point_x = -1.0; + 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(UtilsTests, FurthestAndClosestReachedPoint)