feat(signal_processing): add lpf for twist

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/signal_processing/CMakeLists.txt

@@ -6,11 +6,13 @@ autoware_package()
 
 ament_auto_add_library(lowpass_filter_1d SHARED
   src/lowpass_filter_1d.cpp
+  src/lowpass_filter.cpp
 )
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_signal_processing
     test/src/lowpass_filter_1d_test.cpp
+    test/src/lowpass_filter_test.cpp
   )
   target_link_libraries(test_signal_processing
     lowpass_filter_1d

common/signal_processing/include/signal_processing/lowpass_filter.hpp

@@ -0,0 +1,55 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_
+#define SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_
+
+#include "signal_processing/lowpass_filter_1d.hpp"
+
+#include "geometry_msgs/msg/twist.hpp"
+
+/**
+ * @class First-order low-pass filter
+ * @brief filtering values
+ */
+template <typename T>
+class LowpassFilterInterface
+{
+protected:
+  boost::optional<T> x_;  //!< @brief current filtered value
+  double gain_;           //!< @brief gain value of first-order low-pass filter
+
+public:
+  explicit LowpassFilterInterface(const double gain) : gain_(gain) {}
+
+  void reset() { x_ = T{}; }
+  void reset(const T & x) { x_ = x; }
+
+  boost::optional<T> getValue() const { return x_; }
+
+  virtual T filter(const T & u) = 0;
+};
+
+class LowpassFilterTwist : public LowpassFilterInterface<geometry_msgs::msg::Twist>
+{
+public:
+  explicit LowpassFilterTwist(const double gain)
+  : LowpassFilterInterface<geometry_msgs::msg::Twist>(gain)
+  {
+  }
+
+  geometry_msgs::msg::Twist filter(const geometry_msgs::msg::Twist & u) override;
+};
+
+#endif  // SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_

common/signal_processing/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>geometry_msgs</depend>
+
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

common/signal_processing/src/lowpass_filter.cpp

@@ -0,0 +1,33 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "signal_processing/lowpass_filter.hpp"
+
+geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::Twist & u)
+{
+  if (x_) {
+    x_->linear.x = gain_ * x_->linear.x + (1.0 - gain_) * u.linear.x;
+    x_->linear.y = gain_ * x_->linear.y + (1.0 - gain_) * u.linear.y;
+    x_->linear.z = gain_ * x_->linear.z + (1.0 - gain_) * u.linear.z;
+
+    x_->angular.x = gain_ * x_->angular.x + (1.0 - gain_) * u.angular.x;
+    x_->angular.y = gain_ * x_->angular.y + (1.0 - gain_) * u.angular.y;
+    x_->angular.z = gain_ * x_->angular.z + (1.0 - gain_) * u.angular.z;
+
+    return x_.get();
+  }
+
+  x_ = u;
+  return x_.get();
+}

common/signal_processing/test/src/lowpass_filter_1d_test.cpp

@@ -41,9 +41,3 @@ TEST(lowpass_filter_1d, filter)
   EXPECT_NEAR(lowpass_filter_1d.filter(0.0), -0.11, epsilon);
   EXPECT_NEAR(*lowpass_filter_1d.getValue(), -0.11, epsilon);
 }
-
-int main(int argc, char * argv[])
-{
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

common/signal_processing/test/src/lowpass_filter_test.cpp

@@ -0,0 +1,88 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "signal_processing/lowpass_filter.hpp"
+
+#include <gtest/gtest.h>
+
+constexpr double epsilon = 1e-6;
+
+geometry_msgs::msg::Twist createTwist(
+  const double lx, const double ly, const double lz, const double ax, const double ay,
+  const double az)
+{
+  geometry_msgs::msg::Twist twist;
+
+  twist.linear.x = lx;
+  twist.linear.y = ly;
+  twist.linear.z = lz;
+  twist.angular.x = ax;
+  twist.angular.y = ay;
+  twist.angular.z = az;
+
+  return twist;
+}
+
+TEST(lowpass_filter_twist, filter)
+{
+  LowpassFilterTwist lowpass_filter_(0.1);
+
+  {  // initial state
+    const auto filtered_twist = lowpass_filter_.getValue();
+    EXPECT_NEAR(filtered_twist->linear.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.y, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.z, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.y, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.z, 0.0, epsilon);
+  }
+
+  {  // random filter
+    geometry_msgs::msg::Twist twist = createTwist(0.0, 0.1, 0.2, 0.3, 0.4, 0.5);
+
+    const auto filtered_twist = lowpass_filter_.filter(twist);
+    EXPECT_NEAR(filtered_twist.linear.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist.linear.y, 0.1, epsilon);
+    EXPECT_NEAR(filtered_twist.linear.z, 0.2, epsilon);
+    EXPECT_NEAR(filtered_twist.angular.x, 0.3, epsilon);
+    EXPECT_NEAR(filtered_twist.angular.y, 0.4, epsilon);
+    EXPECT_NEAR(filtered_twist.angular.z, 0.5, epsilon);
+  }
+
+  {  // reset without value
+    lowpass_filter_.reset();
+
+    const auto filtered_twist = lowpass_filter_.getValue();
+    EXPECT_NEAR(filtered_twist->linear.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.y, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.z, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.y, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.z, 0.0, epsilon);
+  }
+
+  {  // reset with value
+    geometry_msgs::msg::Twist twist = createTwist(0.0, 0.1, 0.2, 0.3, 0.4, 0.5);
+
+    lowpass_filter_.reset(twist);
+
+    const auto filtered_twist = lowpass_filter_.getValue();
+    EXPECT_NEAR(filtered_twist->linear.x, 0.0, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.y, 0.1, epsilon);
+    EXPECT_NEAR(filtered_twist->linear.z, 0.2, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.x, 0.3, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.y, 0.4, epsilon);
+    EXPECT_NEAR(filtered_twist->angular.z, 0.5, epsilon);
+  }
+}