Enable multiple MPPI path angle modes depending on preferences in beh…

…avior (ros-navigation#3650)

* fixing path angle critic's non-directional bias

* adding reformat

* handle linting

* add utility unit tests

* adding unit tests for path angle

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

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

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

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<PathAngleMode>(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;
+      }
   }
 }
 

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<PathAngleMode>(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, &param_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<float>({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<float>({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<PathAngleMode>(4);
+  EXPECT_EQ(modeToStr(mode), std::string("Invalid mode!"));
 }
 
 TEST(CriticTests, PreferForwardCritic)

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)