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

[MarqRazz]

This PR adds the option to create a LowPassFilter object with type std::vector<double> (seeing that most system interfaces are this type 😄 )

The velocity data from my robot is noisy so I needed to clean it up

to

[christophfroehlich]

Implementation-wise LGTM.
Do you mind adding the load test here, and a simple test here?

[MarqRazz]

Implementation-wise LGTM. Do you mind adding the load test here, and a simple test here?

Sure thing. Just wanted to get the initial changes out there to see what you guys thought.

[christophfroehlich]

In principle we have this missing in every filter, which is applicable to a double. Maybe we should use the upstream MultiChannelFilter instead? https://github.com/ros/filters/blob/ros2/include/filters/filter_base.hpp#L285C7-L285C29

[MarqRazz]

In principle we have this missing in every filter, which is applicable to a double. Maybe we should use the upstream MultiChannelFilter instead? https://github.com/ros/filters/blob/ros2/include/filters/filter_base.hpp#L285C7-L285C29

If we did that we would need a new header and class for each filter type correct? For example:

template <typename T>
class LowPassMultiChannelFilter : public filters::MultiChannelFilterBase<T>

What does MultiChannel buy us over the current approach? You still need to write bool update(const std::vector<T> & data_in, std::vector<T> & data_out) for each type.

Technically in the hardware_interface I'm writing I am directly using the control_toolbox::LowPassFilter because I don't have any access to the node the interface is running in and that seems to be a requirement to use a control_filters::LowPassFilter.

That is I assume because my hw_interface doesn't have access to this ROS stuff it segfaults if I try to call configure() on a control_filters::LowPassFilter object.

[taskset-1] #0    Object "/root/c3p0_ws/build/kortex3_driver/libkortex3_driver.so", at 0x72bcfba43d25, in control_filters::LowPassFilter<std::vector<double, std::allocator<double> > >::configure()
[taskset-1] Segmentation fault (Address not mapped to object [(nil)])

[christophfroehlich]

Oh, I see your point. Using the LPF without ros/fillters is a different story.

[saikishor]

Apart from the minor comments, @MarqRazz have you considered using Eigen::VectorXd instead of std::vector? I think it is better to use Eigen::VectorXd

[xguay]

LGTM other than the changes that have already been requested.

May want to consider wrapping one update with the other so there's only one source of logic. It's unusual but I'd suggest wrapping the vector version in the original. i.e. put the value in a vector then feed it to the vector based updater. It will create a single source of logic. You also get the benefit of doing checks once, instead of every index like you would if you wrapped the other way.

[MarqRazz]

Apart from the minor comments, @MarqRazz have you considered using Eigen::VectorXd instead of std::vector? I think it is better to use Eigen::VectorXd

So internally you think it would be better to convert between Eigen and std::vector on each update()? What does switching to Eigen add for us here?

[MarqRazz]

The control_filters tests that include ROS seem to be flaky. I can get 100% tests passed, 0 tests failed out of 12 but frequently the PID and G-comp test timeout. I think it might be because multiple tests are running at the same time and conflicting with each other.

 5/12 Test  #5: test_gravity_compensation ........***Timeout  60.02 sec
test 6
      Start  6: test_load_gravity_compensation

6: Test command: /usr/bin/python3 "-u" "/opt/ros/jazzy/share/ament_cmake_test/cmake/run_test.py" "/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml" "--package-name" "control_toolbox" "--output-file" "/root/colcon_ws/build/control_toolbox/ament_cmake_gmock/test_load_gravity_compensation.txt" "--command" "/root/colcon_ws/build/control_toolbox/test_load_gravity_compensation" "--gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml"
6: Working Directory: /root/colcon_ws/build/control_toolbox
6: Test timeout computed to be: 60
6: -- run_test.py: invoking following command in '/root/colcon_ws/build/control_toolbox':
6:  - /root/colcon_ws/build/control_toolbox/test_load_gravity_compensation --gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml
6: Running main() from gmock_main.cc

The following tests FAILED:
	  5 - test_gravity_compensation (Timeout)

[christophfroehlich]

The control_filters tests that include ROS seem to be flaky. I can get 100% tests passed, 0 tests failed out of 12 but frequently the PID and G-comp test timeout. I think it might be because multiple tests are running at the same time and conflicting with each other.

 5/12 Test  #5: test_gravity_compensation ........***Timeout  60.02 sec
test 6
      Start  6: test_load_gravity_compensation

6: Test command: /usr/bin/python3 "-u" "/opt/ros/jazzy/share/ament_cmake_test/cmake/run_test.py" "/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml" "--package-name" "control_toolbox" "--output-file" "/root/colcon_ws/build/control_toolbox/ament_cmake_gmock/test_load_gravity_compensation.txt" "--command" "/root/colcon_ws/build/control_toolbox/test_load_gravity_compensation" "--gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml"
6: Working Directory: /root/colcon_ws/build/control_toolbox
6: Test timeout computed to be: 60
6: -- run_test.py: invoking following command in '/root/colcon_ws/build/control_toolbox':
6:  - /root/colcon_ws/build/control_toolbox/test_load_gravity_compensation --gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml
6: Running main() from gmock_main.cc

The following tests FAILED:
	  5 - test_gravity_compensation (Timeout)

Try with a sequential executor
https://colcon.readthedocs.io/en/released/reference/executor-arguments.html
https://github.com/ros-controls/ros2_control_ci/blob/master/.github/workflows/reusable-ros-tooling-source-build.yml#L93-L98

[MarqRazz]

Try with a sequential executor

I tried and it didn't help so I could be wrong about what is hanging, but it is flaky. Can you enable the tests and lets see how it goes? I'm also testing this on the jazzy branch (in ROS Jazzy) so not sure if that changes anything 🤷
colcon test --executor sequential --event-handlers console_direct+ --packages-select control_toolbox --pytest-args -s

I was surprised that I needed to declare my parameters in testing the vector double calculation and it isn't required in the others.

Here is the full output of the other package failing.

test 6
      Start  6: test_load_gravity_compensation

6: Test command: /usr/bin/python3 "-u" "/opt/ros/jazzy/share/ament_cmake_test/cmake/run_test.py" "/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml" "--package-name" "control_toolbox" "--output-file" "/root/colcon_ws/build/control_toolbox/ament_cmake_gmock/test_load_gravity_compensation.txt" "--command" "/root/colcon_ws/build/control_toolbox/test_load_gravity_compensation" "--gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml"
6: Working Directory: /root/colcon_ws/build/control_toolbox
6: Test timeout computed to be: 60
6: -- run_test.py: invoking following command in '/root/colcon_ws/build/control_toolbox':
6:  - /root/colcon_ws/build/control_toolbox/test_load_gravity_compensation --gtest_output=xml:/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml
6: Running main() from gmock_main.cc
6: [==========] Running 1 test from 1 test suite.
6: [----------] Global test environment set-up.
6: [----------] 1 test from TestLoadGravityCompensationFilter
6: [ RUN      ] TestLoadGravityCompensationFilter.load_gravity_compensation_filter_wrench
6: [       OK ] TestLoadGravityCompensationFilter.load_gravity_compensation_filter_wrench (2 ms)
6: [----------] 1 test from TestLoadGravityCompensationFilter (2 ms total)
6: 
6: [----------] Global test environment tear-down
6: [==========] 1 test from 1 test suite ran. (2 ms total)
6: [  PASSED  ] 1 test.
6: -- run_test.py: return code 0
6: -- run_test.py: inject classname prefix into gtest result file '/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml'
6: -- run_test.py: verify result file '/root/colcon_ws/build/control_toolbox/test_results/control_toolbox/test_load_gravity_compensation.gtest.xml'
 6/12 Test  #6: test_load_gravity_compensation ...   Passed    0.06 sec

The following tests FAILED:
	  5 - test_gravity_compensation (Timeout)

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 76.31579% with 18 lines in your changes missing coverage. Please review.

Project coverage is 77.41%. Comparing base (24cf0d8) to head (a7f0207).
Report is 24 commits behind head on ros2-master.

Files with missing lines	Patch %	Lines
...oolbox/include/control_toolbox/low_pass_filter.hpp	56.66%	8 Missing and 5 partials ⚠️
...oolbox/include/control_filters/low_pass_filter.hpp	28.57%	4 Missing and 1 partial ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           ros2-master     #340      +/-   ##
===============================================
- Coverage        77.67%   77.41%   -0.26%     
===============================================
  Files               29       29              
  Lines             1272     1333      +61     
  Branches            85       92       +7     
===============================================
+ Hits               988     1032      +44     
- Misses             239      250      +11     
- Partials            45       51       +6     

Flag	Coverage Δ
unittests	77.41% <76.31%> (-0.26%)	⬇️

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

Files with missing lines	Coverage Δ
...test/control_filters/test_load_low_pass_filter.cpp	100.00% <100.00%> (ø)
...lbox/test/control_filters/test_low_pass_filter.cpp	100.00% <100.00%> (ø)
...oolbox/include/control_filters/low_pass_filter.hpp	50.00% <28.57%> (-5.18%)	⬇️
...oolbox/include/control_toolbox/low_pass_filter.hpp	68.11% <56.66%> (-8.08%)	⬇️

🚀 New features to boost your workflow:

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

[saikishor]

So internally you think it would be better to convert between Eigen and std::vector on each update()? What does switching to Eigen add for us here?

I mean not only internally, but also to the method's inputs and outputs. Eigen is especially efficient if the vector is larger. Moreover, it has some operations that lets you do operations on whole vector at same time

[MarqRazz]

I mean not only internally, but also to the method's inputs and outputs. Eigen is especially efficient if the vector is larger. Moreover, it has some operations that lets you do operations on whole vector at same time

I would suggest we add another type in a different PR. I am using std::vector<double> because that is what my system interface has so it's super easy to add filters on the incoming noisy joint data.

[christophfroehlich]

minor suggestion, otherwise this looks fine!

[MarqRazz]

Thanks for the quick responses guys! ❤️

Could we please back port to Jazzy 🙏

[saikishor]

Looks good to me, just a minor thing

[saikishor]

LGTM