From 0dde40d1176c2ae0fb60cfaaa9af4e0205244492 Mon Sep 17 00:00:00 2001 From: dzumkeller Date: Mon, 6 Sep 2021 19:03:29 +0200 Subject: [PATCH 01/27] Ported low_pass_filter from iirob_filters: - added testcases for loading parameters --- CMakeLists.txt | 51 +++- control_filters.xml | 11 + include/control_filters/low_pass_filter.hpp | 217 ++++++++++++++++++ src/control_filters/low_pass_filter.cpp | 21 ++ test/control_filters/test_low_pass_filter.cpp | 91 ++++++++ test/control_filters/test_low_pass_filter.hpp | 58 +++++ 6 files changed, 447 insertions(+), 2 deletions(-) create mode 100644 control_filters.xml create mode 100644 include/control_filters/low_pass_filter.hpp create mode 100644 src/control_filters/low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fda0ecb7..c2faa770 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp + rclcpp_lifecycle rcutils realtime_tools ) @@ -41,10 +42,43 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") +######################## +# Build control filters +######################## +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(Eigen3 REQUIRED) + +set(CONTROL_FILTERS_INCLUDE_DEPENDS + filters + geometry_msgs + pluginlib + rclcpp +) + +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp +) +target_include_directories(low_pass_filter PUBLIC + $ + $ + $ +) +ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) find_package(rclcpp_lifecycle REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + + ament_lint_auto_find_test_dependencies() ament_add_gmock(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests control_toolbox) @@ -55,19 +89,32 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests control_toolbox) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) + + ## Control Filters + ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) + target_link_libraries(test_low_pass_filter low_pass_filter) + ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() +# Install install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox +install(TARGETS control_toolbox low_pass_filter EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) + ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +ament_export_libraries( + control_toolbox + low_pass_filter +) ament_package() diff --git a/control_filters.xml b/control_filters.xml new file mode 100644 index 00000000..824df79c --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,11 @@ + + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + + + diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp new file mode 100644 index 00000000..e02ee80f --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,217 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP + +#include +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "filters/filter_base.hpp" +#include "filters/filter_chain.hpp" +#include +#include "rclcpp/node.hpp" + + +namespace control_filters +{ +template +class LowPassFilter : public filters::FilterBase +{ +public: + LowPassFilter(); + + ~LowPassFilter(); + + /** @brief Configure filter parameters */ + virtual bool configure(); + + bool configure(const std::string& ns); + + virtual bool update(const T& data_in, T& data_out); + + /** \brief Get most recent parameters */ + bool updateParameters(); + +private: + /** \brief Dynamic parameter callback activated when parameters change */ + void parameterCallback(); + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Parameters + std::string parameter_namespace_; + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + int divider_; + + // Filter parameters + double b1; + double a1; + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // dynamic parameters + bool params_updated_; + bool initialized_; + bool configured_; +}; + +template +LowPassFilter::LowPassFilter() + : logger_(rclcpp::get_logger("LowPassFilter")), params_updated_(false), initialized_(false), configured_(false) +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + configure(""); + return true; +} + +template +bool LowPassFilter::configure(const std::string& ns) +{ + if (!initialized_) + { + RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); + return false; + } + + parameter_namespace_ = ns; + + if (!updateParameters()) + { + return false; + } + + configured_ = true; + + RCLCPP_INFO(logger_, + "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " + "divider :%d", + sampling_frequency_, damping_frequency_, damping_intensity_, divider_); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + for (unsigned int i = 0; i < 6; i++) + { + msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; + } + + return true; +} + +template <> +inline bool LowPassFilter::update(const geometry_msgs::msg::WrenchStamped& data_in, + geometry_msgs::msg::WrenchStamped& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + updateParameters(); + } + + // IIR Filter + msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO use wrenchMsgToEigen + msg_old[0] = data_in.wrench.force.x; + msg_old[1] = data_in.wrench.force.y; + msg_old[2] = data_in.wrench.force.z; + msg_old[3] = data_in.wrench.torque.x; + msg_old[4] = data_in.wrench.torque.y; + msg_old[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = msg_filtered[0]; + data_out.wrench.force.y = msg_filtered[1]; + data_out.wrench.force.z = msg_filtered[2]; + data_out.wrench.torque.x = msg_filtered[3]; + data_out.wrench.torque.y = msg_filtered[4]; + data_out.wrench.torque.z = msg_filtered[5]; + return true; +} + +template +bool LowPassFilter::update(const T& data_in, T& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + if (!updateParameters()) + { + RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); + return false; + } + } + + data_out = b1 * old_value + a1 * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +template +bool LowPassFilter::updateParameters() +{ + + if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); + return false; + } + + params_updated_ = false; + a1 = exp(-1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / (pow(10.0, damping_intensity_ / -10.0))); + b1 = 1.0 - a1; + return true; +} + +template +void LowPassFilter::parameterCallback() +{ + params_updated_ = true; +} + +} // namespace iirob_filters +#endif diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp new file mode 100644 index 00000000..56142344 --- /dev/null +++ b/src/control_filters/low_pass_filter.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "control_filters/low_pass_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, + filters::FilterBase) + diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp new file mode 100644 index 00000000..5b6c517e --- /dev/null +++ b/test/control_filters/test_low_pass_filter.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_low_pass_filter.hpp" + +TEST_F(LowPassFilterTest, TestLowPassFilter) +{ + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + int divider = 3; + + bool 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; + + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + + //not yet configured, should deny update + ASSERT_FALSE(low_pass_filter_.update(in,out)); + + //should deny configuration as parameters are not yet set + ASSERT_FALSE(low_pass_filter_.configure()); + + //declare parameters and configure + node_->declare_parameter("sampling_frequency", sampling_freq); + node_->declare_parameter("damping_frequency", damping_freq); + node_->declare_parameter("damping_intensity", damping_intensity); + node_->declare_parameter("divider", divider); + ASSERT_TRUE(low_pass_filter_.configure()); + + + //first filter pass, output should be zero as no old wrench was stored + ASSERT_TRUE(low_pass_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: + //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; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_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); + + //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; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_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); + +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp new file mode 100644 index 00000000..124616fa --- /dev/null +++ b/test/control_filters/test_low_pass_filter.hpp @@ -0,0 +1,58 @@ + + +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" +#include + +#include "control_filters/low_pass_filter.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); +} // namespace + + +class LowPassFilterTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + LowPassFilterTest() + : node_(std::make_shared("test_low_pass_filter")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::LowPassFilter low_pass_filter_; + std::thread executor_thread_; +}; From 1e6e6b7b4303cd03038f9a617b80e17f319cabae Mon Sep 17 00:00:00 2001 From: dzumkeller Date: Thu, 7 Oct 2021 17:31:54 +0200 Subject: [PATCH 02/27] added new testcase after review --- .../test_load_low_pass_filter.cpp | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 test/control_filters/test_load_low_pass_filter.cpp diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp new file mode 100644 index 00000000..47ac7bad --- /dev/null +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -0,0 +1,43 @@ +Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadLowPassFilter, load_low_pass_filter) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_low_pass_filter", "controlFilters/LowPassFilter")); + + rclcpp::shutdown(); +} + + From 89970d6cd61df01f9c9d5538ffc6f8b0e24c8946 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:18:45 +0200 Subject: [PATCH 03/27] Update low_pass_filter.hpp removed parameter_namespace and added override flag --- include/control_filters/low_pass_filter.hpp | 30 +++++++-------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e02ee80f..5ad04519 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -16,10 +16,11 @@ #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP #include +#include + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "filters/filter_base.hpp" #include "filters/filter_chain.hpp" -#include #include "rclcpp/node.hpp" @@ -31,14 +32,11 @@ class LowPassFilter : public filters::FilterBase public: LowPassFilter(); - ~LowPassFilter(); + ~LowPassFilter() override; - /** @brief Configure filter parameters */ - virtual bool configure(); + bool configure() override; - bool configure(const std::string& ns); - - virtual bool update(const T& data_in, T& data_out); + bool update(const T& data_in, T& data_out) override; /** \brief Get most recent parameters */ bool updateParameters(); @@ -50,7 +48,6 @@ class LowPassFilter : public filters::FilterBase rclcpp::Logger logger_; // Parameters - std::string parameter_namespace_; double sampling_frequency_; double damping_frequency_; double damping_intensity_; @@ -81,21 +78,12 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() -{ - configure(""); - return true; -} - -template -bool LowPassFilter::configure(const std::string& ns) { if (!initialized_) { RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); return false; } - - parameter_namespace_ = ns; if (!updateParameters()) { @@ -184,19 +172,19 @@ template bool LowPassFilter::updateParameters() { - if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + if (!filters::FilterBase::getParam("divider", divider_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); return false; } From ea334d7ed903c3e8d6b6b6d45194d147a3881626 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:31:27 +0200 Subject: [PATCH 04/27] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2faa770..7d1fe18c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,6 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp - rclcpp_lifecycle rcutils realtime_tools ) From dc4623ae1b6948c09a9733648d6c72b3b27c28e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 12 Jun 2021 17:05:10 +0200 Subject: [PATCH 05/27] Added initial files of control filters --- README.md | 22 ++ .../control_filters/gravity_compensation.hpp | 222 ++++++++++++++++++ package.xml | 3 + src/control_filters/gravity_compensation.cpp | 20 ++ .../test_gravity_compensation.cpp | 42 ++++ .../test_gravity_compensation.hpp | 59 +++++ .../test_load_gravity_compensation.cpp | 41 ++++ 7 files changed, 409 insertions(+) create mode 100644 include/control_filters/gravity_compensation.hpp create mode 100644 src/control_filters/gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.hpp create mode 100644 test/control_filters/test_load_gravity_compensation.cpp diff --git a/README.md b/README.md index 21acbbc7..369c15c5 100644 --- a/README.md +++ b/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 + +This repository uses `pre-commit` tool for code formatting. +The tool checks formatting each time you commit to a repository. +To install it locally use: + ``` + pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) + ``` + +To run it initially over the whole repo you can use: + ``` + pre-commit run -a + ``` + +If you get error that something is missing on your computer, do the following for: + + - `clang-format-10` + ``` + sudo apt install clang-format-10 + ``` diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..7e4d7fcd --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,222 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/LinearMath/Transform.h" +#include "filters/filter_base.hpp" + +namespace control_filters +{ + template + class GravityCompensation : public filters::FilterBase + { + public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + virtual bool configure() override; + + /** \brief Update the filter and return the data seperately + * \param data_in T array with length width + * \param data_out T array with length width + */ + virtual bool update(const T& data_in, T& data_out) override; + + /** \brief Get most recent parameters */ + bool updateParameters(); + + private: + /** \brief Dynamic parameter callback activated when parameters change */ + // void parameterCallback(); + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force + + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // tf2 objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; + + bool configured_; + + uint num_transform_errors_; + }; + + template + GravityCompensation::GravityCompensation() + : logger_(rclcpp::get_logger("GravityCompensation")) + , configured_(false) + , num_transform_errors_(0) + { + } + + template + GravityCompensation::~GravityCompensation() + { + } + + template + bool GravityCompensation::configure() + { + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + if(!updateParameters()){ + return false; + } + else{ + configured_ = true; + } + RCLCPP_INFO(logger_, + "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " + "CoG y:%f; CoG z:%f; force: %f.", + world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); + + return true; + } + + template + bool GravityCompensation::update(const T& data_in, T& data_out) + { + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + //if (params_updated_) + //{ + // updateParameters(); + //} + + try + { + transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); + transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); + transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); + } + + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + + temp_vector_in.vector = data_in.wrench.force; + tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + + temp_vector_in.vector = data_in.wrench.torque; + tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + + // Transform CoG Vector + geometry_msgs::msg::Vector3Stamped cog_transformed; + tf2::doTransform(cog_, cog_transformed, transform_cog_); + + // Compensate for gravity force + temp_force_transformed.vector.z += force_z_; + // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) + temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); + + // Copy Message and Compensate values for Gravity Force and Resulting Torque + // geometry_msgs::WrenchStamped compensated_wrench; + data_out = data_in; + + tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); + data_out.wrench.force = temp_vector_out.vector; + + tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); + data_out.wrench.torque = temp_vector_out.vector; + + return true; + } + + template + bool GravityCompensation::updateParameters() + { + //params_updated_ = false; + + if (!filters::FilterBase::getParam("world_frame", world_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param world_frame_"); + return false; + } + if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param sensor_frame"); + return false; + } + if (!filters::FilterBase::getParam("force_frame", force_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force_frame"); + return false; + } + if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_x"); + return false; + } + if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_y"); + return false; + } + if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_z"); + return false; + } + if (!filters::FilterBase::getParam("force", force_z_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force"); + return false; + } + return true; + } + + //template + //void GravityCompensation::parameterCallback() + //{ + // params_updated_ = true; + //} + +} // namespace iirob_filters +#endif diff --git a/package.xml b/package.xml index 52c9b467..64acfcd9 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,9 @@ ament_cmake control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..6c38439f --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "control_filters/gravity_compensation.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, + filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp new file mode 100644 index 00000000..ae9d5f05 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_gravity_compensation.hpp" + +TEST_F(GravityCompensationTest, TestGravityCompensation) +{ + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("CoG_x", 0.0); + node_->declare_parameter("CoG_y", 0.0); + node_->declare_parameter("CoG_z", 0.0); + node_->declare_parameter("force", 50.0); + + ASSERT_TRUE(gravity_compensation_.configure()); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + ASSERT_TRUE(gravity_compensation_.update(in, out)); + + ASSERT_EQ(out.wrench.force.x, 1.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 50.0); + + ASSERT_EQ(out.wrench.torque.x, 10.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp new file mode 100644 index 00000000..a614ebb6 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" +#include + +#include "control_filters/gravity_compensation.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); +} // namespace + +// TODO(destogl): do this +// subclassing and friending so we can access member variables + + +class GravityCompensationTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + GravityCompensationTest() + : node_(std::make_shared("test_gravity_compensation")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::GravityCompensation gravity_compensation_; + std::thread executor_thread_; +}; diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp new file mode 100644 index 00000000..22f66c1e --- /dev/null +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// #include +// +// #include +// +// #include "controller_manager/controller_manager.hpp" +// #include "hardware_interface/resource_manager.hpp" +// #include "rclcpp/executor.hpp" +// #include "rclcpp/executors/single_threaded_executor.hpp" +// #include "rclcpp/utilities.hpp" +// #include "ros2_control_test_assets/descriptions.hpp" +// +// TEST(TestLoadAdmittanceController, load_controller) +// { +// rclcpp::init(0, nullptr); +// +// std::shared_ptr executor = +// std::make_shared(); +// +// controller_manager::ControllerManager cm(std::make_unique( +// ros2_control_test_assets::minimal_robot_urdf), +// executor, "test_controller_manager"); +// +// ASSERT_NO_THROW( +// cm.load_controller( +// "test_admittance_controller", +// "admittance_controller/AdmittanceController")); +// } From 096ea4f79b374aa14324ac803a2c27e6b7411846 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:31:27 +0200 Subject: [PATCH 06/27] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- CMakeLists.txt | 4 ---- 1 file changed, 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d1fe18c..48a66a58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,11 +73,7 @@ ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(rclcpp_lifecycle REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) - - ament_lint_auto_find_test_dependencies() ament_add_gmock(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests control_toolbox) From b764379667566d3ed1b72cd97c485680f6a98e39 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 7 Mar 2023 15:49:39 +0100 Subject: [PATCH 07/27] Apply formatting to filters. # Conflicts: # include/control_filters/gravity_compensation.hpp # src/control_filters/gravity_compensation.cpp --- .../control_filters/gravity_compensation.hpp | 222 -------------- include/control_filters/low_pass_filter.hpp | 275 +++++++++--------- src/control_filters/gravity_compensation.cpp | 20 -- src/control_filters/low_pass_filter.cpp | 6 +- 4 files changed, 145 insertions(+), 378 deletions(-) delete mode 100644 include/control_filters/gravity_compensation.hpp delete mode 100644 src/control_filters/gravity_compensation.cpp diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp deleted file mode 100644 index 7e4d7fcd..00000000 --- a/include/control_filters/gravity_compensation.hpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP -#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP - -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "geometry_msgs/msg/vector3_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/transform_listener.h" -#include "tf2/LinearMath/Transform.h" -#include "filters/filter_base.hpp" - -namespace control_filters -{ - template - class GravityCompensation : public filters::FilterBase - { - public: - /** \brief Constructor */ - GravityCompensation(); - - /** \brief Destructor */ - ~GravityCompensation(); - - /** @brief Configure filter parameters */ - virtual bool configure() override; - - /** \brief Update the filter and return the data seperately - * \param data_in T array with length width - * \param data_out T array with length width - */ - virtual bool update(const T& data_in, T& data_out) override; - - /** \brief Get most recent parameters */ - bool updateParameters(); - - private: - /** \brief Dynamic parameter callback activated when parameters change */ - // void parameterCallback(); - - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - // Storage for Calibration Values - geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) - double force_z_; // Gravitational Force - - // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; - std::string sensor_frame_; - std::string force_frame_; - - // tf2 objects - std::unique_ptr p_tf_Buffer_; - std::unique_ptr p_tf_Listener_; - geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; - - bool configured_; - - uint num_transform_errors_; - }; - - template - GravityCompensation::GravityCompensation() - : logger_(rclcpp::get_logger("GravityCompensation")) - , configured_(false) - , num_transform_errors_(0) - { - } - - template - GravityCompensation::~GravityCompensation() - { - } - - template - bool GravityCompensation::configure() - { - clock_ = std::make_shared(RCL_SYSTEM_TIME); - p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); - p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); - - if(!updateParameters()){ - return false; - } - else{ - configured_ = true; - } - RCLCPP_INFO(logger_, - "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " - "CoG y:%f; CoG z:%f; force: %f.", - world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); - - return true; - } - - template - bool GravityCompensation::update(const T& data_in, T& data_out) - { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } - - //if (params_updated_) - //{ - // updateParameters(); - //} - - try - { - transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); - transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); - transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); - } - - geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; - - temp_vector_in.vector = data_in.wrench.force; - tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); - - temp_vector_in.vector = data_in.wrench.torque; - tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); - - // Transform CoG Vector - geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(cog_, cog_transformed, transform_cog_); - - // Compensate for gravity force - temp_force_transformed.vector.z += force_z_; - // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) - temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); - temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); - - // Copy Message and Compensate values for Gravity Force and Resulting Torque - // geometry_msgs::WrenchStamped compensated_wrench; - data_out = data_in; - - tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); - data_out.wrench.force = temp_vector_out.vector; - - tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); - data_out.wrench.torque = temp_vector_out.vector; - - return true; - } - - template - bool GravityCompensation::updateParameters() - { - //params_updated_ = false; - - if (!filters::FilterBase::getParam("world_frame", world_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param world_frame_"); - return false; - } - if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param sensor_frame"); - return false; - } - if (!filters::FilterBase::getParam("force_frame", force_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param force_frame"); - return false; - } - if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_x"); - return false; - } - if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_y"); - return false; - } - if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_z"); - return false; - } - if (!filters::FilterBase::getParam("force", force_z_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param force"); - return false; - } - return true; - } - - //template - //void GravityCompensation::parameterCallback() - //{ - // params_updated_ = true; - //} - -} // namespace iirob_filters -#endif diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 5ad04519..c7c19895 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -12,62 +12,64 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP -#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP +#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ -#include #include +#include -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "filters/filter_base.hpp" #include "filters/filter_chain.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/node.hpp" - namespace control_filters { template class LowPassFilter : public filters::FilterBase { public: - LowPassFilter(); + LowPassFilter(); - ~LowPassFilter() override; + ~LowPassFilter() override; - bool configure() override; + bool configure() override; - bool update(const T& data_in, T& data_out) override; + bool update(const T & data_in, T & data_out) override; - /** \brief Get most recent parameters */ - bool updateParameters(); + /** \brief Get most recent parameters */ + bool updateParameters(); private: - /** \brief Dynamic parameter callback activated when parameters change */ - void parameterCallback(); - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - - // Parameters - double sampling_frequency_; - double damping_frequency_; - double damping_intensity_; - int divider_; - - // Filter parameters - double b1; - double a1; - double filtered_value, filtered_old_value, old_value; - Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - - // dynamic parameters - bool params_updated_; - bool initialized_; - bool configured_; + /** \brief Dynamic parameter callback activated when parameters change */ + void parameterCallback(); + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Parameters + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + int divider_; + + // Filter parameters + double b1; + double a1; + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // dynamic parameters + bool params_updated_; + bool initialized_; + bool configured_; }; template LowPassFilter::LowPassFilter() - : logger_(rclcpp::get_logger("LowPassFilter")), params_updated_(false), initialized_(false), configured_(false) +: logger_(rclcpp::get_logger("LowPassFilter")), + params_updated_(false), + initialized_(false), + configured_(false) { } @@ -79,127 +81,134 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - if (!initialized_) - { - RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); - return false; - } - - if (!updateParameters()) - { - return false; - } - - configured_ = true; - - RCLCPP_INFO(logger_, - "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " - "divider :%d", - sampling_frequency_, damping_frequency_, damping_intensity_, divider_); - - // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = 0; - for (unsigned int i = 0; i < 6; i++) - { - msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; - } - - return true; + if (!initialized_) + { + RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); + return false; + } + + if (!updateParameters()) + { + return false; + } + + configured_ = true; + + RCLCPP_INFO( + logger_, + "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " + "divider :%d", + sampling_frequency_, damping_frequency_, damping_intensity_, divider_); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + for (unsigned int i = 0; i < 6; i++) + { + msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; + } + + return true; } template <> -inline bool LowPassFilter::update(const geometry_msgs::msg::WrenchStamped& data_in, - geometry_msgs::msg::WrenchStamped& data_out) +inline bool LowPassFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } - - if (params_updated_) - { - updateParameters(); - } - - // IIR Filter - msg_filtered = b1 * msg_old + a1 * msg_filtered_old; - msg_filtered_old = msg_filtered; - - // TODO use wrenchMsgToEigen - msg_old[0] = data_in.wrench.force.x; - msg_old[1] = data_in.wrench.force.y; - msg_old[2] = data_in.wrench.force.z; - msg_old[3] = data_in.wrench.torque.x; - msg_old[4] = data_in.wrench.torque.y; - msg_old[5] = data_in.wrench.torque.z; - - data_out.wrench.force.x = msg_filtered[0]; - data_out.wrench.force.y = msg_filtered[1]; - data_out.wrench.force.z = msg_filtered[2]; - data_out.wrench.torque.x = msg_filtered[3]; - data_out.wrench.torque.y = msg_filtered[4]; - data_out.wrench.torque.z = msg_filtered[5]; - return true; + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + updateParameters(); + } + + // IIR Filter + msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO(destogl) use wrenchMsgToEigen + msg_old[0] = data_in.wrench.force.x; + msg_old[1] = data_in.wrench.force.y; + msg_old[2] = data_in.wrench.force.z; + msg_old[3] = data_in.wrench.torque.x; + msg_old[4] = data_in.wrench.torque.y; + msg_old[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = msg_filtered[0]; + data_out.wrench.force.y = msg_filtered[1]; + data_out.wrench.force.z = msg_filtered[2]; + data_out.wrench.torque.x = msg_filtered[3]; + data_out.wrench.torque.y = msg_filtered[4]; + data_out.wrench.torque.z = msg_filtered[5]; + return true; } template -bool LowPassFilter::update(const T& data_in, T& data_out) +bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } - - if (params_updated_) + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + if (!updateParameters()) { - if (!updateParameters()) - { - RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); - return false; - } + RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); + return false; } + } - data_out = b1 * old_value + a1 * filtered_old_value; - filtered_old_value = data_out; - old_value = data_in; + data_out = b1 * old_value + a1 * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; - return true; + return true; } template bool LowPassFilter::updateParameters() { - - if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); - return false; - } - if (!filters::FilterBase::getParam("divider", divider_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); - return false; - } - - params_updated_ = false; - a1 = exp(-1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / (pow(10.0, damping_intensity_ / -10.0))); - b1 = 1.0 - a1; - return true; + if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); + return false; + } + if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); + return false; + } + if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); + return false; + } + if (!filters::FilterBase::getParam("divider", divider_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); + return false; + } + + params_updated_ = false; + a1 = exp( + -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / + (pow(10.0, damping_intensity_ / -10.0))); + b1 = 1.0 - a1; + return true; } template void LowPassFilter::parameterCallback() { - params_updated_ = true; + params_updated_ = true; } -} // namespace iirob_filters -#endif +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp deleted file mode 100644 index 6c38439f..00000000 --- a/src/control_filters/gravity_compensation.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "control_filters/gravity_compensation.hpp" - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, - filters::FilterBase) diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index 56142344..3aba0735 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -16,6 +16,6 @@ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, - filters::FilterBase) - +PLUGINLIB_EXPORT_CLASS( + control_filters::LowPassFilter, + filters::FilterBase) From 581f47442979a5dce008f60847b11eb5d6ba02db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 30 Oct 2021 12:11:50 +0200 Subject: [PATCH 08/27] Update filters to use full impl of parameter hanlder. --- CMakeLists.txt | 25 ++- control_filters.xml | 21 +- include/control_filters/low_pass_filter.hpp | 208 +++++++++--------- src/control_filters/low_pass_filter.cpp | 2 + .../test_gravity_compensation.cpp | 42 ---- .../test_gravity_compensation.hpp | 59 ----- .../test_load_low_pass_filter.cpp | 44 ++-- test/control_filters/test_low_pass_filter.hpp | 16 +- 8 files changed, 188 insertions(+), 229 deletions(-) delete mode 100644 test/control_filters/test_gravity_compensation.cpp delete mode 100644 test/control_filters/test_gravity_compensation.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 48a66a58..b4b13f9c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,23 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") +######################## +# Parameter handler +######################## +add_library(parameter_handler SHARED + src/parameter_handler.cpp +) + +target_include_directories(parameter_handler PUBLIC + $ + $ +) +ament_target_dependencies(parameter_handler + rcl_interfaces + rclcpp + rcutils +) + ######################## # Build control filters ######################## @@ -60,11 +77,14 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS add_library(low_pass_filter SHARED src/control_filters/low_pass_filter.cpp ) + target_include_directories(low_pass_filter PUBLIC $ $ $ ) +#target_include_directories(low_pass_filter PUBLIC include) +target_link_libraries(low_pass_filter parameter_handler) ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ########## @@ -87,7 +107,7 @@ if(BUILD_TESTING) ## Control Filters ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) - target_link_libraries(test_low_pass_filter low_pass_filter) + target_link_libraries(test_low_pass_filter low_pass_filter parameter_handler) ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() @@ -96,7 +116,7 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter +install(TARGETS control_toolbox low_pass_filter parameter_handler EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -111,5 +131,6 @@ ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCL ament_export_libraries( control_toolbox low_pass_filter + parameter_handler ) ament_package() diff --git a/control_filters.xml b/control_filters.xml index 824df79c..7a1924b0 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,11 +1,18 @@ - - - This is a low pass filter working with geometry_msgs::WrenchStamped. - - + + + This is a low pass filter working with a double value. + + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index c7c19895..cf99fcac 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -17,14 +17,88 @@ #include #include +#include +#include +#include +#include "control_toolbox/parameter_handler.hpp" #include "filters/filter_base.hpp" -#include "filters/filter_chain.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp/node.hpp" namespace control_filters { +class LowPassParameters : public control_toolbox::ParameterHandler +{ +public: + explicit LowPassParameters(const std::string & params_prefix) + : control_toolbox::ParameterHandler(params_prefix, 0, 1, 3) + { + add_double_parameter("sampling_frequency", false); + add_double_parameter("damping_frequency", false); + add_double_parameter("damping_intensity", false); + + add_integer_parameter("divider", false); + } + + bool check_if_parameters_are_valid() override + { + bool ret = true; + + // Check if any string parameter is empty + ret = !empty_parameter_in_list(string_parameters_); + + for (size_t i = 0; i < 3; ++i) + { + if (std::isnan(double_parameters_[i].second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be set", + double_parameters_[i].first.name.c_str()); + ret = false; + } + } + + if (integer_parameters_[0].second < 0) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be positive", + integer_parameters_[0].first.name.c_str()); + } + + return ret; + } + + void update_storage() override + { + sampling_frequency_ = double_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sampling frequency is %e", sampling_frequency_); + damping_frequency_ = double_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping frequency is %e", damping_frequency_); + damping_intensity_ = double_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping intensity is %e", damping_intensity_); + + divider_ = integer_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Divider %d", divider_); + + a1_ = exp( + -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / + (pow(10.0, damping_intensity_ / -10.0))); + b1_ = 1.0 - a1_; + } + + // Parameters from parameter server + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + + int divider_; + + // Filter Parameters + double a1_; + double b1_; +}; + template class LowPassFilter : public filters::FilterBase { @@ -37,39 +111,22 @@ class LowPassFilter : public filters::FilterBase bool update(const T & data_in, T & data_out) override; - /** \brief Get most recent parameters */ - bool updateParameters(); - private: - /** \brief Dynamic parameter callback activated when parameters change */ - void parameterCallback(); - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::unique_ptr parameters_; - // Parameters - double sampling_frequency_; - double damping_frequency_; - double damping_intensity_; - int divider_; + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; // Filter parameters - double b1; - double a1; + // TODO(destogl): we should do this more intelligently using only one set of types double filtered_value, filtered_old_value, old_value; Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - - // dynamic parameters - bool params_updated_; - bool initialized_; - bool configured_; }; template LowPassFilter::LowPassFilter() -: logger_(rclcpp::get_logger("LowPassFilter")), - params_updated_(false), - initialized_(false), - configured_(false) { } @@ -81,32 +138,34 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - if (!initialized_) - { - RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); - return false; - } + clock_ = std::make_shared(RCL_SYSTEM_TIME); + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + parameters_.reset(new LowPassParameters(this->param_prefix_)); + + parameters_->initialize(this->params_interface_, logger_->get_name()); - if (!updateParameters()) + parameters_->declare_parameters(); + + if (!parameters_->get_parameters()) { return false; } - configured_ = true; - - RCLCPP_INFO( - logger_, - "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " - "divider :%d", - sampling_frequency_, damping_frequency_, damping_intensity_, divider_); - // Initialize storage Vectors filtered_value = filtered_old_value = old_value = 0; - for (unsigned int i = 0; i < 6; i++) + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (size_t 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] = 0; } + // Add callback to dynamically update parameters + on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + return parameters_->set_parameter_callback(parameters); + }); + return true; } @@ -114,22 +173,19 @@ template <> inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!configured_) + if (!this->configured_) { - RCLCPP_ERROR(logger_, "Filter is not configured"); + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - if (params_updated_) - { - updateParameters(); - } + parameters_->update(); // IIR Filter - msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered = parameters_->b1_ * msg_old + parameters_->a1_ * msg_filtered_old; msg_filtered_old = msg_filtered; - // TODO(destogl) use wrenchMsgToEigen + // TODO(destogl): use wrenchMsgToEigen msg_old[0] = data_in.wrench.force.x; msg_old[1] = data_in.wrench.force.y; msg_old[2] = data_in.wrench.force.z; @@ -149,66 +205,22 @@ inline bool LowPassFilter::update( template bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!configured_) + if (!this->configured_) { - RCLCPP_ERROR(logger_, "Filter is not configured"); + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - if (params_updated_) - { - if (!updateParameters()) - { - RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); - return false; - } - } + parameters_->update(); - data_out = b1 * old_value + a1 * filtered_old_value; + // Filter + data_out = parameters_->b1_ * old_value + parameters_->a1_ * filtered_old_value; filtered_old_value = data_out; old_value = data_in; return true; } -template -bool LowPassFilter::updateParameters() -{ - if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); - return false; - } - if (!filters::FilterBase::getParam("divider", divider_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); - return false; - } - - params_updated_ = false; - a1 = exp( - -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / - (pow(10.0, damping_intensity_ / -10.0))); - b1 = 1.0 - a1; - return true; -} - -template -void LowPassFilter::parameterCallback() -{ - params_updated_ = true; -} - } // namespace control_filters #endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index 3aba0735..ccd5fe2b 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -16,6 +16,8 @@ #include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) + PLUGINLIB_EXPORT_CLASS( control_filters::LowPassFilter, filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp deleted file mode 100644 index ae9d5f05..00000000 --- a/test/control_filters/test_gravity_compensation.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "test_gravity_compensation.hpp" - -TEST_F(GravityCompensationTest, TestGravityCompensation) -{ - node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("CoG_x", 0.0); - node_->declare_parameter("CoG_y", 0.0); - node_->declare_parameter("CoG_z", 0.0); - node_->declare_parameter("force", 50.0); - - ASSERT_TRUE(gravity_compensation_.configure()); - - geometry_msgs::msg::WrenchStamped in, out; - in.header.frame_id = "world"; - in.wrench.force.x = 1.0; - in.wrench.torque.x = 10.0; - - ASSERT_TRUE(gravity_compensation_.update(in, out)); - - ASSERT_EQ(out.wrench.force.x, 1.0); - ASSERT_EQ(out.wrench.force.y, 0.0); - ASSERT_EQ(out.wrench.force.z, 50.0); - - ASSERT_EQ(out.wrench.torque.x, 10.0); - ASSERT_EQ(out.wrench.torque.y, 0.0); - ASSERT_EQ(out.wrench.torque.z, 0.0); -} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp deleted file mode 100644 index a614ebb6..00000000 --- a/test/control_filters/test_gravity_compensation.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gmock/gmock.h" -#include - -#include "control_filters/gravity_compensation.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); -} // namespace - -// TODO(destogl): do this -// subclassing and friending so we can access member variables - - -class GravityCompensationTest : public ::testing::Test -{ -public: - void SetUp() override - { - executor_->add_node(node_); - executor_thread_ = std::thread([this]() { executor_->spin(); }); - } - - GravityCompensationTest() - : node_(std::make_shared("test_gravity_compensation")) - , executor_(std::make_shared()) - { - } - - void TearDown() override - { - executor_->cancel(); - if (executor_thread_.joinable()) - { - executor_thread_.join(); - } - } - -protected: - rclcpp::Node::SharedPtr node_; - rclcpp::Executor::SharedPtr executor_; - control_filters::GravityCompensation gravity_compensation_; - std::thread executor_thread_; -}; diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index 47ac7bad..8be57b2c 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -1,16 +1,16 @@ -Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include @@ -34,10 +34,24 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_low_pass_filter", "controlFilters/LowPassFilter")); + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilter")); rclcpp::shutdown(); } +TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilterWrench")); + rclcpp::shutdown(); +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index 124616fa..c2dd4e27 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -1,5 +1,3 @@ - - // Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -14,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ + +#include #include +#include "gmock/gmock.h" #include "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/rclcpp.hpp" namespace @@ -25,7 +28,6 @@ namespace static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); } // namespace - class LowPassFilterTest : public ::testing::Test { public: @@ -36,8 +38,8 @@ class LowPassFilterTest : public ::testing::Test } LowPassFilterTest() - : node_(std::make_shared("test_low_pass_filter")) - , executor_(std::make_shared()) + : node_(std::make_shared("test_low_pass_filter")), + executor_(std::make_shared()) { } @@ -56,3 +58,5 @@ class LowPassFilterTest : public ::testing::Test control_filters::LowPassFilter low_pass_filter_; std::thread executor_thread_; }; + +#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ From f8caa76115de4a2e3e1925c91c380abf6275da4a Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 7 Mar 2023 16:13:06 +0100 Subject: [PATCH 09/27] Fixed PR115 especially for humble 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 --- CMakeLists.txt | 1 + include/control_filters/low_pass_filter.hpp | 3 +- include/control_toolbox/parameter_handler.hpp | 387 ++++++++++++++++++ package.xml | 1 + src/parameter_handler.cpp | 216 ++++++++++ test/control_filters/test_low_pass_filter.cpp | 30 +- test/control_filters/test_low_pass_filter.hpp | 12 +- 7 files changed, 631 insertions(+), 19 deletions(-) create mode 100644 include/control_toolbox/parameter_handler.hpp create mode 100644 src/parameter_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b4b13f9c..afbd2751 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp + rcl_interfaces rcutils realtime_tools ) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index cf99fcac..4a156143 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -175,7 +175,8 @@ inline bool LowPassFilter::update( { if (!this->configured_) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } diff --git a/include/control_toolbox/parameter_handler.hpp b/include/control_toolbox/parameter_handler.hpp new file mode 100644 index 00000000..f38fd093 --- /dev/null +++ b/include/control_toolbox/parameter_handler.hpp @@ -0,0 +1,387 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ +#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +using rclcpp::ParameterType; + +namespace impl +{ +inline std::string normalize_params_prefix(std::string prefix) +{ + if (!prefix.empty()) + { + if ('.' != prefix.back()) + { + prefix += '.'; + } + } + return prefix; +} +} // namespace impl + +struct Parameter +{ + Parameter() = default; + + explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) + : name(name), type(type), configurable(configurable) + { + } + + std::string name; + uint8_t type; + bool configurable; +}; + +class ParameterHandler +{ +public: + ParameterHandler( + const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, + int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, + int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, + int nr_string_array_params = 0); + + virtual ~ParameterHandler() = default; + + void initialize(rclcpp::Node::SharedPtr node) + { + initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); + } + + void initialize( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, + const std::string & logger_name = "::ControllerParameters") + { + params_interface_ = node_params; + logger_name_ = logger_name + "." + "parameters"; + } + + void declare_parameters(); + + /** + * Gets all defined parameters from parameter server after parameters are declared. + * Optionally, check parameters' validity and update storage. + * + * \param[in] check_validity check validity after getting parameters (default: **true**). + * \param[in] update update parameters in storage after getting them (default: **true**). + * \return true if all parameters are read successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ + bool get_parameters(bool check_validity = true, bool update = true); + + /** + * Update all storage variables from the parameter maps. + * Parameters will be only updated if they are previously read from parameter server or dynamic + * reconfigure callback occurred. + * + * \param[in] check_validity check validity before updating parameters (default: **true**). + * \return is update was successful, i.e., parameters are valid. If \p check_validity is set + * **false**, the result is always **true**. + */ + bool update(bool check_validity = true); + + rcl_interfaces::msg::SetParametersResult set_parameter_callback( + const std::vector & parameters); + +protected: + /** + * Override this method to implement custom parameters check. + * Default implementation does not checks anything, always returns true. + * + * \return **true** if parameters are valid, **false** otherwise. + */ + virtual bool check_if_parameters_are_valid() { return true; } + + /** + * Mapping between parameter storage and implementation members. + * The order of parameter in storage is the order of adding parameters to the storage. + * E.g., to access the value of 5-th string parameter added to storage use: + * `fifth_string_param_ = string_parameters_[4].second; + * where `fifth_string_param_` is the name of the member of a child class. + */ + virtual void update_storage() = 0; + +protected: + void add_bool_parameter( + const std::string & name, const bool & configurable = false, const bool & default_value = false) + { + bool_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), + default_value}); + } + + void add_integer_parameter( + const std::string & name, const bool & configurable = false, + const int & default_value = std::numeric_limits::quiet_NaN()) + { + integer_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), + default_value}); + } + + void add_double_parameter( + const std::string & name, const bool & configurable = false, + const double & default_value = std::numeric_limits::quiet_NaN()) + { + double_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), + default_value}); + } + + void add_string_parameter( + const std::string & name, const bool & configurable = false, + const std::string & default_value = "") + { + string_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), + default_value}); + } + + void add_byte_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + byte_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), + default_value}); + } + + void add_bool_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + bool_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), + default_value}); + } + + void add_integer_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + integer_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), + default_value}); + } + void add_double_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + double_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), + default_value}); + } + + void add_string_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + string_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), + default_value}); + } + + template + bool empty_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (parameter.second.empty()) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + template + bool find_and_assign_parameter_value( + std::vector> & parameter_list, + const rclcpp::Parameter & input_parameter) + { + auto is_in_list = [&](const auto & parameter) { + return parameter.first.name == input_parameter.get_name(); + }; + + auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); + + if (it != parameter_list.end()) + { + if (!it->first.configurable) + { + throw std::runtime_error( + "Parameter " + input_parameter.get_name() + " is not configurable."); + } + else + { + it->second = input_parameter.get_value(); + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), + input_parameter.value_to_string().c_str()); + return true; + } + } + else + { + return false; + } + } + + // Storage members + std::vector> bool_parameters_; + std::vector> integer_parameters_; + std::vector> double_parameters_; + std::vector> string_parameters_; + std::vector>> byte_array_parameters_; + std::vector>> bool_array_parameters_; + std::vector>> integer_array_parameters_; + std::vector>> double_array_parameters_; + std::vector>> string_array_parameters_; + + // Functional members + bool declared_; + bool up_to_date_; + std::string params_prefix_; + + // Input/Output members to ROS 2 + std::string logger_name_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; + +private: + template + void declare_parameters_from_list( + rclcpp::Node::SharedPtr node, const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + node->declare_parameter(parameter.first.name, parameter.second); + } + } + + template + void declare_parameters_from_list(const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + // declare parameter only if it does not already exists + if (!params_interface_->has_parameter(parameter.first.name)) + { + rclcpp::ParameterValue default_parameter_value(parameter.second); + rcl_interfaces::msg::ParameterDescriptor descr; + descr.name = parameter.first.name; + descr.type = parameter.first.type; + descr.read_only = false; + + params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); + } + } + } + + template + bool empty_numeric_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (std::isnan(parameter.second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + template + bool get_parameters_from_list( + const rclcpp::Node::SharedPtr node, std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = node->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } + + template + bool get_parameters_from_list(std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = params_interface_->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } +}; + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/package.xml b/package.xml index 64acfcd9..c770a879 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ filters geometry_msgs pluginlib + rcl_interfaces rclcpp rcutils realtime_tools diff --git a/src/parameter_handler.cpp b/src/parameter_handler.cpp new file mode 100644 index 00000000..52ca742e --- /dev/null +++ b/src/parameter_handler.cpp @@ -0,0 +1,216 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#include "control_toolbox/parameter_handler.hpp" + +#include +#include + +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +ParameterHandler::ParameterHandler( + const std::string & params_prefix, int nr_bool_params, int nr_integer_params, + int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, + int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) +: declared_(false), up_to_date_(false), params_prefix_("") +{ + params_prefix_ = impl::normalize_params_prefix(params_prefix); + + bool_parameters_.reserve(nr_bool_params); + integer_parameters_.reserve(nr_integer_params); + double_parameters_.reserve(nr_double_params); + string_parameters_.reserve(nr_string_params); + byte_array_parameters_.reserve(nr_byte_array_params); + bool_array_parameters_.reserve(nr_bool_array_params); + integer_array_parameters_.reserve(nr_integer_array_params); + double_array_parameters_.reserve(nr_double_array_params); + string_array_parameters_.reserve(nr_string_array_params); +} + +void ParameterHandler::declare_parameters() +{ + if (!declared_) + { + declare_parameters_from_list(bool_parameters_); + declare_parameters_from_list(integer_parameters_); + declare_parameters_from_list(double_parameters_); + declare_parameters_from_list(string_parameters_); + declare_parameters_from_list(byte_array_parameters_); + declare_parameters_from_list(bool_array_parameters_); + declare_parameters_from_list(integer_array_parameters_); + declare_parameters_from_list(double_array_parameters_); + declare_parameters_from_list(string_array_parameters_); + + declared_ = true; + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), + "Parameters already declared. Declaration should be done only once. " + "Nothing bad will happen, but please correct your code-logic."); + } +} + +bool ParameterHandler::get_parameters(bool check_validity, bool update) +{ + // TODO(destogl): Should we "auto-declare" parameters here? + if (!declared_) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Can not get parameters. Please declare them first."); + return false; + } + + bool ret = false; + + // If parameters are updated using dynamic reconfigure callback then there is no need to read + // them again. To ignore multiple manual reads + ret = + get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && + get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && + get_parameters_from_list(byte_array_parameters_) && + get_parameters_from_list(bool_array_parameters_) && + get_parameters_from_list(integer_array_parameters_) && + get_parameters_from_list(double_array_parameters_) && + get_parameters_from_list(string_array_parameters_); + + if (ret && check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + // If it is all good until now the parameters are not up to date anymore + if (ret) + { + up_to_date_ = false; + } + + if (ret && update) + { + ret = this->update(false); + } + + return ret; +} + +bool ParameterHandler::update(bool check_validity) +{ + bool ret = true; + + // Let's make this efficient and execute code only if parameters are updated + if (!up_to_date_) + { + if (check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + if (ret) + { + this->update_storage(); + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); + } + // reset variable to update parameters only when this is needed + up_to_date_ = true; + } + + return ret; +} + +rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. + for (const auto & input_parameter : parameters) + { + bool found = false; + + try + { + found = find_and_assign_parameter_value(bool_parameters_, input_parameter); + if (!found) + { + found = find_and_assign_parameter_value(integer_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); + } + + RCUTILS_LOG_INFO_EXPRESSION_NAMED( + found, logger_name_.c_str(), + "Dynamic parameters got changed! Maybe you have to restart controller to update the " + "parameters internally."); + + if (found) + { + up_to_date_ = false; + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + catch (const std::runtime_error & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + } + + return result; +} + +} // namespace control_toolbox diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 5b6c517e..8682b118 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -21,7 +21,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) double damping_intensity = 1.25; int divider = 3; - bool a1, b1; + 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; @@ -29,24 +29,28 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) in.header.frame_id = "world"; in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; - - + + std::shared_ptr> filter_ = + std::make_shared>(); + + // WORKS BUT makes the next call to say parameter was already declared + // should deny configuration as parameters are not yet set + // ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", + // node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + //not yet configured, should deny update - ASSERT_FALSE(low_pass_filter_.update(in,out)); - - //should deny configuration as parameters are not yet set - ASSERT_FALSE(low_pass_filter_.configure()); + ASSERT_FALSE(filter_->update(in,out)); //declare parameters and configure node_->declare_parameter("sampling_frequency", sampling_freq); node_->declare_parameter("damping_frequency", damping_freq); node_->declare_parameter("damping_intensity", damping_intensity); node_->declare_parameter("divider", divider); - ASSERT_TRUE(low_pass_filter_.configure()); - - + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + //first filter pass, output should be zero as no old wrench was stored - ASSERT_TRUE(low_pass_filter_.update(in, out)); + 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); @@ -63,7 +67,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) calculated.wrench.torque.y = b1 * in.wrench.torque.y; calculated.wrench.torque.z = b1 * in.wrench.torque.z; //check equality with low-pass-filter - ASSERT_TRUE(low_pass_filter_.update(in, out)); + 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); @@ -80,7 +84,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) 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; //check equality with low-pass-filter - ASSERT_TRUE(low_pass_filter_.update(in, out)); + 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); diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index c2dd4e27..a2292ba8 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -33,14 +33,15 @@ class LowPassFilterTest : public ::testing::Test public: void SetUp() override { - executor_->add_node(node_); + executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } - LowPassFilterTest() - : node_(std::make_shared("test_low_pass_filter")), - executor_(std::make_shared()) + LowPassFilterTest() { + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_low_pass_filter"); + executor_ = std::make_shared(); } void TearDown() override @@ -50,12 +51,13 @@ class LowPassFilterTest : public ::testing::Test { executor_thread_.join(); } + node_.reset(); + rclcpp::shutdown(); } protected: rclcpp::Node::SharedPtr node_; rclcpp::Executor::SharedPtr executor_; - control_filters::LowPassFilter low_pass_filter_; std::thread executor_thread_; }; From a59795e8b4be7ac515c32267e6129eaba1885439 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 6 Mar 2023 17:11:18 +0100 Subject: [PATCH 10/27] Removed GravityCompensation --- .../test_load_gravity_compensation.cpp | 41 ------------------- 1 file changed, 41 deletions(-) delete mode 100644 test/control_filters/test_load_gravity_compensation.cpp diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp deleted file mode 100644 index 22f66c1e..00000000 --- a/test/control_filters/test_load_gravity_compensation.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// #include -// -// #include -// -// #include "controller_manager/controller_manager.hpp" -// #include "hardware_interface/resource_manager.hpp" -// #include "rclcpp/executor.hpp" -// #include "rclcpp/executors/single_threaded_executor.hpp" -// #include "rclcpp/utilities.hpp" -// #include "ros2_control_test_assets/descriptions.hpp" -// -// TEST(TestLoadAdmittanceController, load_controller) -// { -// rclcpp::init(0, nullptr); -// -// std::shared_ptr executor = -// std::make_shared(); -// -// controller_manager::ControllerManager cm(std::make_unique( -// ros2_control_test_assets::minimal_robot_urdf), -// executor, "test_controller_manager"); -// -// ASSERT_NO_THROW( -// cm.load_controller( -// "test_admittance_controller", -// "admittance_controller/AdmittanceController")); -// } From 9702d40aa3e29e84bf01453316e1441a92f1110e Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 7 Mar 2023 11:17:26 +0100 Subject: [PATCH 11/27] Switched to generate_parameter_library --- CMakeLists.txt | 33 +- include/control_filters/low_pass_filter.hpp | 135 ++---- include/control_toolbox/parameter_handler.hpp | 387 ------------------ package.xml | 2 +- .../low_pass_filter_parameters.yaml | 37 ++ src/parameter_handler.cpp | 216 ---------- test/control_filters/test_low_pass_filter.cpp | 139 ++++--- test/control_filters/test_low_pass_filter.hpp | 4 +- 8 files changed, 173 insertions(+), 780 deletions(-) delete mode 100644 include/control_toolbox/parameter_handler.hpp create mode 100644 src/control_filters/low_pass_filter_parameters.yaml delete mode 100644 src/parameter_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index afbd2751..5bd9e672 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,6 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp - rcl_interfaces rcutils realtime_tools ) @@ -42,33 +41,20 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") -######################## -# Parameter handler -######################## -add_library(parameter_handler SHARED - src/parameter_handler.cpp -) - -target_include_directories(parameter_handler PUBLIC - $ - $ -) -ament_target_dependencies(parameter_handler - rcl_interfaces - rclcpp - rcutils -) ######################## # Build control filters ######################## find_package(filters REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) set(CONTROL_FILTERS_INCLUDE_DEPENDS + Eigen3 + generate_parameter_library filters geometry_msgs pluginlib @@ -84,8 +70,11 @@ target_include_directories(low_pass_filter PUBLIC $ $ ) -#target_include_directories(low_pass_filter PUBLIC include) -target_link_libraries(low_pass_filter parameter_handler) +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}) ########## @@ -108,7 +97,7 @@ if(BUILD_TESTING) ## Control Filters ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) - target_link_libraries(test_low_pass_filter low_pass_filter parameter_handler) + target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters) ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() @@ -117,7 +106,7 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter parameter_handler +install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -132,6 +121,6 @@ ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCL ament_export_libraries( control_toolbox low_pass_filter - parameter_handler + low_pass_filter_parameters ) ament_package() diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 4a156143..11a100c0 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -21,83 +21,13 @@ #include #include -#include "control_toolbox/parameter_handler.hpp" +#include "low_pass_filter_parameters.hpp" #include "filters/filter_base.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_filters { -class LowPassParameters : public control_toolbox::ParameterHandler -{ -public: - explicit LowPassParameters(const std::string & params_prefix) - : control_toolbox::ParameterHandler(params_prefix, 0, 1, 3) - { - add_double_parameter("sampling_frequency", false); - add_double_parameter("damping_frequency", false); - add_double_parameter("damping_intensity", false); - - add_integer_parameter("divider", false); - } - - bool check_if_parameters_are_valid() override - { - bool ret = true; - - // Check if any string parameter is empty - ret = !empty_parameter_in_list(string_parameters_); - - for (size_t i = 0; i < 3; ++i) - { - if (std::isnan(double_parameters_[i].second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Parameter '%s' has to be set", - double_parameters_[i].first.name.c_str()); - ret = false; - } - } - - if (integer_parameters_[0].second < 0) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Parameter '%s' has to be positive", - integer_parameters_[0].first.name.c_str()); - } - - return ret; - } - - void update_storage() override - { - sampling_frequency_ = double_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sampling frequency is %e", sampling_frequency_); - damping_frequency_ = double_parameters_[1].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping frequency is %e", damping_frequency_); - damping_intensity_ = double_parameters_[2].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping intensity is %e", damping_intensity_); - - divider_ = integer_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Divider %d", divider_); - - a1_ = exp( - -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / - (pow(10.0, damping_intensity_ / -10.0))); - b1_ = 1.0 - a1_; - } - - // Parameters from parameter server - double sampling_frequency_; - double damping_frequency_; - double damping_intensity_; - - int divider_; - - // Filter Parameters - double a1_; - double b1_; -}; template class LowPassFilter : public filters::FilterBase @@ -111,22 +41,31 @@ class LowPassFilter : public filters::FilterBase bool update(const T & data_in, T & data_out) override; +protected: + void compute_internal_params() + { + a1_ = exp( + -1.0 / parameters_.sampling_frequency * (2.0 * M_PI * parameters_.damping_frequency) / + (pow(10.0, parameters_.damping_intensity / -10.0))); + b1_ = 1.0 - a1_; + }; + private: rclcpp::Clock::SharedPtr clock_; std::shared_ptr logger_; - std::unique_ptr parameters_; - - // Callback for updating dynamic parameters - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + std::shared_ptr parameter_handler_; + low_pass_filter::Params parameters_; // Filter parameters // TODO(destogl): we should do this more intelligently using only one set of types double filtered_value, filtered_old_value, old_value; Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + double a1_; + double b1_; }; template -LowPassFilter::LowPassFilter() +LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) { } @@ -141,16 +80,24 @@ bool LowPassFilter::configure() clock_ = std::make_shared(RCL_SYSTEM_TIME); logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - parameters_.reset(new LowPassParameters(this->param_prefix_)); - - parameters_->initialize(this->params_interface_, logger_->get_name()); - parameters_->declare_parameters(); - - if (!parameters_->get_parameters()) + // Initialize the parameters + try { + parameter_handler_ = std::make_shared(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); return false; } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); // Initialize storage Vectors filtered_value = filtered_old_value = old_value = 0; @@ -160,12 +107,6 @@ bool LowPassFilter::configure() msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; } - // Add callback to dynamically update parameters - on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( - [this](const std::vector & parameters) { - return parameters_->set_parameter_callback(parameters); - }); - return true; } @@ -180,10 +121,15 @@ inline bool LowPassFilter::update( return false; } - parameters_->update(); + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + } // IIR Filter - msg_filtered = parameters_->b1_ * msg_old + parameters_->a1_ * msg_filtered_old; + msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; msg_filtered_old = msg_filtered; // TODO(destogl): use wrenchMsgToEigen @@ -212,10 +158,15 @@ bool LowPassFilter::update(const T & data_in, T & data_out) return false; } - parameters_->update(); + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + } // Filter - data_out = parameters_->b1_ * old_value + parameters_->a1_ * filtered_old_value; + data_out = b1_ * old_value + a1_ * filtered_old_value; filtered_old_value = data_out; old_value = data_in; diff --git a/include/control_toolbox/parameter_handler.hpp b/include/control_toolbox/parameter_handler.hpp deleted file mode 100644 index f38fd093..00000000 --- a/include/control_toolbox/parameter_handler.hpp +++ /dev/null @@ -1,387 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ -#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -using rclcpp::ParameterType; - -namespace impl -{ -inline std::string normalize_params_prefix(std::string prefix) -{ - if (!prefix.empty()) - { - if ('.' != prefix.back()) - { - prefix += '.'; - } - } - return prefix; -} -} // namespace impl - -struct Parameter -{ - Parameter() = default; - - explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) - : name(name), type(type), configurable(configurable) - { - } - - std::string name; - uint8_t type; - bool configurable; -}; - -class ParameterHandler -{ -public: - ParameterHandler( - const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, - int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, - int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, - int nr_string_array_params = 0); - - virtual ~ParameterHandler() = default; - - void initialize(rclcpp::Node::SharedPtr node) - { - initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); - } - - void initialize( - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, - const std::string & logger_name = "::ControllerParameters") - { - params_interface_ = node_params; - logger_name_ = logger_name + "." + "parameters"; - } - - void declare_parameters(); - - /** - * Gets all defined parameters from parameter server after parameters are declared. - * Optionally, check parameters' validity and update storage. - * - * \param[in] check_validity check validity after getting parameters (default: **true**). - * \param[in] update update parameters in storage after getting them (default: **true**). - * \return true if all parameters are read successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ - bool get_parameters(bool check_validity = true, bool update = true); - - /** - * Update all storage variables from the parameter maps. - * Parameters will be only updated if they are previously read from parameter server or dynamic - * reconfigure callback occurred. - * - * \param[in] check_validity check validity before updating parameters (default: **true**). - * \return is update was successful, i.e., parameters are valid. If \p check_validity is set - * **false**, the result is always **true**. - */ - bool update(bool check_validity = true); - - rcl_interfaces::msg::SetParametersResult set_parameter_callback( - const std::vector & parameters); - -protected: - /** - * Override this method to implement custom parameters check. - * Default implementation does not checks anything, always returns true. - * - * \return **true** if parameters are valid, **false** otherwise. - */ - virtual bool check_if_parameters_are_valid() { return true; } - - /** - * Mapping between parameter storage and implementation members. - * The order of parameter in storage is the order of adding parameters to the storage. - * E.g., to access the value of 5-th string parameter added to storage use: - * `fifth_string_param_ = string_parameters_[4].second; - * where `fifth_string_param_` is the name of the member of a child class. - */ - virtual void update_storage() = 0; - -protected: - void add_bool_parameter( - const std::string & name, const bool & configurable = false, const bool & default_value = false) - { - bool_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), - default_value}); - } - - void add_integer_parameter( - const std::string & name, const bool & configurable = false, - const int & default_value = std::numeric_limits::quiet_NaN()) - { - integer_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), - default_value}); - } - - void add_double_parameter( - const std::string & name, const bool & configurable = false, - const double & default_value = std::numeric_limits::quiet_NaN()) - { - double_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), - default_value}); - } - - void add_string_parameter( - const std::string & name, const bool & configurable = false, - const std::string & default_value = "") - { - string_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), - default_value}); - } - - void add_byte_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - byte_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), - default_value}); - } - - void add_bool_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - bool_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), - default_value}); - } - - void add_integer_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - integer_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), - default_value}); - } - void add_double_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - double_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), - default_value}); - } - - void add_string_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - string_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), - default_value}); - } - - template - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (parameter.second.empty()) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - template - bool find_and_assign_parameter_value( - std::vector> & parameter_list, - const rclcpp::Parameter & input_parameter) - { - auto is_in_list = [&](const auto & parameter) { - return parameter.first.name == input_parameter.get_name(); - }; - - auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); - - if (it != parameter_list.end()) - { - if (!it->first.configurable) - { - throw std::runtime_error( - "Parameter " + input_parameter.get_name() + " is not configurable."); - } - else - { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), - input_parameter.value_to_string().c_str()); - return true; - } - } - else - { - return false; - } - } - - // Storage members - std::vector> bool_parameters_; - std::vector> integer_parameters_; - std::vector> double_parameters_; - std::vector> string_parameters_; - std::vector>> byte_array_parameters_; - std::vector>> bool_array_parameters_; - std::vector>> integer_array_parameters_; - std::vector>> double_array_parameters_; - std::vector>> string_array_parameters_; - - // Functional members - bool declared_; - bool up_to_date_; - std::string params_prefix_; - - // Input/Output members to ROS 2 - std::string logger_name_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; - -private: - template - void declare_parameters_from_list( - rclcpp::Node::SharedPtr node, const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - node->declare_parameter(parameter.first.name, parameter.second); - } - } - - template - void declare_parameters_from_list(const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - // declare parameter only if it does not already exists - if (!params_interface_->has_parameter(parameter.first.name)) - { - rclcpp::ParameterValue default_parameter_value(parameter.second); - rcl_interfaces::msg::ParameterDescriptor descr; - descr.name = parameter.first.name; - descr.type = parameter.first.type; - descr.read_only = false; - - params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); - } - } - } - - template - bool empty_numeric_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (std::isnan(parameter.second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - template - bool get_parameters_from_list( - const rclcpp::Node::SharedPtr node, std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = node->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } - - template - bool get_parameters_from_list(std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = params_interface_->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } -}; - -} // namespace control_toolbox - -#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/package.xml b/package.xml index c770a879..a6ce7c41 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,7 @@ filters geometry_msgs pluginlib - rcl_interfaces + generate_parameter_library rclcpp rcutils realtime_tools diff --git a/src/control_filters/low_pass_filter_parameters.yaml b/src/control_filters/low_pass_filter_parameters.yaml new file mode 100644 index 00000000..1735f631 --- /dev/null +++ b/src/control_filters/low_pass_filter_parameters.yaml @@ -0,0 +1,37 @@ +low_pass_filter: + sampling_frequency: { + type: double, + # default_value: 1000.0, + description: "Sampling frequency", + validation: { + gt<>: [0.0] + }, + } + damping_frequency: { + type: double, + # default_value: 20.5, + description: "Damping frequency", + #validation: { + # gt<>: [0.0] + #}, + } + damping_intensity: { + type: double, + # default_value: 1.25, + description: "Damping intensity", + #validation: { + # gt<>: [0.0] + #}, + } + + #divider: + #{ + # type: int, + # # default_value: 3, + # description: "Divider", + # validation: + # { + # gt_eq<>: [0] + # }, + # read_only: true + #} diff --git a/src/parameter_handler.cpp b/src/parameter_handler.cpp deleted file mode 100644 index 52ca742e..00000000 --- a/src/parameter_handler.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#include "control_toolbox/parameter_handler.hpp" - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -ParameterHandler::ParameterHandler( - const std::string & params_prefix, int nr_bool_params, int nr_integer_params, - int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, - int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) -: declared_(false), up_to_date_(false), params_prefix_("") -{ - params_prefix_ = impl::normalize_params_prefix(params_prefix); - - bool_parameters_.reserve(nr_bool_params); - integer_parameters_.reserve(nr_integer_params); - double_parameters_.reserve(nr_double_params); - string_parameters_.reserve(nr_string_params); - byte_array_parameters_.reserve(nr_byte_array_params); - bool_array_parameters_.reserve(nr_bool_array_params); - integer_array_parameters_.reserve(nr_integer_array_params); - double_array_parameters_.reserve(nr_double_array_params); - string_array_parameters_.reserve(nr_string_array_params); -} - -void ParameterHandler::declare_parameters() -{ - if (!declared_) - { - declare_parameters_from_list(bool_parameters_); - declare_parameters_from_list(integer_parameters_); - declare_parameters_from_list(double_parameters_); - declare_parameters_from_list(string_parameters_); - declare_parameters_from_list(byte_array_parameters_); - declare_parameters_from_list(bool_array_parameters_); - declare_parameters_from_list(integer_array_parameters_); - declare_parameters_from_list(double_array_parameters_); - declare_parameters_from_list(string_array_parameters_); - - declared_ = true; - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), - "Parameters already declared. Declaration should be done only once. " - "Nothing bad will happen, but please correct your code-logic."); - } -} - -bool ParameterHandler::get_parameters(bool check_validity, bool update) -{ - // TODO(destogl): Should we "auto-declare" parameters here? - if (!declared_) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Can not get parameters. Please declare them first."); - return false; - } - - bool ret = false; - - // If parameters are updated using dynamic reconfigure callback then there is no need to read - // them again. To ignore multiple manual reads - ret = - get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && - get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && - get_parameters_from_list(byte_array_parameters_) && - get_parameters_from_list(bool_array_parameters_) && - get_parameters_from_list(integer_array_parameters_) && - get_parameters_from_list(double_array_parameters_) && - get_parameters_from_list(string_array_parameters_); - - if (ret && check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - // If it is all good until now the parameters are not up to date anymore - if (ret) - { - up_to_date_ = false; - } - - if (ret && update) - { - ret = this->update(false); - } - - return ret; -} - -bool ParameterHandler::update(bool check_validity) -{ - bool ret = true; - - // Let's make this efficient and execute code only if parameters are updated - if (!up_to_date_) - { - if (check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - if (ret) - { - this->update_storage(); - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); - } - // reset variable to update parameters only when this is needed - up_to_date_ = true; - } - - return ret; -} - -rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. - for (const auto & input_parameter : parameters) - { - bool found = false; - - try - { - found = find_and_assign_parameter_value(bool_parameters_, input_parameter); - if (!found) - { - found = find_and_assign_parameter_value(integer_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); - } - - RCUTILS_LOG_INFO_EXPRESSION_NAMED( - found, logger_name_.c_str(), - "Dynamic parameters got changed! Maybe you have to restart controller to update the " - "parameters internally."); - - if (found) - { - up_to_date_ = false; - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - catch (const std::runtime_error & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - } - - return result; -} - -} // namespace control_toolbox diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 8682b118..da136c71 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -14,15 +14,40 @@ #include "test_low_pass_filter.hpp" +TEST_F(LowPassFilterTest, TestLowPassFilterParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("damping_frequency", 20.5); + node_->declare_parameter("damping_intensity", 1.25); + node_->declare_parameter("divider", 3); + + // should deny configuration as sampling frequency is missing + ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 0.0)); + // should deny configuration as sampling frequency is invalid + ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 1000.0)); + // should allow configuration + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + TEST_F(LowPassFilterTest, TestLowPassFilter) { double sampling_freq = 1000.0; double damping_freq = 20.5; double damping_intensity = 1.25; int divider = 3; - + double a1, b1; - a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); + a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / + (pow(10.0, damping_intensity / -10.0))); b1 = 1.0 - a1; geometry_msgs::msg::WrenchStamped in, calculated, out; @@ -30,66 +55,60 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; - std::shared_ptr> filter_ = + std::shared_ptr> filter_ = std::make_shared>(); - // WORKS BUT makes the next call to say parameter was already declared - // should deny configuration as parameters are not yet set - // ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", - // node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + // not yet configured, should deny update + ASSERT_FALSE(filter_->update(in, out)); + + // declare parameters and configure + node_->declare_parameter("sampling_frequency", sampling_freq); + node_->declare_parameter("damping_frequency", damping_freq); + node_->declare_parameter("damping_intensity", damping_intensity); + node_->declare_parameter("divider", divider); + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // first filter pass, output should be zero as no old wrench was stored + 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: + // 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; + // 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); - //not yet configured, should deny update - ASSERT_FALSE(filter_->update(in,out)); - - //declare parameters and configure - node_->declare_parameter("sampling_frequency", sampling_freq); - node_->declare_parameter("damping_frequency", damping_freq); - node_->declare_parameter("damping_intensity", damping_intensity); - node_->declare_parameter("divider", divider); - ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", - node_->get_node_logging_interface(), node_->get_node_parameters_interface())); - - //first filter pass, output should be zero as no old wrench was stored + // 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; + // check equality with low-pass-filter 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: - //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; - //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); - - //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; - //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_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); } diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index a2292ba8..e358df1e 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -33,11 +33,11 @@ class LowPassFilterTest : public ::testing::Test public: void SetUp() override { - executor_->add_node(node_); + executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } - LowPassFilterTest() + LowPassFilterTest() { rclcpp::init(0, nullptr); node_ = std::make_shared("test_low_pass_filter"); From 4386f124f1061bd06f9f28d6a0cd8026a3c2c66f Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 7 Mar 2023 18:08:52 +0100 Subject: [PATCH 12/27] Made check xml pass --- control_filters.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control_filters.xml b/control_filters.xml index 7a1924b0..faa6a0cc 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,15 +1,15 @@ + type="control_filters::LowPassFilter<double>" + base_class_type="filters::FilterBase<double>"> This is a low pass filter working with a double value. + type="control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>" + base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>"> This is a low pass filter working with geometry_msgs::WrenchStamped. From 5a7a9cc2111e37146af916d3a4b24c4448b09959 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 9 Mar 2023 10:48:28 +0100 Subject: [PATCH 13/27] Homogenized naming with typical filters --- control_filters.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_filters.xml b/control_filters.xml index faa6a0cc..a2280008 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,6 +1,6 @@ - From 7f1c73007b3390a264b18726c589ebae8eee9b70 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 9 Mar 2023 10:49:21 +0100 Subject: [PATCH 14/27] Reworked and activated control filter loading test --- CMakeLists.txt | 4 ++ .../test_load_low_pass_filter.cpp | 55 +++++++++++-------- 2 files changed, 36 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5bd9e672..dcd72dcf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,6 +99,10 @@ if(BUILD_TESTING) ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters) ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp) + target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters) + ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() # Install diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index 8be57b2c..e5695932 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -14,27 +14,31 @@ #include #include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" +#include +#include "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" +#include + -TEST(TestLoadLowPassFilter, load_low_pass_filter) +TEST(TestLoadLowPassFilter, load_low_pass_filter_double) { rclcpp::init(0, nullptr); - std::shared_ptr executor = - std::make_shared(); + pluginlib::ClassLoader> filter_loader("filters", + "filters::FilterBase"); + std::shared_ptr> 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; + } - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilter")); + std::string filter_type = "control_filters/LowPassFilterDouble"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } @@ -43,15 +47,20 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) { rclcpp::init(0, nullptr); - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + pluginlib::ClassLoader> + filter_loader("filters", "filters::FilterBase"); + std::shared_ptr> 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; + } - ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilterWrench")); + std::string filter_type = "control_filters/LowPassFilterWrench"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } From f620e3aebd2d2259f0b31e7afa10b6b7f59dca2a Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 9 Mar 2023 16:16:16 +0100 Subject: [PATCH 15/27] Improved parameter_handler usage when re-configure --- include/control_filters/low_pass_filter.hpp | 30 +++++++++++-------- test/control_filters/test_low_pass_filter.cpp | 8 ++++- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 11a100c0..b24de9b7 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -81,20 +81,24 @@ bool LowPassFilter::configure() logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - // Initialize the parameters - try + // Initialize the parameters once + if (!parameter_handler_) { - parameter_handler_ = std::make_shared(this->params_interface_); - } - catch (rclcpp::exceptions::ParameterUninitializedException & ex) { - RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - catch (rclcpp::exceptions::InvalidParameterValueException & ex) { - RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; + try + { + parameter_handler_ = + std::make_shared(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } } parameters_ = parameter_handler_->get_params(); compute_internal_params(); diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index da136c71..e4c127b5 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -33,7 +33,13 @@ TEST_F(LowPassFilterTest, TestLowPassFilterParameters) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); node_->set_parameter(rclcpp::Parameter("sampling_frequency", 1000.0)); - // should allow configuration + // should allow configuration and pass second call to unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0)); + // accept second call to configure with valid parameters to already configured filter ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } From eda275e8e671bea3c7ce47485143f671c811f638 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 11 Mar 2023 17:04:27 +0100 Subject: [PATCH 16/27] Applied code-review suggestions and fixed includes --- CMakeLists.txt | 47 ++++++++----------- README.md | 4 +- include/control_filters/low_pass_filter.hpp | 2 +- src/control_filters/low_pass_filter.cpp | 2 +- .../test_load_low_pass_filter.cpp | 2 +- test/control_filters/test_low_pass_filter.cpp | 2 +- test/control_filters/test_low_pass_filter.hpp | 2 +- 7 files changed, 27 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dcd72dcf..bede8c0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,37 +45,38 @@ target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB ######################## # Build control filters ######################## -find_package(filters REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(Eigen3 REQUIRED) - set(CONTROL_FILTERS_INCLUDE_DEPENDS - Eigen3 - generate_parameter_library filters - geometry_msgs - pluginlib rclcpp + generate_parameter_library + pluginlib + geometry_msgs + Eigen3 +) + +foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + low_pass_filter_parameters + src/control_filters/low_pass_filter_parameters.yaml ) add_library(low_pass_filter SHARED src/control_filters/low_pass_filter.cpp ) - +target_compile_features(low_pass_filter PUBLIC cxx_std_17) target_include_directories(low_pass_filter PUBLIC $ $ - $ -) -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_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters) +ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) ########## # Testing @@ -117,14 +118,6 @@ install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters RUNTIME DESTINATION bin ) -# Install pluginlib xml -pluginlib_export_plugin_description_file(filters control_filters.xml) - ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) -ament_export_libraries( - control_toolbox - low_pass_filter - low_pass_filter_parameters -) ament_package() diff --git a/README.md b/README.md index 369c15c5..ebe3750e 100644 --- a/README.md +++ b/README.md @@ -45,7 +45,7 @@ To run it initially over the whole repo you can use: If you get error that something is missing on your computer, do the following for: - - `clang-format-10` + - `clang-format-140` ``` - sudo apt install clang-format-10 + sudo apt install clang-format-14 ``` diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index b24de9b7..e1084f35 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index ccd5fe2b..d5063722 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index e5695932..f3c00fa8 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index e4c127b5..27777caa 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index e358df1e..d34a9e41 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 78b386aea9d7280441eedd6202c91d4fe2441d14 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 11 Mar 2023 17:04:41 +0100 Subject: [PATCH 17/27] potential other change --- CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bede8c0b..ac7f2c55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,7 +70,7 @@ target_compile_features(low_pass_filter PUBLIC cxx_std_17) target_include_directories(low_pass_filter PUBLIC $ $ - $ + $ ) target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters) ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) @@ -108,8 +108,12 @@ endif() # Install install( - DIRECTORY include/ - DESTINATION include/control_toolbox + DIRECTORY include/control_toolbox + DESTINATION include/control_toolbox/ +) + install( + DIRECTORY include/control_filters + DESTINATION include/control_filters/ ) install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters EXPORT export_control_toolbox From f40f53b188d1faf40ebfe1a75e6c00eaf3ebe5e3 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 13 Mar 2023 10:01:33 +0100 Subject: [PATCH 18/27] Revert "potential other change" This reverts commit 78b386aea9d7280441eedd6202c91d4fe2441d14. --- CMakeLists.txt | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac7f2c55..bede8c0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,7 +70,7 @@ target_compile_features(low_pass_filter PUBLIC cxx_std_17) target_include_directories(low_pass_filter PUBLIC $ $ - $ + $ ) target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters) ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) @@ -108,12 +108,8 @@ endif() # Install install( - DIRECTORY include/control_toolbox - DESTINATION include/control_toolbox/ -) - install( - DIRECTORY include/control_filters - DESTINATION include/control_filters/ + DIRECTORY include/ + DESTINATION include/control_toolbox ) install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters EXPORT export_control_toolbox From 41545080dd84cc17339f73ebecddd1ef0063d3b6 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 13 Mar 2023 15:40:36 +0100 Subject: [PATCH 19/27] Added validator for damping frequency --- src/control_filters/low_pass_filter_parameters.yaml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/control_filters/low_pass_filter_parameters.yaml b/src/control_filters/low_pass_filter_parameters.yaml index 1735f631..5cf0f842 100644 --- a/src/control_filters/low_pass_filter_parameters.yaml +++ b/src/control_filters/low_pass_filter_parameters.yaml @@ -11,17 +11,14 @@ low_pass_filter: type: double, # default_value: 20.5, description: "Damping frequency", - #validation: { - # gt<>: [0.0] - #}, + validation: { + gt<>: [0.0] + }, } damping_intensity: { type: double, # default_value: 1.25, description: "Damping intensity", - #validation: { - # gt<>: [0.0] - #}, } #divider: From b98f199dc50430d2ac8e021aec97db6f3ab0514c Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 13 Mar 2023 20:02:48 +0100 Subject: [PATCH 20/27] Fixed missing namespace forwarding --- include/control_filters/low_pass_filter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e1084f35..412e69d6 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -87,7 +87,8 @@ bool LowPassFilter::configure() try { parameter_handler_ = - std::make_shared(this->params_interface_); + std::make_shared(this->params_interface_, + this->param_prefix_); } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); From 2bdaacd45720e2b4b49a3cab9b211614dac09368 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 16 Mar 2023 21:40:30 +0100 Subject: [PATCH 21/27] Added missing Wrench.header copy --- include/control_filters/low_pass_filter.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 412e69d6..47155b14 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -151,6 +151,9 @@ inline bool LowPassFilter::update( data_out.wrench.torque.x = msg_filtered[3]; data_out.wrench.torque.y = msg_filtered[4]; data_out.wrench.torque.z = msg_filtered[5]; + + // copy the header + data_out.header = data_in.header; return true; } From 5fb123375eeaf87eb65fcbaefe70c60d479e2608 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 20 Mar 2023 08:14:34 +0100 Subject: [PATCH 22/27] Removed unrelated README changes --- README.md | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/README.md b/README.md index ebe3750e..21acbbc7 100644 --- a/README.md +++ b/README.md @@ -27,25 +27,3 @@ doi = {10.21105/joss.00456}, URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf} } ``` - - -## Code Formatting - -This repository uses `pre-commit` tool for code formatting. -The tool checks formatting each time you commit to a repository. -To install it locally use: - ``` - pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) - ``` - -To run it initially over the whole repo you can use: - ``` - pre-commit run -a - ``` - -If you get error that something is missing on your computer, do the following for: - - - `clang-format-140` - ``` - sudo apt install clang-format-14 - ``` From a6fee4b1afabc7edaccd56239332dcac7955dc21 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 20 Mar 2023 09:37:44 +0100 Subject: [PATCH 23/27] Cleaned-up unused variables --- src/control_filters/low_pass_filter_parameters.yaml | 12 ------------ test/control_filters/test_low_pass_filter.cpp | 3 --- 2 files changed, 15 deletions(-) diff --git a/src/control_filters/low_pass_filter_parameters.yaml b/src/control_filters/low_pass_filter_parameters.yaml index 5cf0f842..f98395e1 100644 --- a/src/control_filters/low_pass_filter_parameters.yaml +++ b/src/control_filters/low_pass_filter_parameters.yaml @@ -20,15 +20,3 @@ low_pass_filter: # default_value: 1.25, description: "Damping intensity", } - - #divider: - #{ - # type: int, - # # default_value: 3, - # description: "Divider", - # validation: - # { - # gt_eq<>: [0] - # }, - # read_only: true - #} diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 27777caa..2d6d06b3 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -21,7 +21,6 @@ TEST_F(LowPassFilterTest, TestLowPassFilterParameters) node_->declare_parameter("damping_frequency", 20.5); node_->declare_parameter("damping_intensity", 1.25); - node_->declare_parameter("divider", 3); // should deny configuration as sampling frequency is missing ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", @@ -49,7 +48,6 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) double sampling_freq = 1000.0; double damping_freq = 20.5; double damping_intensity = 1.25; - int divider = 3; double a1, b1; a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / @@ -71,7 +69,6 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) node_->declare_parameter("sampling_frequency", sampling_freq); node_->declare_parameter("damping_frequency", damping_freq); node_->declare_parameter("damping_intensity", damping_intensity); - node_->declare_parameter("divider", divider); ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); From b6cbb02d5c2bd911ad04c89b72805c93e4dfa7c6 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 25 Mar 2023 18:32:32 +0100 Subject: [PATCH 24/27] Switched to yaml-based param in filter test --- CMakeLists.txt | 4 +- test/control_filters/test_low_pass_filter.cpp | 51 +++++++++++++------ test/control_filters/test_low_pass_filter.hpp | 5 +- .../test_low_pass_filter_parameters.yaml | 22 ++++++++ 4 files changed, 63 insertions(+), 19 deletions(-) create mode 100644 test/control_filters/test_low_pass_filter_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index bede8c0b..7d3a2c73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -97,7 +97,9 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters - ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) + add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml + ) target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters) ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 2d6d06b3..4f2cc9ce 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -14,37 +14,52 @@ #include "test_low_pass_filter.hpp" -TEST_F(LowPassFilterTest, TestLowPassFilterParameters) +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) { std::shared_ptr> filter_ = std::make_shared>(); - node_->declare_parameter("damping_frequency", 20.5); - node_->declare_parameter("damping_intensity", 1.25); + // should allow configuration and find parameters in sensor_filter_chain param namespace + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0)); + // accept second call to configure with valid parameters to already configured filter + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); // should deny configuration as sampling frequency is missing ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); - node_->set_parameter(rclcpp::Parameter("sampling_frequency", 0.0)); // should deny configuration as sampling frequency is invalid ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + // fix the param node_->set_parameter(rclcpp::Parameter("sampling_frequency", 1000.0)); // should allow configuration and pass second call to unconfigured filter ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); - - // change a parameter - node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0)); - // accept second call to configure with valid parameters to already configured filter - ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", - node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } -TEST_F(LowPassFilterTest, TestLowPassFilter) +TEST_F(LowPassFilterTest, TestLowPassFilterComputation) { + // parameters should match the test yaml file double sampling_freq = 1000.0; double damping_freq = 20.5; double damping_intensity = 1.25; @@ -65,10 +80,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) // not yet configured, should deny update ASSERT_FALSE(filter_->update(in, out)); - // declare parameters and configure - node_->declare_parameter("sampling_frequency", sampling_freq); - node_->declare_parameter("damping_frequency", damping_freq); - node_->declare_parameter("damping_intensity", damping_intensity); + // configure ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); @@ -115,3 +127,12 @@ TEST_F(LowPassFilterTest, TestLowPassFilter) ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index d34a9e41..c81e495c 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -33,14 +33,14 @@ class LowPassFilterTest : public ::testing::Test public: void SetUp() override { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } LowPassFilterTest() { - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_low_pass_filter"); executor_ = std::make_shared(); } @@ -52,7 +52,6 @@ class LowPassFilterTest : public ::testing::Test executor_thread_.join(); } node_.reset(); - rclcpp::shutdown(); } protected: diff --git a/test/control_filters/test_low_pass_filter_parameters.yaml b/test/control_filters/test_low_pass_filter_parameters.yaml new file mode 100644 index 00000000..11e08cc1 --- /dev/null +++ b/test/control_filters/test_low_pass_filter_parameters.yaml @@ -0,0 +1,22 @@ +TestLowPassWrenchFilterAllParameters: + ros__parameters: + sampling_frequency: 1000.0 + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassWrenchFilterMissingParameter: + ros__parameters: + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassWrenchFilterInvalidThenFixedParameter: + ros__parameters: + sampling_frequency: 0.0 + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassFilterComputation: + ros__parameters: + sampling_frequency: 1000.0 + damping_frequency: 20.5 + damping_intensity: 1.25 From dd8770087bb4801b1f2348e1dc57c62cfd1f5db1 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 17 Apr 2023 17:49:12 +0200 Subject: [PATCH 25/27] Removed unused defaults --- src/control_filters/low_pass_filter_parameters.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/control_filters/low_pass_filter_parameters.yaml b/src/control_filters/low_pass_filter_parameters.yaml index f98395e1..bb26f5b7 100644 --- a/src/control_filters/low_pass_filter_parameters.yaml +++ b/src/control_filters/low_pass_filter_parameters.yaml @@ -1,7 +1,6 @@ low_pass_filter: sampling_frequency: { type: double, - # default_value: 1000.0, description: "Sampling frequency", validation: { gt<>: [0.0] @@ -9,7 +8,6 @@ low_pass_filter: } damping_frequency: { type: double, - # default_value: 20.5, description: "Damping frequency", validation: { gt<>: [0.0] @@ -17,6 +15,5 @@ low_pass_filter: } damping_intensity: { type: double, - # default_value: 1.25, description: "Damping intensity", } From 36733bfbbcdbf5351de80cd664f4b545ba9a2709 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 19 Apr 2023 09:46:14 +0200 Subject: [PATCH 26/27] Added doxygen doc --- include/control_filters/low_pass_filter.hpp | 72 ++++++++++++++++++++- 1 file changed, 69 insertions(+), 3 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 47155b14..e4a11782 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -29,19 +29,84 @@ namespace control_filters { +/***************************************************/ +/*! \class LowPassFilter + \brief A Low-pass filter class. + + This class implements a low-pass filter for + various data types based on an Infinite Impulse Response Filter. + For vector elements, the filtering is applied separately on + each element of the vector. + + In particular, this class implements a simplified version of + an IIR filter equation : + + \f$y(n) = b x(n-1) + a y(n-1)\f$ + + where:
+
    +
  • \f$ x(n)\f$ is the input signal +
  • \f$ y(n)\f$ is the output signal (filtered) +
  • \f$ b \f$ is the feedforward filter coefficient +
  • \f$ a \f$ is the feedback filter coefficient +
+ + and the Low-Pass coefficient equation: +
+
    +
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ +
  • \f$ b = 1 - a \f$ +
+ + where:
+
    +
  • \f$ sf \f$ is the sampling frequency +
  • \f$ df \f$ is the damping frequency +
  • \f$ di \f$ is the damping intensity (amplitude) +
+ + \section Usage + + The LowPassFilter class is meant to be instantiated as a filter in + a controller but can also be used elsewhere. + For manual instantiation, you should first call configure() + (in non-realtime) and then call update() at every update step. + +*/ +/***************************************************/ + template class LowPassFilter : public filters::FilterBase { public: + // Default constructor LowPassFilter(); + /*! + * \brief Destructor of LowPassFilter class. + */ ~LowPassFilter() override; + /*! + * \brief Configure the LowPassFilter (access and process params). + */ bool configure() override; + /*! + * \brief Applies one iteration of the IIR filter. + * + * \param data_in input to the filter + * \param data_out filtered output + * + * \returns false if filter is not configured, true otherwise + */ bool update(const T & data_in, T & data_out) override; protected: + /*! + * \brief Internal computation of the feedforward and feedbackward coefficients + * according to the LowPassFilter parameters. + */ void compute_internal_params() { a1_ = exp( @@ -57,11 +122,12 @@ class LowPassFilter : public filters::FilterBase low_pass_filter::Params parameters_; // Filter parameters - // TODO(destogl): we should do this more intelligently using only one set of types + /** internal data storage (double). */ double filtered_value, filtered_old_value, old_value; + /** internal data storage (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - double a1_; - double b1_; + double a1_; /**< feedbackward coefficient. */ + double b1_; /**< feedforward coefficient. */ }; template From 036dac06811d0f4dcd6cee5fed9475c2dd0978c6 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 19 Apr 2023 09:46:14 +0200 Subject: [PATCH 27/27] Added low-pass filter documentation in a README --- src/control_filters/README.md | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/control_filters/README.md diff --git a/src/control_filters/README.md b/src/control_filters/README.md new file mode 100644 index 00000000..cac37a26 --- /dev/null +++ b/src/control_filters/README.md @@ -0,0 +1,31 @@ +# Control filters + +## Available filters + +* Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench). + +## Low Pass filter + +This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench). +The feedforward and feedback coefficients of the IIR filter are computed from the low-pass filter parameters. + +### Required parameters + +* sampling frequency as `sf` +* damping frequency as `df` +* damping intensity as `di` + +### Algorithm + +Given +* above-required parameters, `sf`, `df`, `di` +* `data_in`, a double or wrench `x` + +Compute `data_out`, the filtered output `y(n)` with equation: + +y(n) = b x(n-1) + a y(n-1) + +with + +* a the feedbackward coefficient such that a = exp( -1/sf (2 pi df) / (10^(di/-10)) ) +* b the feedforward coefficient such that b = 1 - a