Add lowpass filter #152

guihomework · 2023-03-07T16:58:46Z

This is a rework of PR #115 for the low-pass filter part, now using generate_parameter_library.

- added testcases for loading parameters

removed parameter_namespace and added override flag

Co-authored-by: Denis Štogl <[email protected]>

# Conflicts: # include/control_filters/gravity_compensation.hpp # src/control_filters/gravity_compensation.cpp

fixed gravity compensation calculated values fixed guard condition issues (rclcpp init must be called before) fixed configure issues (low-level must be configured to avoid undefined logger) # Conflicts: # CMakeLists.txt # test/control_filters/test_gravity_compensation.cpp # test/control_filters/test_gravity_compensation.hpp

guihomework · 2023-03-09T08:32:25Z

xmlcheck complained about angle brackets, to type of class to instantiate.
since there is no cli for pluginlib in ros2 yet, to quickly test if the <> would be usable in the command line, a pluginlib instantiation should be tested within the tests

guihomework · 2023-03-09T14:23:45Z

Test passes with the escaped angle brackets.

destogl · 2023-03-11T12:14:59Z

@bmagyar can we branch-out here for humble and rolling?, i.e., we need one foxy branch that holds current code for foxy and galactic.

destogl

1st part...

CMakeLists.txt

destogl · 2023-03-11T12:31:27Z

CMakeLists.txt

+
+target_include_directories(low_pass_filter PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+    $<INSTALL_INTERFACE:include>
+)
+generate_parameter_library(
+  low_pass_filter_parameters
+  src/control_filters/low_pass_filter_parameters.yaml
+)
+target_link_libraries(low_pass_filter low_pass_filter_parameters)
+ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})


Suggested change

target_include_directories(low_pass_filter PUBLIC

$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

$<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>

$<INSTALL_INTERFACE:include>

)

generate_parameter_library(

low_pass_filter_parameters

src/control_filters/low_pass_filter_parameters.yaml

)

target_link_libraries(low_pass_filter low_pass_filter_parameters)

ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})

target_compile_features(low_pass_filter PUBLIC cxx_std_17)

target_include_directories(low_pass_filter PUBLIC

$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

$<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>

$<INSTALL_INTERFACE:include/control_filters>

)

target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters)

ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})

This line $<INSTALL_INTERFACE:include/control_filters> maybe have to be $<INSTALL_INTERFACE:include/control_toolbox> - please test which version is working.

i am actually surprised one needs to install the includes in a different folder than the project name or our own name (control_filters).
The code before my PR #152 (which is only 3 weeks old from #145 ) already installs in a control_toolbox sub-folder of the project name, making it look like
my_ws/install/control_toolbox/include/control_toolbox/control_toolbox
my_ws/install/control_toolbox/include/control_toolbox/control_filters
is that really what we want ? I have difficulties understanding the "override" thing, so maybe there is a good reason, otherwise I would expect:
my_ws/install/control_toolbox/include/control_toolbox
and for the control filters
my_ws/install/control_toolbox/include/control_filters

it is possible this got un-noticed, because with underlays still having the control_toolbox includes looking as I would expect it, this simply worked (the underlay libs were found ?) until this new change gets released ?

I understand this is confusing. It is also for all of us, but this is how ament und colcon work...

you can read here more details and check references why we have to do that: ros-controls/ros2_control#926

it is possible this got un-noticed, because with underlays still having the control_toolbox includes looking as I would expect it, this simply worked (the underlay libs were found ?) until this new change gets released ?

You are adding here new libs, and they work without problem. The issue comes when you have a lib, install in the system and want to override it as in ROS 1 – this doesn't work without these tricks. After loosing already a dozen of hours debugging non-existing issues, we decided to take this ugly implementation and just make things to be easily used.

OK, maybe #145 should have at least pointed to the doc justifying that, then I would not have asked.

This is not nice but very important and something others will stumble upon. I had seen the override warnings in colcon build but I did not relate the "ordering" to the strange naming.

I see that ROS 1 solved that known issue of overlays by having re-ordering of the includes done in the cmake when catkin was called.

I hope this gets solved one day.

This line $<INSTALL_INTERFACE:include/control_filters> maybe have to be $<INSTALL_INTERFACE:include/control_toolbox> - please test which version is working.

since the destination of the source includes installation is this name-trick to permit override, the install interface of the filters has to be in the same folder as this name-trick.
if one wants to have a different name-trick applied to the control_filters so that it goes in control_filters/control_filters
the install cmake command must be modified to do 2 sub-folder installs for control_toolbox and control_filters separately.

depends what you prefer:

include/control_toolbox/control_toolbox
include/control_toolbox/control_filters
or
include/control_toolbox/control_toolbox
include/control_filters/control_filters

For my understanding :
Could you confirm there is no override issue with the generated libraries headers (in case a this generate library yaml gets changed in an overlay), although they get installed in include/low_pass_filter_parameters/ and not in include/low_pass_filter_parameters/low_pass_filter_parameters because the target_link_library sees it is a local target and finds the local header ?

We didn't test this yet with generated parameters.

CMakeLists.txt

destogl · 2023-03-11T12:36:50Z

README.md

+  - `clang-format-10`
+     ```
+     sudo apt install clang-format-10


We use clang-format-14

Suggested change

- `clang-format-10`

```

sudo apt install clang-format-10

- `clang-format-14`

```

sudo apt install clang-format-14

See here: https://github.com/ros-controls/control_toolbox/blob/ros2-master/.github/workflows/ci-format.yml

Please let the reviewer close/mark the comments as resolved. You can just give a short remark like "done" or thumbs up emoji if you just agree. Sometimes is confusing when something is marked as done, but actually one doesn't sees it in the changes.

Oh ok, this is how it works. I did not use it this way in the past.

control_filters.xml

include/control_filters/low_pass_filter.hpp

src/control_filters/low_pass_filter_parameters.yaml

test/control_filters/test_low_pass_filter.cpp

This reverts commit 78b386a.

include/control_filters/low_pass_filter.hpp

destogl

Except the small issues, I am happy with this.

destogl · 2023-03-19T20:45:20Z

README.md

@@ -27,3 +27,25 @@ doi = {10.21105/joss.00456},
 URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf}
 }
 ```
+
+
+## Code Formatting


We should add this in the separate PR. It's not part of the functionality presented here

destogl · 2023-03-19T20:48:16Z

include/control_filters/low_pass_filter.hpp

+  low_pass_filter::Params parameters_;
+
+  // Filter parameters
+  // TODO(destogl): we should do this more intelligently using only one set of types


Not necessary since we didn't come up with any better solution.

Suggested change

// TODO(destogl): we should do this more intelligently using only one set of types

This was not worked on yet. I had a quick look, maybe something along using std::enable_if to detect if a type can be directly filtered with the y(n)= b * x(n-1) + a * y(n-1) example double, ints Eigen<N,1>, etc..) or if a specialization has been provided for converting the T type to a processable storage...
https://medium.com/@sidbhasin82/c-templates-what-is-std-enable-if-and-how-to-use-it-fd76d3abbabe

src/control_filters/low_pass_filter_parameters.yaml

destogl · 2023-03-19T20:50:17Z

src/control_filters/low_pass_filter_parameters.yaml

+  #divider:
+  #{
+  #  type: int,
+  #  # default_value: 3,
+  #  description: "Divider",
+  #  validation:
+  #  {
+  #    gt_eq<>: [0]
+  #  },
+  #  read_only: true
+  #}


Suggested change

#divider:

#{

# type: int,

# # default_value: 3,

# description: "Divider",

# validation:

# {

# gt_eq<>: [0]

# },

# read_only: true

#}

test/control_filters/test_low_pass_filter.cpp

destogl

This looks great! Thanks. Just a few more cleanings. One test is duplicate since using generate_paramter_library and since it it done in a test later.

destogl · 2023-03-27T10:44:25Z

src/control_filters/low_pass_filter_parameters.yaml

+low_pass_filter:
+  sampling_frequency: {
+    type: double,
+    # default_value: 1000.0,


Suggested change

# default_value: 1000.0,

destogl · 2023-03-27T10:44:31Z

src/control_filters/low_pass_filter_parameters.yaml

+  }
+  damping_frequency: {
+    type: double,
+    # default_value: 20.5,


Suggested change

# default_value: 20.5,

destogl · 2023-03-27T10:44:37Z

src/control_filters/low_pass_filter_parameters.yaml

+  }
+  damping_intensity: {
+    type: double,
+    # default_value: 1.25,


Suggested change

# default_value: 1.25,

done all of these 3 default removal

destogl · 2023-03-27T10:49:38Z

test/control_filters/test_low_pass_filter.cpp

+TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter)
+{
+    std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
+        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+
+    // should deny configuration as sampling frequency is missing
+    ASSERT_FALSE(filter_->configure("", "TestLowPassFilter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+}


not needed when using generate_parameter_library

I don't see why one should not test for filter configure failing "properly". This test permits to verify that the parameters were correctly defined in the generate_parameter_library YAML as "should fail if missing", and that the throw is caught correctly isn't it ?

destogl · 2023-03-27T10:51:52Z

test/control_filters/test_low_pass_filter_parameters.yaml

+TestLowPassWrenchFilterMissingParameter:
+  ros__parameters:
+    damping_frequency: 20.5
+    damping_intensity: 1.25
+


Not needed since using generate_parameter library.

christophfroehlich

If I were allowed to wish something: a short README would help guiding new users, as well as adding some function API documentation inside the header (like in all other functions of this repository).

guihomework · 2023-04-18T15:41:03Z

If I were allowed to wish something: a short README would help guiding new users

Do you mean a doc similar to the one in the Gravity Compensation "filter" https://github.com/ros-controls/control_toolbox/blob/f181fe76279860ff9b1c9f3d9edde927da29703b/src/control_filters/README.md
or more a general description of the role of control filters and why they are there and how to use them ?

christophfroehlich · 2023-04-18T16:11:50Z

If I were allowed to wish something: a short README would help guiding new users

Do you mean a doc similar to the one in the Gravity Compensation "filter" https://github.com/ros-controls/control_toolbox/blob/f181fe76279860ff9b1c9f3d9edde927da29703b/src/control_filters/README.md or more a general description of the role of control filters and why they are there and how to use them ?

I mean a doc similar to the Gravity filter -> it will be short for this one. A general readme for this repo is out of scope for this PR IMHO.

guihomework · 2023-04-18T17:18:01Z

@christophfroehlich Thanks for the clarification. Another question: I am new to PR that require rebasing+changes, when the reviewer does merges of master in the PR. What should I do on my side ? rebase my initial branch (before your merge from master) on top of maaster, commit the requested changes and push or now add the requested changes over your merged-commit. I am used to always keep history of branches clean and straight.

christophfroehlich · 2023-04-18T18:04:54Z

I also prefer having a clean history, but in the end everything gets squashed in a single commit on master -> I'd suggest that you just commit on top of the last merge-commit.

guihomework · 2023-04-18T18:17:19Z

This makes sense. When I kept branches clean, it was to keep all commits in the master as well.

guihomework · 2023-04-19T07:48:35Z

If I were allowed to wish something: a short README would help guiding new users, as well as adding some function API documentation inside the header (like in all other functions of this repository).

both done now.

christophfroehlich

Great, thx!

…filter-rolling

dzumkeller and others added 12 commits March 7, 2023 15:29

Ported low_pass_filter from iirob_filters:

0dde40d

- added testcases for loading parameters

added new testcase after review

1e6e6b7

Update low_pass_filter.hpp

89970d6

removed parameter_namespace and added override flag

Apply suggestions from code review

ea334d7

Co-authored-by: Denis Štogl <[email protected]>

Added initial files of control filters

dc4623a

Apply suggestions from code review

096ea4f

Co-authored-by: Denis Štogl <[email protected]>

Apply formatting to filters.

b764379

# Conflicts: # include/control_filters/gravity_compensation.hpp # src/control_filters/gravity_compensation.cpp

Update filters to use full impl of parameter hanlder.

581f474

Removed GravityCompensation

a59795e

Switched to generate_parameter_library

9702d40

Made check xml pass

4386f12

guihomework added 2 commits March 9, 2023 10:48

Homogenized naming with typical filters

5a7a9cc

Reworked and activated control filter loading test

7f1c730

Improved parameter_handler usage when re-configure

f620e3a

destogl requested review from destogl, bmagyar and christophfroehlich March 11, 2023 12:17

destogl marked this pull request as ready for review March 11, 2023 12:17

destogl reviewed Mar 11, 2023

View reviewed changes

guihomework added 2 commits March 11, 2023 17:04

Applied code-review suggestions and fixed includes

eda275e

potential other change

78b386a

christophfroehlich requested changes Mar 13, 2023

View reviewed changes

guihomework added 2 commits March 13, 2023 10:01

Revert "potential other change"

f40f53b

This reverts commit 78b386a.

Added validator for damping frequency

4154508

destogl mentioned this pull request Mar 13, 2023

Ported LowPass and GravityCompensation filters iirob_filters repository #115

Closed

guihomework commented Mar 13, 2023

View reviewed changes

include/control_filters/low_pass_filter.hpp Outdated Show resolved Hide resolved

Fixed missing namespace forwarding

b98f199

guihomework force-pushed the add-lowpass-filter-rolling branch from ee9115c to b98f199 Compare March 13, 2023 19:28

Added missing Wrench.header copy

2bdaacd

guihomework mentioned this pull request Mar 19, 2023

Admittance with filters destogl/ros2_controllers#45

Closed

destogl requested changes Mar 19, 2023

View reviewed changes

guihomework added 2 commits March 20, 2023 09:37

Removed unrelated README changes

5fb1233

Cleaned-up unused variables

a6fee4b

guihomework mentioned this pull request Mar 20, 2023

Add gravity compensation filter #153

Merged

Switched to yaml-based param in filter test

b6cbb02

destogl previously approved these changes Mar 27, 2023

View reviewed changes

This was referenced Apr 3, 2023

Add filtering capability and publishers for additional frames to fts broadcaster ros-controls/ros2_controllers#559

Closed

Admittance with filters ros-controls/ros2_controllers#560

Open

Removed unused defaults

dd87700

guihomework dismissed destogl’s stale review via dd87700 April 17, 2023 15:49

christophfroehlich requested changes Apr 18, 2023

View reviewed changes

Merge branch 'ros2-master' into add-lowpass-filter-rolling

b41ec68

guihomework added 2 commits April 19, 2023 09:46

Added doxygen doc

36733bf

Added low-pass filter documentation in a README

036dac0

christophfroehlich approved these changes Apr 19, 2023

View reviewed changes

bmagyar approved these changes May 7, 2023

View reviewed changes

Merge remote-tracking branch 'upstream/ros2-master' into add-lowpass-…

5cae8ce

…filter-rolling

destogl merged commit 00eb150 into ros-controls:ros2-master Aug 8, 2023

Add lowpass filter #152

Add lowpass filter #152

Uh oh!

Conversation

guihomework commented Mar 7, 2023

Uh oh!

guihomework commented Mar 9, 2023

Uh oh!

guihomework commented Mar 9, 2023

Uh oh!

destogl commented Mar 11, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

destogl left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

destogl left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

destogl left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

destogl commented Mar 11, 2023 •

edited

Loading