Make member variables of LowPassFilter class generic #351

pedroazeredo04 · 2025-04-27T01:16:59Z

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 · 2025-04-27T01:17:35Z

@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.

Signed-off-by: Pedro Nogueira <[email protected]>

mergify · 2025-04-27T01:26:36Z

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 · 2025-04-27T06:39:01Z

control_toolbox/include/control_toolbox/low_pass_filter.hpp

+
+  // Define the storage type based on T
+  using StorageType = typename std::conditional<
+    std::is_same<T, geometry_msgs::msg::WrenchStamped>::value, Eigen::Matrix<double, 6, 1>,


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

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

control_toolbox/include/control_toolbox/low_pass_filter.hpp

christophfroehlich · 2025-04-27T07:32:57Z

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 · 2025-04-27T08:27:28Z

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?

Signed-off-by: Pedro Nogueira <[email protected]>

christophfroehlich · 2025-04-27T09:28:22Z

@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.

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 · 2025-04-27T14:44:19Z

@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 · 2025-04-27T19:01:41Z

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?

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 · 2025-04-28T16:02:13Z

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!

Signed-off-by: Pedro Nogueira <[email protected]>

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.

control_toolbox/include/control_toolbox/filter_traits.hpp

christophfroehlich · 2025-04-28T19:48:49Z

control_toolbox/include/control_toolbox/low_pass_filter.hpp

-    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]))
+    if constexpr (std::is_same_v<T, std::vector<double>>)


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 😄

Do you think I should change that to generalize the type of the vector? If I understood correctly, I believe the previous code only supported std::vector<double also

have you tried my code snippet above? I thought this could work.
we could also make another method for "validate_input" or similar? could be useful for other message types also, like checking if the frame_id the same or whatever.

control_toolbox/include/control_toolbox/low_pass_filter.hpp

control_toolbox/include/control_toolbox/filter_traits.hpp

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 · 2025-04-30T07:24:31Z

Thanks for the review @christophfroehlich !!

I commited some the small fixes that you suggested, and I will try to fix the failing tests later.

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 · 2025-05-02T12:36:25Z

Hi @christophfroehlich. I had some time today and I added the generic method Traits::validate_input to the FilterTraits struct, as you suggested. I also found a bug in the Eigen::Matrix initialization, and fixed that too. Hopefully, with those changes, all tests will pass😄. If you could take a look at the code and give me your feedback when you have a chance, that would be nice!

codecov-commenter · 2025-05-02T13:00:01Z

Codecov Report

Attention: Patch coverage is 81.66667% with 11 lines in your changes missing coverage. Please review.

Project coverage is 78.17%. Comparing base (28cc66f) to head (4a7609a).
Report is 3 commits behind head on ros2-master.

Files with missing lines	Patch %	Lines
..._toolbox/include/control_toolbox/filter_traits.hpp	80.70%	8 Missing and 3 partials ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           ros2-master     #351      +/-   ##
===============================================
+ Coverage        77.24%   78.17%   +0.93%     
===============================================
  Files               29       30       +1     
  Lines             1336     1338       +2     
  Branches            93       89       -4     
===============================================
+ Hits              1032     1046      +14     
+ Misses             252      245       -7     
+ Partials            52       47       -5

Flag	Coverage Δ
unittests	`78.17% <81.66%> (+0.93%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...oolbox/include/control_toolbox/low_pass_filter.hpp	`88.88% <100.00%> (+23.13%)`	⬆️
..._toolbox/include/control_toolbox/filter_traits.hpp	`80.70% <80.70%> (ø)`

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

christophfroehlich

LGTM, 2 minor comments

christophfroehlich · 2025-05-02T21:51:54Z

control_toolbox/include/control_toolbox/filter_traits.hpp

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


is this used now?

christophfroehlich · 2025-05-02T21:55:12Z

control_toolbox/include/control_toolbox/filter_traits.hpp

+};
+
+template <>
+struct FilterTraits<std::vector<double>>


can this be generalized for all types inside a std::vector?

Signed-off-by: Pedro Nogueira <[email protected]>

saikishor · 2025-05-03T12:55:12Z

control_toolbox/include/control_toolbox/filter_traits.hpp

+  static void validate_input(const T & data_in, const StorageType & filtered_value, T & data_out)
+  {
+    (void)data_in;
+    (void)filtered_value;
+    (void)data_out;  // Suppress unused warnings
+  }
+
+  static void add_metadata(StorageType & storage, const StorageType & data_in)
+  {
+    (void)storage;
+    (void)data_in;
+  }


Can't we make it pure virtual?

I think we can't do virtual templates (I say this based on this discussion here). But the discussion does mention type erasure as an alternative, which might be applicable in our case. What do you think?

saikishor · 2025-05-03T12:57:34Z

control_toolbox/include/control_toolbox/filter_traits.hpp

+template <typename U, typename Alloc>
+struct FilterTraits<std::vector<U, Alloc>>


Are we specialising with allocator here?

If I understand it correctly this function will only be used by "default" vectors, so I believe we could just drop the Alloc, what do you think?

Yes! You are right. That's why I asked about the alloc template argument

control_toolbox/include/control_toolbox/filter_traits.hpp

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 · 2025-05-04T19:49:34Z

Sorry @christophfroehlich, I'm not sure what I did but for some reason I dismissed your review 😢

christophfroehlich · 2025-05-04T20:11:24Z

This happens if you push a new commit after a review, no worries

saikishor

LGTM. Just a minor comment

saikishor · 2025-05-04T21:22:13Z

control_toolbox/include/control_toolbox/filter_traits.hpp

+
+  static void initialize(StorageType & storage)
+  {
+    storage.data = std::vector<U>{std::numeric_limits<U>::quiet_NaN()};


I'm thinking about this one. I'm not sure if we have to initialize it or not, because in the first loop it will have to fill in the data right?. Do we want to have it pre-filled with the data?

I'm just saying because we are not sure about the size at the initialization time

That's true, I think it would be better to leave it empty.

In the low_pass_filter.hpp, we already check for emptiness to check if it is a first call. So it would make sense to leave the std::vector empty, because, as you said, we don't know its size at the initialization time.

But after your question I thought: does it even make sense to have a configure() method to make the filter variables be NaN? I think it would be simpler if we removed the configure() method for all overloads, not only the std::vector.

And we could check if it is the first time that the update() method is being called by a member bool first_run_ or something like that. So instead of having:

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 (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) { // rest of the code } }

We could have:

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 (first_run_) { // rest of the code first_run_ = false } }

And remove the configure() method entirely. What do you think?

It depends on the type of filter; it might make sense to have the configure method.
I would say just change it to be empty for now and that should do

Suggested change

storage.data = std::vector<U>{std::numeric_limits<U>::quiet_NaN()};

(void)storage;

It is done!

LGTM. let me run the CI

Signed-off-by: Pedro Nogueira <[email protected]>

github-actions bot requested review from bmagyar, destogl, fmauch, LukasMacha97, mhubii, peterdavidfagan and rosterloh April 27, 2025 01:17

Make member variables of LowPassFilter class generic

d21cfcc

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 changed the base branch from jazzy to ros2-master April 27, 2025 01:26

pedroazeredo04 force-pushed the improve_low_pass_filter_class branch from e817778 to d21cfcc Compare April 27, 2025 01:26

saikishor requested changes Apr 27, 2025

View reviewed changes

Run pre-commit and format code

1612ade

Signed-off-by: Pedro Nogueira <[email protected]>

Add filter traits structure

72a80ba

Signed-off-by: Pedro Nogueira <[email protected]>

Make LowPassFilter::update() function generic

68a550d

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 and others added 4 commits April 28, 2025 19:58

Remove useless comments

5402d8d

Signed-off-by: Pedro Nogueira <[email protected]>

Fix pre commit warning

a21b6a2

Signed-off-by: Pedro Nogueira <[email protected]>

Remove useless function

82da940

Signed-off-by: Pedro Nogueira <[email protected]>

Merge branch 'ros2-master' into improve_low_pass_filter_class

8a3d6e6

christophfroehlich requested changes Apr 28, 2025

View reviewed changes

pedroazeredo04 added 2 commits April 30, 2025 09:07

Implement generic Traits::add_metadata method

45f3c74

Signed-off-by: Pedro Nogueira <[email protected]>

Rename Traits::is_infinite method to Traits::is_finite

85b46c4

Signed-off-by: Pedro Nogueira <[email protected]>

Change affilliation in FilterTraits header

a1710db

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 added 4 commits April 30, 2025 10:06

Change Vector6d to Eigen::Matrix in FilterTraits

dd0e3db

Signed-off-by: Pedro Nogueira <[email protected]>

Fix eigen matrix initialization as NaN

e6a066e

Signed-off-by: Pedro Nogueira <[email protected]>

Add is_std_vector method

00c0ffa

Signed-off-by: Pedro Nogueira <[email protected]>

Add generic validate_input method

60c132a

Signed-off-by: Pedro Nogueira <[email protected]>

christophfroehlich previously approved these changes May 2, 2025

View reviewed changes

christophfroehlich requested a review from saikishor May 2, 2025 21:58

pedroazeredo04 added 2 commits May 3, 2025 09:46

Remove unused is_std_vector helper

ab695fb

Signed-off-by: Pedro Nogueira <[email protected]>

Generalize types inside the std::vector overload of FilterTraits

56c7edb

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 dismissed christophfroehlich’s stale review via 56c7edb May 3, 2025 08:19

pedroazeredo04 requested a review from christophfroehlich May 3, 2025 08:19

christophfroehlich previously approved these changes May 3, 2025

View reviewed changes

saikishor reviewed May 3, 2025

View reviewed changes

control_toolbox/include/control_toolbox/filter_traits.hpp Show resolved Hide resolved

Remove allocator specialization

9156cb0

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 dismissed christophfroehlich’s stale review via 9156cb0 May 4, 2025 19:47

pedroazeredo04 requested a review from saikishor May 4, 2025 19:48

saikishor previously approved these changes May 4, 2025

View reviewed changes

saikishor and others added 2 commits May 4, 2025 23:22

Merge branch 'ros2-master' into improve_low_pass_filter_class

f68ad4a

Initialize std::vector as empty inside filter traits

4a7609a

Signed-off-by: Pedro Nogueira <[email protected]>

pedroazeredo04 dismissed saikishor’s stale review via 4a7609a May 5, 2025 20:22

	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,

		template <typename U, typename Alloc>
		struct FilterTraits<std::vector<U, Alloc>>

	storage.data = std::vector<U>{std::numeric_limits<U>::quiet_NaN()};
	(void)storage;

Make member variables of LowPassFilter class generic #351

Are you sure you want to change the base?

Make member variables of LowPassFilter class generic #351

Conversation

pedroazeredo04 commented Apr 27, 2025 • edited Loading

mergify bot commented Apr 27, 2025

mergify bot commented Apr 27, 2025

saikishor left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

christophfroehlich commented Apr 27, 2025

pedroazeredo04 commented Apr 27, 2025

christophfroehlich commented Apr 27, 2025

pedroazeredo04 commented Apr 27, 2025

christophfroehlich commented Apr 27, 2025

pedroazeredo04 commented Apr 28, 2025

christophfroehlich left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pedroazeredo04 commented Apr 30, 2025

pedroazeredo04 commented May 2, 2025

codecov-commenter commented May 2, 2025 • edited Loading

Codecov Report

christophfroehlich left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pedroazeredo04 commented May 4, 2025

christophfroehlich commented May 4, 2025

saikishor left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pedroazeredo04 commented Apr 27, 2025 •

edited

Loading

christophfroehlich left a comment •

edited

Loading

codecov-commenter commented May 2, 2025 •

edited

Loading