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/include/nav2_mppi_controller/tools/utils.hpp

@@ -467,6 +467,31 @@ inline float posePointAngle(
   return fabs(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

@@ -43,9 +43,6 @@ void PathAngleCritic::initialize()
   getParam(
     max_angle_to_furthest_,
     "max_angle_to_furthest", 1.2);
-  getParam(
-    forward_preference_,
-    "forward_preference", true);
 
   int mode = 0;
   getParam(mode, "mode", mode);