updating mppi's path angle critic for optional bidirectionality (#3624)

* updating mppi's path angle critic for optional bidirectionality

* Update README.md

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

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};

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));
 }
 

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)

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

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)