fix(filt_vector): fix a problem of list index out of range in filt_ve…

…ctor (autowarefoundation#5099) fix filt_vector Signed-off-by: tohmae <[email protected]>
Interplai · Jun 3, 2024 · 4fff3a6 · 4fff3a6
1 parent 2ce55e0
commit 4fff3a6
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 4 deletions.
diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp
@@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector<double> & u)
     double tmp = 0.0;
     int num_tmp = 0;
     double count = 0;
-    if (i - num < 0) {
-      num_tmp = i;
-    } else if (i + num > static_cast<int>(u.size()) - 1) {
-      num_tmp = static_cast<int>(u.size()) - i - 1;
+    if ((i - num < 0) || (i + num > static_cast<int>(u.size()) - 1)) {
+      num_tmp = std::min(i, static_cast<int>(u.size()) - i - 1);
     } else {
       num_tmp = num;
     }

diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp
@@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter)
     EXPECT_EQ(filtered_vec[4], 23.0 / 3);
     EXPECT_EQ(filtered_vec[5], original_vec[5]);
   }
+  {
+    const int window_size = 3;
+    const std::vector<double> original_vec = {1.0, 1.0, 1.0, 1.0};
+    std::vector<double> filtered_vec = original_vec;
+    EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec));
+    ASSERT_EQ(filtered_vec.size(), original_vec.size());
+    EXPECT_EQ(filtered_vec[0], original_vec[0]);
+    EXPECT_EQ(filtered_vec[1], 1.0);
+    EXPECT_EQ(filtered_vec[2], 1.0);
+    EXPECT_EQ(filtered_vec[3], original_vec[3]);
+  }
+  {
+    const int window_size = 4;
+    const std::vector<double> original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0};
+    std::vector<double> filtered_vec = original_vec;
+    EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec));
+    ASSERT_EQ(filtered_vec.size(), original_vec.size());
+    EXPECT_EQ(filtered_vec[0], original_vec[0]);
+    EXPECT_EQ(filtered_vec[1], 8.0 / 3);
+    EXPECT_EQ(filtered_vec[2], 21.0 / 5);
+    EXPECT_EQ(filtered_vec[3], 30.0 / 5);
+    EXPECT_EQ(filtered_vec[4], 23.0 / 3);
+    EXPECT_EQ(filtered_vec[5], original_vec[5]);
+  }
 }
 TEST(TestLowpassFilter, Butterworth2dFilter)
 {