Fixing SGF in MPPI and Smoother (ros-navigation#4669)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Author: SteveMacenski

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -461,9 +461,8 @@ inline void savitskyGolayFilter(
   xt::xarray<float> filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0};
   filter /= 231.0;
 
-  const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
-
   // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
   if (num_sequences < 20) {
     return;
   }
@@ -473,137 +472,49 @@ inline void savitskyGolayFilter(
     };
 
   auto applyFilterOverAxis =
-    [&](xt::xtensor<float, 1> & sequence,
+    [&](xt::xtensor<float, 1> & sequence, const xt::xtensor<float, 1> & initial_sequence,
     const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
     {
-      unsigned int idx = 0;
-      sequence(idx) = applyFilter(
-      {
-        hist_0,
-        hist_1,
-        hist_2,
-        hist_3,
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 3),
-        sequence(idx + 4)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        hist_1,
-        hist_2,
-        hist_3,
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 3),
-        sequence(idx + 4)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        hist_2,
-        hist_3,
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 3),
-        sequence(idx + 4)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        hist_3,
-        sequence(idx - 3),
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 3),
-        sequence(idx + 4)});
-
-      for (idx = 4; idx != num_sequences - 4; idx++) {
-        sequence(idx) = applyFilter(
-        {
-          sequence(idx - 4),
-          sequence(idx - 3),
-          sequence(idx - 2),
-          sequence(idx - 1),
-          sequence(idx),
-          sequence(idx + 1),
-          sequence(idx + 2),
-          sequence(idx + 3),
-          sequence(idx + 4)});
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
       }
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        sequence(idx - 4),
-        sequence(idx - 3),
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 3),
-        sequence(idx + 3)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        sequence(idx - 4),
-        sequence(idx - 3),
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 2),
-        sequence(idx + 2),
-        sequence(idx + 2)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        sequence(idx - 4),
-        sequence(idx - 3),
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx + 1),
-        sequence(idx + 1),
-        sequence(idx + 1),
-        sequence(idx + 1)});
-
-      idx++;
-      sequence(idx) = applyFilter(
-      {
-        sequence(idx - 4),
-        sequence(idx - 3),
-        sequence(idx - 2),
-        sequence(idx - 1),
-        sequence(idx),
-        sequence(idx),
-        sequence(idx),
-        sequence(idx),
-        sequence(idx)});
     };
 
   // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
   applyFilterOverAxis(
-    control_sequence.vx, control_history[0].vx,
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
     control_history[1].vx, control_history[2].vx, control_history[3].vx);
   applyFilterOverAxis(
-    control_sequence.vy, control_history[0].vy,
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
     control_history[1].vy, control_history[2].vy, control_history[3].vy);
   applyFilterOverAxis(
-    control_sequence.wz, control_history[0].wz,
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
     control_history[1].wz, control_history[2].wz, control_history[3].wz);
 
   // Update control history

nav2_smoother/src/savitzky_golay_smoother.cpp

@@ -118,74 +118,44 @@ bool SavitzkyGolaySmoother::smoothImpl(
     };
 
   auto applyFilterOverAxes =
-    [&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts) -> void
+    [&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts,
+    const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
     {
-      // Handle initial boundary conditions, first point is fixed
-      unsigned int idx = 1;
-      plan_pts[idx].pose.position = applyFilter(
-      {
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx].pose.position,
-        plan_pts[idx + 1].pose.position,
-        plan_pts[idx + 2].pose.position,
-        plan_pts[idx + 3].pose.position});
-
-      idx++;
-      plan_pts[idx].pose.position = applyFilter(
-      {
-        plan_pts[idx - 2].pose.position,
-        plan_pts[idx - 2].pose.position,
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx].pose.position,
-        plan_pts[idx + 1].pose.position,
-        plan_pts[idx + 2].pose.position,
-        plan_pts[idx + 3].pose.position});
-
-      // Apply nominal filter
-      for (idx = 3; idx < path_size - 4; ++idx) {
-        plan_pts[idx].pose.position = applyFilter(
-        {
-          plan_pts[idx - 3].pose.position,
-          plan_pts[idx - 2].pose.position,
-          plan_pts[idx - 1].pose.position,
-          plan_pts[idx].pose.position,
-          plan_pts[idx + 1].pose.position,
-          plan_pts[idx + 2].pose.position,
-          plan_pts[idx + 3].pose.position});
+      auto pt_m3 = init_plan_pts[0].pose.position;
+      auto pt_m2 = init_plan_pts[0].pose.position;
+      auto pt_m1 = init_plan_pts[0].pose.position;
+      auto pt = init_plan_pts[1].pose.position;
+      auto pt_p1 = init_plan_pts[2].pose.position;
+      auto pt_p2 = init_plan_pts[3].pose.position;
+      auto pt_p3 = init_plan_pts[4].pose.position;
+
+      // First point is fixed
+      for (unsigned int idx = 1; idx != path_size - 1; idx++) {
+        plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3});
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+
+        if (idx + 4 < path_size - 1) {
+          pt_p3 = init_plan_pts[idx + 4].pose.position;
+        } else {
+          // Return the last point
+          pt_p3 = init_plan_pts[path_size - 1].pose.position;
+        }
       }
-
-      // Handle terminal boundary conditions, last point is fixed
-      idx++;
-      plan_pts[idx].pose.position = applyFilter(
-      {
-        plan_pts[idx - 3].pose.position,
-        plan_pts[idx - 2].pose.position,
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx].pose.position,
-        plan_pts[idx + 1].pose.position,
-        plan_pts[idx + 2].pose.position,
-        plan_pts[idx + 2].pose.position});
-
-      idx++;
-      plan_pts[idx].pose.position = applyFilter(
-      {
-        plan_pts[idx - 3].pose.position,
-        plan_pts[idx - 2].pose.position,
-        plan_pts[idx - 1].pose.position,
-        plan_pts[idx].pose.position,
-        plan_pts[idx + 1].pose.position,
-        plan_pts[idx + 1].pose.position,
-        plan_pts[idx + 1].pose.position});
     };
 
-  applyFilterOverAxes(path.poses);
+  const auto initial_path_poses = path.poses;
+  applyFilterOverAxes(path.poses, initial_path_poses);
 
   // Lets do additional refinement, it shouldn't take more than a couple milliseconds
   if (do_refinement_) {
     for (int i = 0; i < refinement_num_; i++) {
-      applyFilterOverAxes(path.poses);
+      const auto reined_initial_path_poses = path.poses;
+      applyFilterOverAxes(path.poses, reined_initial_path_poses);
     }
   }