Make member variables of LowPassFilter class generic

[pedroazeredo04]

This PR aims to tackle #345.

In order to do so, I followed the advice from @christophfroehlich, using type_traits to identify which type was passed to the template of the LowPassFilter and configure the StorageType accordingly, all of that in compile time.

Since there is now only one set of three variables instead of two sets, I also corrected all the other occurrences of the variables were removed.

Also, @christophfroehlich said

as an alternative, a traits struct with an initialize() method and operator overloads with a specialization for every non-trivial msg type could work and make the implementation itself cleaner. (no overload of update() would be necessary)

I thought this was a very beautiful idea, but I had some difficulties making this actually happen, because the function update() has some particularities for each data type. At the end, I was basically re-implementing the update() function three times within the traits struct. So I thought it was simpler and more readable to leave the overload the way it is. However, I am open to suggestions!!

One last thing: I had to change the base branch to jazzy, because the ros2-master branch was not compiling in my machine. Would you like for me to target the branch ros2-master instead of jazzy? EDIT: The bot told me to do it, so I did it.

Please let me know anything that would be nice to change

[mergify]

@pedroazeredo04, all pull requests must be targeted towards the ros2-master development branch.
Once merged into ros2-master, it is possible to backport to jazzy, but it must be in ros2-master
to have these changes reflected into new distributions.

[mergify]

This pull request is in conflict. Could you fix it @pedroazeredo04?

[saikishor]

@pedroazeredo04 can you fix the pre-commit?. Just install pre-commit and run pre-commit run --all-files

[saikishor]

Suggested change

	std::is_same<T, geometry_msgs::msg::WrenchStamped>::value, Eigen::Matrix<double, 6, 1>,
	std::is_same<T, geometry_msgs::msg::WrenchStamped>::value, Eigen::Vector6d,

May be we could use this directly

[christophfroehlich]

This might come from
https://github.com/ros/geometry/blob/noetic-devel/eigen_conversions/include/eigen_conversions/eigen_msg.h#L100
according to the comment
// TODO(destogl): use wrenchMsgToEigen
But I don't find these conversion wrappers for ROS 2, https://docs.ros.org/en/rolling/p/tf2_eigen/generated/index.html#tf2-eigen

[christophfroehlich]

Thanks a lot for your contribution.

I thought this was a very beautiful idea, but I had some difficulties making this actually happen, because the function update() has some particularities for each data type. At the end, I was basically re-implementing the update() function three times within the traits struct. So I thought it was simpler and more readable to leave the overload the way it is. However, I am open to suggestions!!

You are right about that, I made a proposal in the issue: what do you think? I still think that the algorithms in the filters would be more clean, and we easily could apply that to all the other filters and add for example Pose messages (in a future PR).

[pedroazeredo04]

You are right about that, I made a proposal in the issue: what do you think? I still think that the algorithms in the filters would be more clean, and we easily could apply that to all the other filters and add for example Pose messages (in a future PR).

@christophfroehlich That sounds nice! I didn't know we could use this logic in other filters. I will leave this implementation for a future PR, but in which filters could we also use this? Also, do you think I should create a new file (for instance include/control_toolbox/filter_traits.hpp) for the FilterTraits struct?

[christophfroehlich]

@christophfroehlich That sounds nice! I didn't know we could use this logic in other filters. I will leave this implementation for a future PR, but in which filters could we also use this?

for now, the exponential filter would be the only other candidate. But more might come in the future.

Also, do you think I should create a new file (for instance include/control_toolbox/filter_traits.hpp) for the FilterTraits struct?

yes, exactly.

[pedroazeredo04]

@christophfroehlich I commited your suggestion of having a FilterTrait struct! But I still couldn't quite make the function LowPassFilter::update() have an unique implementation for the three distinct data types :(

That is because the override of this function for the type std::vector<double> does some additional verifications that the other data types doesn't. For instance, this block

if (filtered_value_.empty())
  {
    if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); }))
    {
      return false;
    }
    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");
  }

is very distinct from the other overrides, so it is hard to generalize this for every data type.

---

Also, about the other methods of the TypeTraits struct, I could add the isnan(), isempty() and copy_metadata() if you like, what do you think?

Something like this, plus other necessary member functions like isnan, isempty, copy_metadata (for copying the header if it is a message type) or operator overloads for +operator, *operator, which can go in its own header file:

I would only have to say , in order to do an operator overload for +operator and *operator for every data type, I think we would have to create a wrapper struct for each non-primitive type, because we probably shouldn't overload the +operator of std::vector<double>, for example, as it is undefined behavior. What I mean is that instead of

  // undefined behavior
  template<typename T>
  std::vector<T> operator+(const std::vector<T>& lhs, const std::vector<T>& rhs) {
    std::vector<T> result(lhs.size());
    for (size_t i = 0; i < lhs.size(); ++i) {
      result[i] = lhs[i] + rhs[i];
    }
    return result;
  }

We would probably have to do

// Wrapper around std::vector<double> to enable element-wise + and * without touching std::
struct VecD {
  std::vector<double> v;
  VecD() = default;
  VecD(const std::vector<double>& data) : v(data) {}
  bool empty() const { return v.empty(); }
  void resize(size_t n) { v.resize(n); }
  double& operator[](size_t i) { return v[i]; }
  const double& operator[](size_t i) const { return v[i]; }
};

// element-wise add
inline VecD operator+(const VecD& a, const VecD& b) {
  VecD out(a.v.size());
  for (size_t i = 0; i < a.v.size(); ++i)
    out.v[i] = a.v[i] + b.v[i];
  return out;
}

[christophfroehlich]

Thanks for your work on this. My suggestion was only a draft, I'm not really experienced in writing templated classes, so I might be running in a dead end. Please shout out if this is the case ;)

@christophfroehlich I commited your suggestion of having a FilterTrait struct! But I still couldn't quite make the function LowPassFilter::update() have an unique implementation for the three distinct data types :(

That is because the override of this function for the type std::vector<double> does some additional verifications that the other data types doesn't. For instance, this block

if (filtered_value_.empty())
  {
    if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); }))
    {
      return false;
    }
    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");
  }

is very distinct from the other overrides, so it is hard to generalize this for every data type.

The if branch could be implemented with the already mentionend isempty(), isnan(), and the else branch could be solved by a if constexpr on std:.vector?

// helper: default false
template<typename T>
struct is_std_vector : std::false_type {};

// specialization for std::vector
template<typename U, typename Alloc>
struct is_std_vector<std::vector<U, Alloc>> : std::true_type {};

and use if constexpr (is_std_vector<T>::value) {}

Also, about the other methods of the TypeTraits struct, I could add the isnan(), isempty() and copy_metadata() if you like, what do you think?

Something like this, plus other necessary member functions like isnan, isempty, copy_metadata (for copying the header if it is a message type) or operator overloads for +operator, *operator, which can go in its own header file:

I would only have to say , in order to do an operator overload for +operator and *operator for every data type, I think we would have to create a wrapper struct for each non-primitive type, because we probably shouldn't overload the +operator of std::vector<double>, for example, as it is undefined behavior. What I mean is that instead of

This is a very good hint (haven't thought about that), but we aren't overloading this to std::vector<double>, but to struct FilterTraits<std::vector<T>>? Isn't this a difference w.r.t to your concern?

[pedroazeredo04]

Hi @christophfroehlich ! No need to thank my work on this, I'm having fun messing around with templates :)

I pushed a whip code that tries to make the update() function trully generic.

The if branch could be implemented with the already mentionend isempty(), isnan(), and the else branch could be solved by a if constexpr on std:.vector?

// helper: default false
template<typename T>
struct is_std_vector : std::false_type {};

// specialization for std::vector
template<typename U, typename Alloc>
struct is_std_vector<std::vector<U, Alloc>> : std::true_type {};

and use if constexpr (is_std_vector<T>::value) {}

That is a good idea! I did something similar, but checking for the types inside of the function itself.

This is a very good hint (haven't thought about that), but we aren't overloading this to std::vector, but to struct FilterTraits<std::vector>? Isn't this a difference w.r.t to your concern?

In my understanding, the problem is that FilterTraits<std::vector<T>> isn't the type of the variables filtered_value_, old_value_ and filtered_old_value_. Those variables are std::vector<double>. So, in order for the block

  filtered_value_ = old_value_ * b1_ + filtered_old_value_ * a1_;
  filtered_old_value_ = filtered_value_;

work for every possible datatype, I had to add in a FilterVector wrapper around the std::vector. So, for the std::vector<double> overload, the StorageType is actually of type FilterVector, and not std::vector<double>. At the end I though it turned out not so ugly ahahaha, but I'm of course open to suggestions. If you could review it and give your feedback, I would appreciate it!

[christophfroehlich]

I like the direction of the simplification of the filter itself. The filter_traits look a bit verbose, but the API is clean and understandable IMHO.

The tests are failing now, apart from that only some minor comments from my side are left.

[christophfroehlich]

Suggested change

	// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
	// Copyright (c) 2025, ros2_control development team

[christophfroehlich]

or you can just add your affiliation

[christophfroehlich]

my suggestion of is_std_vector would have been working with any std::vector<T>, but not sure if any other datatype than double will ever make sense here 😄

[christophfroehlich]

this could be generalized with something like add_metadata, because we'll need this for any message type then?

[christophfroehlich]

i'd prefer to use the is_finite instead of is_infinite, as we are used to this from std

[christophfroehlich]

should we use?

Suggested change

	using StorageType = Eigen::Matrix<double, 6, 1>;
	using StorageType = Vector6d;