Add std::vector<double> option to the low pass filter

control_toolbox/control_filters.xml

@@ -18,6 +18,13 @@
           This is a low pass filter working with a double value.
         </description>
       </class>
+      <class name="control_filters/LowPassFilterVectorDouble"
+             type="control_filters::LowPassFilter&lt;std::vector&lt;double&gt;&gt;"
+             base_class_type="filters::FilterBase&lt;std::vector&lt;double&gt;&gt;">
+        <description>
+          This is a low pass filter working with a std::vector double value.
+        </description>
+      </class>
       <class name="control_filters/LowPassFilterWrench"
              type="control_filters::LowPassFilter&lt;geometry_msgs::msg::WrenchStamped&gt;"
              base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">

control_toolbox/include/control_filters/low_pass_filter.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "filters/filter_base.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
@@ -153,6 +154,26 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(
   return lpf_->update(data_in, data_out);
 }
 
+template <>
+inline bool LowPassFilter<std::vector<double>>::update(
+  const std::vector<double> & data_in, std::vector<double> & data_out)
+{
+  if (!this->configured_ || !lpf_ || !lpf_->is_configured())
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
+  {
+    parameters_ = parameter_handler_->get_params();
+    lpf_->set_params(
+      parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity);
+  }
+
+  return lpf_->update(data_in, data_out);
+}
+
 template <typename T>
 bool LowPassFilter<T>::update(const T & data_in, T & data_out)
 {

control_toolbox/include/control_toolbox/low_pass_filter.hpp

@@ -18,6 +18,7 @@
 #include <Eigen/Dense>
 
 #include <cmath>
+#include <limits>
 #include <memory>
 #include <stdexcept>
 #include <string>
@@ -122,8 +123,8 @@ class LowPassFilter
   // Filter parameters
   double a1_; /** feedbackward coefficient. */
   double b1_; /** feedforward coefficient. */
-  /** internal data storage (double). */
-  double filtered_value, filtered_old_value, old_value;
+  /** internal data storage of template type. */
+  T filtered_value, filtered_old_value, old_value;
   /** internal data storage (wrench). */
   Eigen::Matrix<double, 6, 1> msg_filtered, msg_filtered_old, msg_old;
   bool configured_ = false;
@@ -143,11 +144,11 @@ template <typename T>
 bool LowPassFilter<T>::configure()
 {
   // Initialize storage Vectors
-  filtered_value = filtered_old_value = old_value = 0;
+  filtered_value = filtered_old_value = old_value = std::numeric_limits<T>::quiet_NaN();
   // TODO(destogl): make the size parameterizable and more intelligent is using complex types
   for (Eigen::Index i = 0; i < 6; ++i)
   {
-    msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0;
+    msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = std::numeric_limits<double>::quiet_NaN();
   }
 
   return configured_ = true;
@@ -161,6 +162,19 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(
   {
     throw std::runtime_error("Filter is not configured");
   }
+  // If this is the first call to update initialize the filter at the current state
+  // so that we dont apply an impulse to the data.
+  if (msg_filtered.hasNaN())
+  {
+    msg_filtered[0] = data_in.wrench.force.x;
+    msg_filtered[1] = data_in.wrench.force.y;
+    msg_filtered[2] = data_in.wrench.force.z;
+    msg_filtered[3] = data_in.wrench.torque.x;
+    msg_filtered[4] = data_in.wrench.torque.y;
+    msg_filtered[5] = data_in.wrench.torque.z;
+    msg_filtered_old = msg_filtered;
+    msg_old = msg_filtered;
+  }
 
   // IIR Filter
   msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old;
@@ -186,18 +200,62 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(
   return true;
 }
 
+template <>
+inline bool LowPassFilter<std::vector<double>>::update(
+  const std::vector<double> & data_in, std::vector<double> & data_out)
+{
+  if (!configured_)
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+  // If this is the first call to update initialize the filter at the current state
+  // so that we dont apply an impulse to the data.
+  // This also sets the size of the member variables to match the input data.
+  if (filtered_value.empty())
+  {
+    filtered_value = data_in;
+    filtered_old_value = data_in;
+    old_value = data_in;
+  }
+  else
+  {
+    assert(
+      data_in.size() == filtered_value.size() &&
+      "Internal data and the data_in doesn't hold the same size");
+    assert(data_out.size() == data_in.size() && "data_in and data_out doesn't hold same size");
+  }
+
+  // Filter each value in the vector
+  for (std::size_t i = 0; i < data_in.size(); i++)
+  {
+    data_out[i] = b1_ * old_value[i] + a1_ * filtered_old_value[i];
+    filtered_old_value[i] = data_out[i];
+    if (std::isfinite(data_in[i])) old_value[i] = data_in[i];
+  }
+
+  return true;
+}
+
 template <typename T>
 bool LowPassFilter<T>::update(const T & data_in, T & data_out)
 {
   if (!configured_)
   {
     throw std::runtime_error("Filter is not configured");
   }
+  // If this is the first call to update initialize the filter at the current state
+  // so that we dont apply an impulse to the data.
+  if (std::isnan(filtered_value))
+  {
+    filtered_value = data_in;
+    filtered_old_value = data_in;
+    old_value = data_in;
+  }
 
   // Filter
   data_out = b1_ * old_value + a1_ * filtered_old_value;
   filtered_old_value = data_out;
-  old_value = data_in;
+  if (std::isfinite(data_in)) old_value = data_in;
 
   return true;
 }

control_toolbox/src/control_filters/low_pass_filter.cpp

@@ -18,6 +18,9 @@
 
 PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter<double>, filters::FilterBase<double>)
 
+PLUGINLIB_EXPORT_CLASS(
+  control_filters::LowPassFilter<std::vector<double>>, filters::FilterBase<std::vector<double>>)
+
 PLUGINLIB_EXPORT_CLASS(
   control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>,
   filters::FilterBase<geometry_msgs::msg::WrenchStamped>)

control_toolbox/test/control_filters/test_load_low_pass_filter.cpp

@@ -43,6 +43,28 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_double)
   rclcpp::shutdown();
 }
 
+TEST(TestLoadLowPassFilter, load_low_pass_filter_vector_double)
+{
+  rclcpp::init(0, nullptr);
+
+  pluginlib::ClassLoader<filters::FilterBase<std::vector<double>>> filter_loader(
+    "filters", "filters::FilterBase<std::vector<double>>");
+  std::shared_ptr<filters::FilterBase<std::vector<double>>> filter;
+  auto available_classes = filter_loader.getDeclaredClasses();
+  std::stringstream sstr;
+  sstr << "available filters:" << std::endl;
+  for (const auto & available_class : available_classes)
+  {
+    sstr << "  " << available_class << std::endl;
+  }
+
+  std::string filter_type = "control_filters/LowPassFilterVectorDouble";
+  ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str();
+  EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type));
+
+  rclcpp::shutdown();
+}
+
 TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench)
 {
   rclcpp::init(0, nullptr);

control_toolbox/test/control_filters/test_low_pass_filter.cpp

@@ -110,23 +110,28 @@ TEST_F(FilterTest, TestLowPassWrenchFilterComputation)
     "", "TestLowPassFilter", node_->get_node_logging_interface(),
     node_->get_node_parameters_interface()));
 
-  // first filter pass, output should be zero as no old wrench was stored
+  // first filter pass, output should be equal to the input
   ASSERT_TRUE(filter_->update(in, out));
-  ASSERT_EQ(out.wrench.force.x, 0.0);
-  ASSERT_EQ(out.wrench.force.y, 0.0);
-  ASSERT_EQ(out.wrench.force.z, 0.0);
-  ASSERT_EQ(out.wrench.torque.x, 0.0);
-  ASSERT_EQ(out.wrench.torque.y, 0.0);
-  ASSERT_EQ(out.wrench.torque.z, 0.0);
-
-  // second filter pass with same values:
+  ASSERT_EQ(out.wrench.force.x, in.wrench.force.x);
+  ASSERT_EQ(out.wrench.force.y, in.wrench.force.y);
+  ASSERT_EQ(out.wrench.force.z, in.wrench.force.z);
+  ASSERT_EQ(out.wrench.torque.x, in.wrench.torque.x);
+  ASSERT_EQ(out.wrench.torque.y, in.wrench.torque.y);
+  ASSERT_EQ(out.wrench.torque.z, in.wrench.torque.z);
+
+  // second filter pass with a step in values (otherwise there is nothing to filter):
+  in.wrench.force.x = 2.0;
+  in.wrench.torque.x = 20.0;
+  ASSERT_TRUE(filter_->update(in, out));
+
+  // update once again and check results
   // calculate wrench by hand
-  calculated.wrench.force.x = b1 * in.wrench.force.x;
-  calculated.wrench.force.y = b1 * in.wrench.force.y;
-  calculated.wrench.force.z = b1 * in.wrench.force.z;
-  calculated.wrench.torque.x = b1 * in.wrench.torque.x;
-  calculated.wrench.torque.y = b1 * in.wrench.torque.y;
-  calculated.wrench.torque.z = b1 * in.wrench.torque.z;
+  calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * out.wrench.force.x;
+  calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * out.wrench.force.y;
+  calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * out.wrench.force.z;
+  calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * out.wrench.torque.x;
+  calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * out.wrench.torque.y;
+  calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * out.wrench.torque.z;
   // check equality with low-pass-filter
   ASSERT_TRUE(filter_->update(in, out));
   ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x);
@@ -135,23 +140,50 @@ TEST_F(FilterTest, TestLowPassWrenchFilterComputation)
   ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x);
   ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y);
   ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z);
+}
+
+TEST_F(FilterTest, TestLowPassVectorDoubleFilterComputation)
+{
+  // parameters should match the test yaml file
+  double sampling_freq = 1000.0;
+  double damping_freq = 20.5;
+  double damping_intensity = 1.25;
+
+  double a1, b1;
+  a1 = exp(
+    -1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0)));
+  b1 = 1.0 - a1;
+
+  std::vector<double> in{1.0, 2.0, 3.0};
+  std::vector<double> calculated, out;
+  calculated.resize(in.size());
+  out.resize(in.size());
+
+  std::shared_ptr<filters::FilterBase<std::vector<double>>> filter_ =
+    std::make_shared<control_filters::LowPassFilter<std::vector<double>>>();
+
+  node_->declare_parameter("sampling_frequency", rclcpp::ParameterValue(sampling_freq));
+  node_->declare_parameter("damping_frequency", rclcpp::ParameterValue(damping_freq));
+  node_->declare_parameter("damping_intensity", rclcpp::ParameterValue(damping_intensity));
+  ASSERT_TRUE(filter_->configure(
+    "", "TestLowPassFilter", node_->get_node_logging_interface(),
+    node_->get_node_parameters_interface()));
+
+  ASSERT_TRUE(filter_->update(in, out));
+  ASSERT_TRUE(std::equal(in.begin(), in.end(), out.begin()));
+
+  // second filter pass with a step in values (otherwise there is nothing to filter):
+  in = {2.0, 4.0, 6.0};
+  ASSERT_TRUE(filter_->update(in, out));
 
   // update once again and check results
   // calculate wrench by hand
-  calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x;
-  calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y;
-  calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z;
-  calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x;
-  calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y;
-  calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z;
+  calculated[0] = b1 * in[0] + a1 * out[0];
+  calculated[1] = b1 * in[1] + a1 * out[1];
+  calculated[2] = b1 * in[2] + a1 * out[2];
   // check equality with low-pass-filter
   ASSERT_TRUE(filter_->update(in, out));
-  ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x);
-  ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y);
-  ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z);
-  ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x);
-  ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y);
-  ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z);
+  ASSERT_TRUE(std::equal(out.begin(), out.end(), calculated.begin()));
 }
 
 int main(int argc, char ** argv)