fix bug in orientation filtering (#4840)

* fix bug in orientation filtering

some global planners output all zeros for orientation, however
the plan is in the global frame. when transforming to the local
frame, the orientation is no longer zero. Instead of comparing
to zero, we simply check if all the orientations in the middle
of the plan are equal

Signed-off-by: Michael Ferguson <mfergs7@gmail.com>

* account for floating point error

Signed-off-by: Michael Ferguson <mfergs7@gmail.com>

---------

Signed-off-by: Michael Ferguson <mfergs7@gmail.com>

Author: mikeferguson

nav2_graceful_controller/src/graceful_controller.cpp

@@ -15,6 +15,7 @@
 #include <memory>
 #include <mutex>
 
+#include "angles/angles.h"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_graceful_controller/graceful_controller.hpp"
@@ -425,12 +426,15 @@ void GracefulController::computeDistanceAlongPath(
 void GracefulController::validateOrientations(
   std::vector<geometry_msgs::msg::PoseStamped> & path)
 {
-  // This really shouldn't happen
-  if (path.empty()) {return;}
+  // We never change the orientation of the first & last pose
+  // So we need at least three poses to do anything here
+  if (path.size() < 3) {return;}
 
   // Check if we actually need to add orientations
-  for (size_t i = 1; i < path.size() - 1; ++i) {
-    if (tf2::getYaw(path[i].pose.orientation) != 0.0) {return;}
+  double initial_yaw = tf2::getYaw(path[1].pose.orientation);
+  for (size_t i = 2; i < path.size() - 1; ++i) {
+    double this_yaw = tf2::getYaw(path[i].pose.orientation);
+    if (angles::shortest_angular_distance(this_yaw, initial_yaw) > 1e-6) {return;}
   }
 
   // For each pose, point at the next one