From fe360004c86a2c2a38910ba9af223427a18e4c12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 9 Jun 2021 17:26:55 +0200 Subject: [PATCH 01/34] Add support for publishing additional topics to the FTS Broadcaster --- .../CMakeLists.txt | 10 ++ .../force_torque_sensor_broadcaster.hpp | 30 ++++- force_torque_sensor_broadcaster/package.xml | 6 + .../src/force_torque_sensor_broadcaster.cpp | 121 ++++++++++++++++-- ..._torque_sensor_broadcaster_parameters.yaml | 6 + 5 files changed, 161 insertions(+), 12 deletions(-) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 207e978c10..8693955017 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -7,6 +7,8 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + control_toolbox + filters generate_parameter_library geometry_msgs hardware_interface @@ -14,6 +16,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + tf2 + tf2_ros + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -72,6 +77,11 @@ if(BUILD_TESTING) ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface ) + ament_target_dependencies(test_load_force_torque_sensor_broadcaster + controller_manager + hardware_interface + ros2_control_test_assets + ) endif() install( diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 754d1de9ba..9fb53c537f 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -24,6 +24,7 @@ #include #include "controller_interface/controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,9 +33,14 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" namespace force_torque_sensor_broadcaster { +using WrenchMsgType = geometry_msgs::msg::WrenchStamped; + class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface { public: @@ -68,12 +74,30 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte protected: std::shared_ptr param_listener_; Params params_; + std::string sensor_name_; + std::array interface_names_; + std::string frame_id_; + std::vector additional_frames_to_publish_; std::unique_ptr force_torque_sensor_; - using StatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr sensor_state_publisher_; - std::unique_ptr realtime_publisher_; + WrenchMsgType wrench_raw_; + WrenchMsgType wrench_filtered_; + std::unique_ptr> filter_chain_; + + using StatePublisher = rclcpp::Publisher::SharedPtr; + using StateRTPublisher = realtime_tools::RealtimePublisher; + StatePublisher sensor_raw_state_publisher_; + StatePublisher sensor_filtered_state_publisher_; + std::unique_ptr realtime_raw_publisher_; + std::unique_ptr realtime_filtered_publisher_; + + // Transformation variables + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::vector wrench_additional_frames_pubs_; + std::vector> wrench_additional_frames_publishers_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 6db14c62e1..2d1a5de4b8 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -13,6 +13,8 @@ backward_ros controller_interface + filters + control_toolbox geometry_msgs hardware_interface pluginlib @@ -20,9 +22,13 @@ rclcpp_lifecycle realtime_tools generate_parameter_library + tf2 + tf2_geometry_msgs + tf2_ros ament_cmake_gmock controller_manager + hardware_interface ros2_control_test_assets diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9687f9dfee..83cf0429ed 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -18,8 +18,11 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include #include #include +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace force_torque_sensor_broadcaster { @@ -93,12 +96,41 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( torque_names.z)); } + try + { + filter_chain_ = + std::make_unique>("geometry_msgs::msg::WrenchStamped"); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during filter chain creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + if (!filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Could not configure sensor filter chain, please check if the " + "parameters are provided correctly."); + return CallbackReturn::ERROR; + } + try { // register ft sensor data publisher - sensor_state_publisher_ = get_node()->create_publisher( + sensor_raw_state_publisher_ = get_node()->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = std::make_unique(sensor_state_publisher_); + realtime_raw_publisher_ = std::make_unique(sensor_raw_state_publisher_); + + sensor_filtered_state_publisher_ = get_node()->create_publisher( + "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + realtime_filtered_publisher_ = std::make_unique(sensor_filtered_state_publisher_); } catch (const std::exception & e) { @@ -108,9 +140,38 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->unlock(); + wrench_raw_.header.frame_id = frame_id_; + wrench_filtered_.header.frame_id = frame_id_; + + realtime_raw_publisher_->lock(); + realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_raw_publisher_->unlock(); + realtime_filtered_publisher_->lock(); + realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_filtered_publisher_->unlock(); + + // Add additional frames to publish if any exits + if (!params_.additional_frames_to_publish.empty()) + { + auto nr_frames = params_.additional_frames_to_publish.size(); + wrench_additional_frames_pubs_.reserve(nr_frames); + wrench_additional_frames_publishers_.reserve(nr_frames); + for (const auto & frame : params_.additional_frames_to_publish) + { + StatePublisher pub = get_node()->create_publisher( + "~/wrench_filtered_" + frame, rclcpp::SystemDefaultsQoS()); + wrench_additional_frames_pubs_.emplace_back(pub); + wrench_additional_frames_publishers_.emplace_back(std::make_unique(pub)); + + wrench_additional_frames_publishers_.back()->lock(); + wrench_additional_frames_publishers_.back()->msg_.header.frame_id = frame; + wrench_additional_frames_publishers_.back()->unlock(); + } + + // initialize buffer transforms + tf_buffer_ = std::make_shared(get_node()->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -150,11 +211,53 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate controller_interface::return_type ForceTorqueSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + wrench_raw_.header.stamp = time; + force_torque_sensor_->get_values_as_message(wrench_raw_.wrench); + + // Filter sensor data + filter_chain_->update(wrench_raw_, wrench_filtered_); + + if (realtime_raw_publisher_ && realtime_raw_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - realtime_publisher_->unlockAndPublish(); + realtime_raw_publisher_->msg_.header.stamp = time; + realtime_raw_publisher_->msg_.wrench = wrench_raw_.wrench; + realtime_raw_publisher_->unlockAndPublish(); + } + + if (realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) + { + realtime_filtered_publisher_->msg_.header.stamp = time; + realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; + realtime_filtered_publisher_->unlockAndPublish(); + } + + for (const auto & publisher : wrench_additional_frames_publishers_) + { + try + { + auto transform = + tf_buffer_->lookupTransform(publisher->msg_.header.frame_id, frame_id_, tf2::TimePointZero); + tf2::doTransform(wrench_filtered_, publisher->msg_, transform); + + if (publisher && publisher->trylock()) + { + publisher->msg_.header.stamp = time; + publisher->unlockAndPublish(); + } + } + catch (const tf2::TransformException & e) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 5000, + "LookupTransform failed from '%s' to '%s'.", frame_id_.c_str(), + publisher->msg_.header.frame_id.c_str()); + publisher->msg_.wrench.force.x = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.force.y = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.force.z = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.x = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.y = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.z = std::numeric_limits::quiet_NaN(); + } } return controller_interface::return_type::OK; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 059d5ab9a1..711ebc8492 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -42,3 +42,9 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + additional_frames_to_publish: { + type: string_array, + default_value: [], + description: "Array of additional frames to publish filtered Wrench to" + } + From ef7ad6aece7170f2966a514c0edf0bb9489c4ab8 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 13 Mar 2023 19:51:16 +0100 Subject: [PATCH 02/34] Added filtering capabilities --- .../CMakeLists.txt | 18 ++- .../src/force_torque_sensor_broadcaster.cpp | 19 ++- ..._torque_sensor_broadcaster_parameters.yaml | 1 - ...torque_sensor_broadcaster_with_filters.cpp | 143 ++++++++++++++++++ ...torque_sensor_broadcaster_with_filters.hpp | 65 ++++++++ ...r_broadcaster_with_filters_parameters.yaml | 19 +++ 6 files changed, 255 insertions(+), 10 deletions(-) create mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp create mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp create mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 8693955017..1ac69b89ab 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -60,7 +60,7 @@ if(BUILD_TESTING) test/test_load_force_torque_sensor_broadcaster.cpp ) target_link_libraries(test_load_force_torque_sensor_broadcaster - force_torque_sensor_broadcaster + force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters ) ament_target_dependencies(test_load_force_torque_sensor_broadcaster controller_manager @@ -72,16 +72,28 @@ if(BUILD_TESTING) test/test_force_torque_sensor_broadcaster.cpp ) target_link_libraries(test_force_torque_sensor_broadcaster - force_torque_sensor_broadcaster + force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters ) + ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_force_torque_sensor_broadcaster + + add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster_with_filters + test/test_force_torque_sensor_broadcaster_with_filters.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml + ) + target_include_directories(test_force_torque_sensor_broadcaster_with_filters PRIVATE include) + + target_link_libraries(test_force_torque_sensor_broadcaster_with_filters + force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters) + + ament_target_dependencies(test_force_torque_sensor_broadcaster_with_filters controller_manager hardware_interface ros2_control_test_assets ) + endif() install( diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 83cf0429ed..c5af20d2bd 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -109,7 +109,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( e.what()); return CallbackReturn::ERROR; } - if (!filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface())) @@ -118,19 +117,27 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( get_node()->get_logger(), "Could not configure sensor filter chain, please check if the " "parameters are provided correctly."); + // user helper + auto allparams = get_node()->get_node_parameters_interface()->list_parameters({}, 0); + std::cerr << " available params are:\n" << std::endl; + for (const auto & name : allparams.names) + { + std::cerr << name << std::endl; + } return CallbackReturn::ERROR; } - try { // register ft sensor data publisher sensor_raw_state_publisher_ = get_node()->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_raw_publisher_ = std::make_unique(sensor_raw_state_publisher_); - - sensor_filtered_state_publisher_ = get_node()->create_publisher( - "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); - realtime_filtered_publisher_ = std::make_unique(sensor_filtered_state_publisher_); + + sensor_filtered_state_publisher_ = + get_node()->create_publisher( + "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + realtime_filtered_publisher_ = + std::make_unique(sensor_filtered_state_publisher_); } catch (const std::exception & e) { diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 711ebc8492..3824f2aad6 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -47,4 +47,3 @@ force_torque_sensor_broadcaster: default_value: [], description: "Array of additional frames to publish filtered Wrench to" } - diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp new file mode 100644 index 0000000000..fecf48ed58 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp @@ -0,0 +1,143 @@ +// 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. +// 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. + +/* + * Authors: Subhas Das, Denis Stogl + */ + +#include "test_force_torque_sensor_broadcaster_with_filters.hpp" + +#include +#include +#include + +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedStateInterface; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +void ForceTorqueSensorBroadcasterWithFilterTest::SetUpTestCase() {} + +void ForceTorqueSensorBroadcasterWithFilterTest::TearDownTestCase() {} + +void ForceTorqueSensorBroadcasterWithFilterTest::SetUp() +{ + // initialize controller + fts_broadcaster_ = std::make_unique(); +} + +void ForceTorqueSensorBroadcasterWithFilterTest::TearDown() { fts_broadcaster_.reset(nullptr); } + +void ForceTorqueSensorBroadcasterWithFilterTest::SetUpFTSBroadcaster(const std::string & name) +{ + const auto result = fts_broadcaster_->init(name); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector state_ifs; + state_ifs.emplace_back(fts_force_x_); + state_ifs.emplace_back(fts_force_y_); + state_ifs.emplace_back(fts_force_z_); + state_ifs.emplace_back(fts_torque_x_); + state_ifs.emplace_back(fts_torque_y_); + state_ifs.emplace_back(fts_torque_z_); + + fts_broadcaster_->assign_interfaces({}, std::move(state_ifs)); +} + +TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success) +{ + SetUpFTSBroadcaster("FTS_NoFilter"); + + // configure passed (no filter means the filter_chain is still valid) + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; + wrench_in.wrench.force.x = 1.0; + + // update passed + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + + // no change of the data + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); + ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); + ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); +} + +TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilter_Configure_Success) +{ + SetUpFTSBroadcaster("FTS_SingleFilter"); + + // debug helper, in case filters is not found + pluginlib::ClassLoader> filter_loader( + "filters", "filters::FilterBase"); + std::shared_ptr> filter; + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto & available_class : filter_loader.getDeclaredClasses()) + { + sstr << " " << available_class << std::endl; + } + + // configure passed with one filter + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); + geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; + wrench_in.header.frame_id = "world"; + wrench_in.wrench.force.x = 1.0; + + // update passed + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + // udpdate twice because currently the IIR has a zero output at first iteration + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + + // test if the correct low-pass filter was loaded + // sampling freq:200, damping_freq:50, damping_intensity:1 => a1 = 0.1284 , b1= 1-a1 =0.8616 + double sampling_freq = 200.0; + double damping_freq = 50.0; + double damping_intensity = 1.0; + 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; + // expect change + ASSERT_EQ(wrench_out.wrench.force.x, b1 * wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); + ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); + ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.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/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp new file mode 100644 index 0000000000..7c45065e1f --- /dev/null +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp @@ -0,0 +1,65 @@ +// 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. + +/* + * Authors: Subhas Das, Denis Stogl + */ + +#ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ +#define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ + +#include +#include + +#include "gmock/gmock.h" + +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" + +// subclassing and friending so we can access member variables +class FriendForceTorqueSensorBroadcasterWithFilter +: public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster +{ + FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilter_Configure_Success); +}; + +class ForceTorqueSensorBroadcasterWithFilterTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpFTSBroadcaster(const std::string & name); + +protected: + const std::string sensor_name_ = "fts_sensor"; + const std::string frame_id_ = "fts_sensor_frame"; + std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + + hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; + hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; + hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; + hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; + hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; + hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; + + std::unique_ptr fts_broadcaster_; + + void subscribe_and_get_message(geometry_msgs::msg::WrenchStamped & wrench_msg); +}; + +#endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml new file mode 100644 index 0000000000..455d0ffe04 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml @@ -0,0 +1,19 @@ +FTS_NoFilter: + ros__parameters: + sensor_name: "fts_sensor" + frame_id: "fts_sensor_frame" + sensor_filter_chain: "none" + +FTS_SingleFilter: + ros__parameters: + sensor_name: "fts_sensor" + frame_id: "fts_sensor_frame" + sensor_filter_chain: + filter1: + type: "control_filters/LowPassFilterWrench" + name: "low_pass_filter" + params: + sampling_frequency: 200.0 + damping_frequency: 50.0 + damping_intensity: 1.0 + divider: 1 From a47926adcb02b63f24432a9903282db8db8487d2 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 14 Mar 2023 16:04:28 +0100 Subject: [PATCH 03/34] Added more tests with filters --- ...torque_sensor_broadcaster_with_filters.cpp | 115 +++++++++++++++++- ...torque_sensor_broadcaster_with_filters.hpp | 9 +- ...r_broadcaster_with_filters_parameters.yaml | 40 +++++- 3 files changed, 160 insertions(+), 4 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp index fecf48ed58..a42638daed 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp @@ -89,9 +89,9 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success) ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); } -TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilter_Configure_Success) +TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Update) { - SetUpFTSBroadcaster("FTS_SingleFilter"); + SetUpFTSBroadcaster("FTS_SingleFilterLP"); // debug helper, in case filters is not found pluginlib::ClassLoader> filter_loader( @@ -133,6 +133,117 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilter_Configure_Succes ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); } +TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Update) +{ + SetUpFTSBroadcaster("FTS_SingleFilterGC"); + + // debug helper, in case filters is not found + pluginlib::ClassLoader> filter_loader( + "filters", "filters::FilterBase"); + std::shared_ptr> filter; + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto & available_class : filter_loader.getDeclaredClasses()) + { + sstr << " " << available_class << std::endl; + } + + // configure passed with one filter + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); + geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; + wrench_in.header.frame_id = "world"; + wrench_in.wrench.force.x = 1.0; + + // update passed + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + + // test if the correct gravity compensation filter was loaded (should match values in yaml file) + double gravity_acc = 9.81; + double mass = 5.0; + double cog_y = 0.1; + // with no tf different with world, sensor frame is world frame, calculation is simplified + double grav_force_z = gravity_acc * mass; + double grav_torque_x = cog_y * grav_force_z; + // expect change + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); + EXPECT_NEAR(wrench_out.wrench.force.z, wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); + EXPECT_NEAR( + wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); +} + +TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, ChainedFilterLPGC_Configure_Update) +{ + SetUpFTSBroadcaster("FTS_ChainedFilterLPGC"); + + // debug helper, in case filters is not found + pluginlib::ClassLoader> filter_loader( + "filters", "filters::FilterBase"); + std::shared_ptr> filter; + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto & available_class : filter_loader.getDeclaredClasses()) + { + sstr << " " << available_class << std::endl; + } + + // configure passed with two filters + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); + geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; + wrench_in.header.frame_id = "world"; + wrench_in.wrench.force.x = 1.0; + + // update passed + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + + // test if the correct chained filter was loaded (should match values in yaml file) + double sampling_freq = 200.0; + double damping_freq = 50.0; + double damping_intensity = 1.0; + 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; + + double gravity_acc = 9.81; + double mass = 5.0; + double cog_y = 0.1; + // with no tf different with world, sensor frame is world frame, calculation is simplified + double grav_force_z = gravity_acc * mass; + double grav_torque_x = cog_y * grav_force_z; + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + // expect change + ASSERT_EQ(wrench_out.wrench.force.x, 0.0); + ASSERT_EQ(wrench_out.wrench.force.y, 0.0); + EXPECT_NEAR(wrench_out.wrench.force.z, 0.0 + grav_force_z, COMMON_THRESHOLD); + EXPECT_NEAR(wrench_out.wrench.torque.x, 0.0 + grav_torque_x, COMMON_THRESHOLD); + ASSERT_EQ(wrench_out.wrench.torque.y, 0.0); + ASSERT_EQ(wrench_out.wrench.torque.z, 0.0); + + // udpdate twice because currently the IIR has a zero output at first iteration + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + + ASSERT_EQ(wrench_out.wrench.force.x, b1 * wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, b1 * wrench_in.wrench.force.y); + EXPECT_NEAR( + wrench_out.wrench.force.z, b1 * wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); + EXPECT_NEAR( + wrench_out.wrench.torque.x, b1 * wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); + ASSERT_EQ(wrench_out.wrench.torque.y, b1 * wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, b1 * wrench_in.wrench.torque.z); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp index 7c45065e1f..581024b88c 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp @@ -26,12 +26,19 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +namespace +{ +const double COMMON_THRESHOLD = 0.001; +} + // subclassing and friending so we can access member variables class FriendForceTorqueSensorBroadcasterWithFilter : public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster { FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success); - FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilter_Configure_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Update); + FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Update); + FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, ChainedFilterLPGC_Configure_Update); }; class ForceTorqueSensorBroadcasterWithFilterTest : public ::testing::Test diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml index 455d0ffe04..73739e30d1 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml @@ -4,7 +4,7 @@ FTS_NoFilter: frame_id: "fts_sensor_frame" sensor_filter_chain: "none" -FTS_SingleFilter: +FTS_SingleFilterLP: ros__parameters: sensor_name: "fts_sensor" frame_id: "fts_sensor_frame" @@ -17,3 +17,41 @@ FTS_SingleFilter: damping_frequency: 50.0 damping_intensity: 1.0 divider: 1 + +FTS_SingleFilterGC: + ros__parameters: + sensor_name: "fts_sensor" + frame_id: "fts_sensor_frame" + sensor_filter_chain: + filter1: + type: "control_filters/GravityCompensationWrench" + name: "gravity_compensation" + params: + world_frame: "world" + sensor_frame: "world" + CoG: + pos: [0.0, 0.1, 0.0] + force: [0.0, 0.0, -49.05] # -gravity_acc * mass + +FTS_ChainedFilterLPGC: + ros__parameters: + sensor_name: "fts_sensor" + frame_id: "fts_sensor_frame" + sensor_filter_chain: + filter1: + type: "control_filters/LowPassFilterWrench" + name: "low_pass_filter" + params: + sampling_frequency: 200.0 + damping_frequency: 50.0 + damping_intensity: 1.0 + divider: 1 + filter2: + type: "control_filters/GravityCompensationWrench" + name: "gravity_compensation" + params: + world_frame: "world" + sensor_frame: "world" + CoG: + pos: [0.0, 0.1, 0.0] + force: [0.0, 0.0, -49.05] # -gravity_acc * mass From a4132170784c86828e33c02ec40d51992f3c3a37 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sun, 19 Mar 2023 19:00:00 +0100 Subject: [PATCH 04/34] Removed helper that only shows declared params --- .../src/force_torque_sensor_broadcaster.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index c5af20d2bd..328dc18a49 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -117,13 +117,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( get_node()->get_logger(), "Could not configure sensor filter chain, please check if the " "parameters are provided correctly."); - // user helper - auto allparams = get_node()->get_node_parameters_interface()->list_parameters({}, 0); - std::cerr << " available params are:\n" << std::endl; - for (const auto & name : allparams.names) - { - std::cerr << name << std::endl; - } return CallbackReturn::ERROR; } try From 3652ab12490644acf1aba1273c34b560ab27dfe0 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sun, 19 Mar 2023 19:03:09 +0100 Subject: [PATCH 05/34] Fixed filter handling and unitialized frame_id --- .../force_torque_sensor_broadcaster.hpp | 1 - .../src/force_torque_sensor_broadcaster.cpp | 44 ++++++++++++------- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 9fb53c537f..22f3549c53 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -76,7 +76,6 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte Params params_; std::string sensor_name_; std::array interface_names_; - std::string frame_id_; std::vector additional_frames_to_publish_; std::unique_ptr force_torque_sensor_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 328dc18a49..17b23b4f5b 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -140,8 +140,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - wrench_raw_.header.frame_id = frame_id_; - wrench_filtered_.header.frame_id = frame_id_; + wrench_raw_.header.frame_id = params_.frame_id; + wrench_filtered_.header.frame_id = params_.frame_id; realtime_raw_publisher_->lock(); realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; @@ -214,9 +214,6 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update( wrench_raw_.header.stamp = time; force_torque_sensor_->get_values_as_message(wrench_raw_.wrench); - // Filter sensor data - filter_chain_->update(wrench_raw_, wrench_filtered_); - if (realtime_raw_publisher_ && realtime_raw_publisher_->trylock()) { realtime_raw_publisher_->msg_.header.stamp = time; @@ -224,6 +221,18 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update( realtime_raw_publisher_->unlockAndPublish(); } + // Filter sensor data + auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); + if (!filtered) + { + wrench_filtered_.wrench.force.x = std::numeric_limits::quiet_NaN(); + wrench_filtered_.wrench.force.y = std::numeric_limits::quiet_NaN(); + wrench_filtered_.wrench.force.z = std::numeric_limits::quiet_NaN(); + wrench_filtered_.wrench.torque.x = std::numeric_limits::quiet_NaN(); + wrench_filtered_.wrench.torque.y = std::numeric_limits::quiet_NaN(); + wrench_filtered_.wrench.torque.z = std::numeric_limits::quiet_NaN(); + } + if (realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) { realtime_filtered_publisher_->msg_.header.stamp = time; @@ -235,22 +244,23 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update( { try { - auto transform = - tf_buffer_->lookupTransform(publisher->msg_.header.frame_id, frame_id_, tf2::TimePointZero); - tf2::doTransform(wrench_filtered_, publisher->msg_, transform); - - if (publisher && publisher->trylock()) + if (filtered) + { + auto transform = tf_buffer_->lookupTransform( + publisher->msg_.header.frame_id, params_.frame_id, tf2::TimePointZero); + tf2::doTransform(wrench_filtered_, publisher->msg_, transform); + } + else { - publisher->msg_.header.stamp = time; - publisher->unlockAndPublish(); + throw tf2::TransformException("cannot transform, filtered message is invalid"); } } catch (const tf2::TransformException & e) { RCLCPP_ERROR_SKIPFIRST_THROTTLE( get_node()->get_logger(), *(get_node()->get_clock()), 5000, - "LookupTransform failed from '%s' to '%s'.", frame_id_.c_str(), - publisher->msg_.header.frame_id.c_str()); + "LookupTransform failed from '%s' to '%s'. %s", params_.frame_id.c_str(), + publisher->msg_.header.frame_id.c_str(), e.what()); publisher->msg_.wrench.force.x = std::numeric_limits::quiet_NaN(); publisher->msg_.wrench.force.y = std::numeric_limits::quiet_NaN(); publisher->msg_.wrench.force.z = std::numeric_limits::quiet_NaN(); @@ -258,8 +268,12 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update( publisher->msg_.wrench.torque.y = std::numeric_limits::quiet_NaN(); publisher->msg_.wrench.torque.z = std::numeric_limits::quiet_NaN(); } + if (publisher && publisher->trylock()) + { + publisher->msg_.header.stamp = time; + publisher->unlockAndPublish(); + } } - return controller_interface::return_type::OK; } From 8163413c2a52f7976c56480c8ea19d2b5a30d950 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sun, 19 Mar 2023 19:05:31 +0100 Subject: [PATCH 06/34] Added test for additional_frames_to_publish --- .../test_force_torque_sensor_broadcaster.cpp | 57 +++++++++++++++++++ .../test_force_torque_sensor_broadcaster.hpp | 2 + 2 files changed, 59 insertions(+) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 74e2b15da0..cf596e77a2 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -98,6 +98,37 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); } +void ForceTorqueSensorBroadcasterTest::subscribe_additional_frames_and_get_message( + const std::string & frame, geometry_msgs::msg::WrenchStamped & wrench_msg) +{ + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_force_torque_sensor_broadcaster/wrench_filtered_" + frame, 10, subs_callback); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); +} + TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) { SetUpFTSBroadcaster(); @@ -264,6 +295,32 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, AdditionalFrame_Publish_Success) +{ + SetUpFTSBroadcaster(); + + // set the params 'sensor_name', 'frame_id', additional_frames_to_publish + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + std::string additional_frame = "fts_sensor_frame"; //permits to get a valid lookupTransform + fts_broadcaster_->get_node()->set_parameter( + {"additional_frames_to_publish", std::vector({additional_frame})}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_additional_frames_and_get_message(additional_frame, wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, additional_frame); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..70e7f3a84b 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -67,6 +67,8 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test std::unique_ptr fts_broadcaster_; void subscribe_and_get_message(geometry_msgs::msg::WrenchStamped & wrench_msg); + void subscribe_additional_frames_and_get_message( + const std::string & frame, geometry_msgs::msg::WrenchStamped & wrench_msg); }; #endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ From 31e79e2b43da734ea9f4dfe9f6325fbdc5d490b3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 12 Apr 2025 07:51:03 +0000 Subject: [PATCH 07/34] Fix test parameters --- ...e_sensor_broadcaster_with_filters_parameters.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml index 73739e30d1..646a14d17b 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml @@ -29,9 +29,9 @@ FTS_SingleFilterGC: params: world_frame: "world" sensor_frame: "world" - CoG: - pos: [0.0, 0.1, 0.0] - force: [0.0, 0.0, -49.05] # -gravity_acc * mass + tool: + CoG: [0.0, 0.1, 0.0] + mass: 5.0 FTS_ChainedFilterLPGC: ros__parameters: @@ -52,6 +52,6 @@ FTS_ChainedFilterLPGC: params: world_frame: "world" sensor_frame: "world" - CoG: - pos: [0.0, 0.1, 0.0] - force: [0.0, 0.0, -49.05] # -gravity_acc * mass + tool: + CoG: [0.0, 0.1, 0.0] + mass: 5.0 From c9b52f1c42c9dff5a1753b95c526a02aa7b12eb2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 21 Apr 2025 20:41:32 +0000 Subject: [PATCH 08/34] compile filters from source on humble needs https://github.com/ros/filters/commit/34f154b89aac40c74a1dcf0166338f4837b4590e --- .../rolling-compatibility-humble-binary-build.yml | 4 +++- ros2_controllers-not-released.humble.repos | 9 ++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 54f85ecf4b..1beafb44de 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -46,5 +46,7 @@ jobs: with: ros_distro: humble ros_repo: testing - upstream_workspace: ros2_controllers.rolling.repos + upstream_workspace: | + ros2_controllers-not-released.humble.repos + ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 1b3910e7e7..fc78700f7e 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -1,6 +1,5 @@ repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master + filters: + type: git + url: https://github.com/ros/filters.git + version: ros2 From 0759030d0e96dcc0619d901939ec3d051fb563ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 3 May 2025 14:15:33 +0100 Subject: [PATCH 09/34] Use filters from binary --- .../workflows/rolling-compatibility-humble-binary-build.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 1beafb44de..54f85ecf4b 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -46,7 +46,5 @@ jobs: with: ros_distro: humble ros_repo: testing - upstream_workspace: | - ros2_controllers-not-released.humble.repos - ros2_controllers.rolling.repos + upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master From aa12321d111b94c4cdc04ecec6ba43ed9bd859a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 3 May 2025 14:58:27 +0100 Subject: [PATCH 10/34] Update ros2_controllers-not-released.humble.repos --- ros2_controllers-not-released.humble.repos | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index fc78700f7e..700d7428bb 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -1,5 +1,6 @@ repositories: - filters: - type: git - url: https://github.com/ros/filters.git - version: ros2 + ## EXAMPLE DEPENDENCY + # : + # type: git + # url: git@github.com:/.git + # version: master From 3d15fa5c0b8d93cade9ab3beb26fc96f2e44ff5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 3 May 2025 18:43:17 +0100 Subject: [PATCH 11/34] Fix pre-commit --- ros2_controllers-not-released.humble.repos | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 700d7428bb..b441016205 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -1,6 +1,6 @@ repositories: - ## EXAMPLE DEPENDENCY - # : - # type: git - # url: git@github.com:/.git - # version: master + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master From a78dab05b1c8c45653e12d98fd636a1dde224a09 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 21 May 2025 16:21:40 +0000 Subject: [PATCH 12/34] Fix the tests --- ...torque_sensor_broadcaster_with_filters.cpp | 66 +++++++++++-------- 1 file changed, 40 insertions(+), 26 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp index 1b5e658f3a..3d344ca651 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp @@ -111,9 +111,19 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Upda wrench_in.header.frame_id = "world"; wrench_in.wrench.force.x = 1.0; - // update passed + // update and init with initial value + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); + ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); + ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); + + // update with different value + wrench_in.wrench.force.x = 10.0; + // update twice because currently the IIR has the same output at first iteration EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - // udpdate twice because currently the IIR has a zero output at first iteration EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); // test if the correct low-pass filter was loaded @@ -125,13 +135,14 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Upda a1 = exp( -1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); b1 = 1.0 - a1; - // expect change - ASSERT_EQ(wrench_out.wrench.force.x, b1 * wrench_in.wrench.force.x); + // expect no change ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); + // expect change + ASSERT_EQ(wrench_out.wrench.force.x, a1 * 1.0 + b1 * wrench_in.wrench.force.x); } TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Update) @@ -165,11 +176,10 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Upda // with no tf different with world, sensor frame is world frame, calculation is simplified double grav_force_z = gravity_acc * mass; double grav_torque_x = cog_y * grav_force_z; + // expect change - geometry_msgs::msg::WrenchStamped in, out; - in.header.frame_id = "world"; - in.wrench.force.x = 1.0; - in.wrench.torque.x = 10.0; + wrench_in.wrench.torque.x = 10.0; + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); @@ -220,29 +230,33 @@ TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, ChainedFilterLPGC_Configure_U double grav_force_z = gravity_acc * mass; double grav_torque_x = cog_y * grav_force_z; - geometry_msgs::msg::WrenchStamped in, out; - in.header.frame_id = "world"; - in.wrench.force.x = 1.0; - in.wrench.torque.x = 10.0; + // expect no change from wrench_in + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); // expect change - ASSERT_EQ(wrench_out.wrench.force.x, 0.0); - ASSERT_EQ(wrench_out.wrench.force.y, 0.0); - EXPECT_NEAR(wrench_out.wrench.force.z, 0.0 + grav_force_z, COMMON_THRESHOLD); - EXPECT_NEAR(wrench_out.wrench.torque.x, 0.0 + grav_torque_x, COMMON_THRESHOLD); - ASSERT_EQ(wrench_out.wrench.torque.y, 0.0); - ASSERT_EQ(wrench_out.wrench.torque.z, 0.0); - - // udpdate twice because currently the IIR has a zero output at first iteration + EXPECT_NEAR(wrench_out.wrench.force.z, wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); + EXPECT_NEAR( + wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); + + // update + wrench_in.wrench.force.x = 1.0; + wrench_in.wrench.torque.x = 10.0; + // update twice because currently the IIR has the same output at first iteration + EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - ASSERT_EQ(wrench_out.wrench.force.x, b1 * wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, b1 * wrench_in.wrench.force.y); + ASSERT_EQ(wrench_out.wrench.force.x, a1 * 1.0 + b1 * wrench_in.wrench.force.x); + ASSERT_EQ(wrench_out.wrench.force.y, a1 * 0.0 + b1 * wrench_in.wrench.force.y); EXPECT_NEAR( - wrench_out.wrench.force.z, b1 * wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); + wrench_out.wrench.force.z, a1 * 0.0 + b1 * wrench_in.wrench.force.z + grav_force_z, + COMMON_THRESHOLD); EXPECT_NEAR( - wrench_out.wrench.torque.x, b1 * wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); - ASSERT_EQ(wrench_out.wrench.torque.y, b1 * wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, b1 * wrench_in.wrench.torque.z); + wrench_out.wrench.torque.x, a1 * 0.0 + b1 * wrench_in.wrench.torque.x + grav_torque_x, + COMMON_THRESHOLD); + ASSERT_EQ(wrench_out.wrench.torque.y, a1 * 0.0 + b1 * wrench_in.wrench.torque.y); + ASSERT_EQ(wrench_out.wrench.torque.z, a1 * 0.0 + b1 * wrench_in.wrench.torque.z); } int main(int argc, char ** argv) From 834e1ad3d15522837588ce556fdc0ace0f8a6b25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 1 Jul 2025 22:07:02 +0200 Subject: [PATCH 13/34] Apply suggestions from code review Co-authored-by: Xavier Guay --- force_torque_sensor_broadcaster/CMakeLists.txt | 2 +- force_torque_sensor_broadcaster/package.xml | 2 +- .../src/force_torque_sensor_broadcaster.cpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 76478bd985..b25e8af06a 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -17,8 +17,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle realtime_tools tf2 - tf2_ros tf2_geometry_msgs + tf2_ros ) find_package(ament_cmake REQUIRED) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 6e022a3570..73a541793d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -25,8 +25,8 @@ backward_ros controller_interface - filters control_toolbox + filters geometry_msgs hardware_interface pluginlib diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 397b2d6d27..fca5b2be0c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -21,6 +21,7 @@ #include #include #include + #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" From 98fac42f4b1e99020dba552d35b6cfaea4bed3bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 1 Jul 2025 22:07:27 +0200 Subject: [PATCH 14/34] Apply suggestions from code review Co-authored-by: Xavier Guay --- .../src/force_torque_sensor_broadcaster_parameters.yaml | 2 +- ros2_controllers-not-released.humble.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 419015f7e3..e0132127d0 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -49,7 +49,7 @@ force_torque_sensor_broadcaster: additional_frames_to_publish: { type: string_array, default_value: [], - description: "Array of additional frames to publish filtered wrench to" + description: "Array of additional frames to publish filtered wrench to." } offset: force: diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index b441016205..2149abe505 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -1,5 +1,5 @@ repositories: - ## EXAMPLE DEPENDENCY +## EXAMPLE DEPENDENCY # : # type: git # url: git@github.com:/.git From 3a6978c21a8662cf73c74c969e77d0fac7686bac Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 1 Jul 2025 20:17:46 +0000 Subject: [PATCH 15/34] Use correct hpp file --- .../src/force_torque_sensor_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index fca5b2be0c..17cda19c3a 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -22,7 +22,7 @@ #include #include -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace force_torque_sensor_broadcaster From 63dcce277e1d9cfd8f27e319e4b65ac929a7a4a5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 3 Jul 2025 13:37:57 +0200 Subject: [PATCH 16/34] Fix format --- .../src/force_torque_sensor_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index c5a79fc87b..0805d4acd3 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -206,7 +206,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) -{ +{ param_listener_->try_get_params(params_); wrench_raw_.header.stamp = time; From cad2b29627516340a8339942610113abe7a91949 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 3 Jul 2025 22:17:38 +0530 Subject: [PATCH 17/34] Update description of limit() function in speed_limiter (#1793) --- .../include/diff_drive_controller/speed_limiter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 53a7c09242..6f101f1609 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -57,7 +57,7 @@ class SpeedLimiter } /** - * \brief Limit the velocity and acceleration + * \brief Limit the velocity, acceleration, and jerk * \param [in, out] v Velocity [m/s] * \param [in] v0 Previous velocity to v [m/s] * \param [in] v1 Previous velocity to v0 [m/s] From c6b8ca711980d54fea6cdbaed73e644d1d3199d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 3 Jul 2025 20:50:30 +0200 Subject: [PATCH 18/34] Improve paths filtering of docs CI job (#1796) --- .github/workflows/humble-check-docs.yml | 7 +++++++ .github/workflows/jazzy-check-docs.yml | 7 +++++++ .github/workflows/rolling-check-docs.yml | 6 ++++++ 3 files changed, 20 insertions(+) diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 448ff4084b..09b7d695e9 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -8,7 +8,14 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/humble-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index 72a5332d89..02c6f2af50 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -8,7 +8,14 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/jazzy-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 1144c4e6c0..56a785475d 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -8,7 +8,13 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory - '.github/workflows/rolling-check-docs.yml' concurrency: From da21fd435a9cd138867857da83e90b062bfad72a Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sat, 5 Jul 2025 11:32:17 +0200 Subject: [PATCH 19/34] fix rqt_joint_trajectory_controller for robots with namespace (#1792) --- .../joint_trajectory_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 12ac10c64c..38faed8bc6 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -464,7 +464,7 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.required_state_interfaces: - name = interface.split("/")[-2] + name = "/".join(interface.split("/")[:-1]) if name not in joint_names: joint_names.append(name) From 8c1fa73f34565160ea9cc94ccaafeb1c4f5491bb Mon Sep 17 00:00:00 2001 From: Hilary Luo <103377417+hilary-luo@users.noreply.github.com> Date: Mon, 7 Jul 2025 02:48:24 -0400 Subject: [PATCH 20/34] Mecanum Drive: Populate the pose covariance matrix (#1772) --- .../src/mecanum_drive_controller.cpp | 7 ++++--- .../test/mecanum_drive_controller_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 18 ++++++++++++++++++ 3 files changed, 23 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 793043f990..b96cacc5b4 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -173,13 +173,14 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( rt_odom_state_publisher_->msg_.child_frame_id = base_frame_id; rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + auto & pose_covariance = rt_odom_state_publisher_->msg_.pose.covariance; + auto & twist_covariance = rt_odom_state_publisher_->msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) { const size_t diagonal_index = NUM_DIMENSIONS * index + index; - covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + pose_covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + twist_covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } rt_odom_state_publisher_->unlock(); diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 658cd3b2fa..dfa3e5de94 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -17,7 +17,7 @@ odom_frame_id: "odom" enable_odom_tf: true twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 6.0, 12.0, 18.0, 24.0, 30.0] test_mecanum_drive_controller_with_rotation: diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index ae7f551693..66bc2d6ab9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -62,6 +62,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + ASSERT_EQ( + controller_->params_.pose_covariance_diagonal, + std::vector({0.0, 6.0, 12.0, 18.0, 24.0, 30.0})); + ASSERT_EQ( + controller_->params_.twist_covariance_diagonal, + std::vector({0.0, 7.0, 14.0, 21.0, 28.0, 35.0})); + ASSERT_EQ( controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); ASSERT_EQ( @@ -140,6 +147,17 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na /* tf_frame_prefix_enable is false so no modifications to the frame id's */ ASSERT_EQ(test_odom_frame_id, odom_id); ASSERT_EQ(test_base_frame_id, base_link_id); + + std::array pose_covariance = { + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 12.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 18.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30.0}}; + + std::array twist_covariance = { + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 14.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 21.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 28.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35.0}}; + + ASSERT_EQ(odometry_message.pose.covariance, pose_covariance); + ASSERT_EQ(odometry_message.twist.covariance, twist_covariance); } TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace) From 284c4a125e57dd32996f868ab51b55900a9f28bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 8 Jul 2025 08:54:30 +0200 Subject: [PATCH 21/34] Explicit cast `rcutils_duration_value_t` (#1808) --- diff_drive_controller/src/diff_drive_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e559958244..8464867c49 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -129,7 +129,8 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub else { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + logger, *get_node()->get_clock(), + static_cast(cmd_vel_timeout_.seconds() * 1000), "Command message contains NaNs. Not updating reference interfaces."); } From 96191954987200c6cf4381498fa04eaf38c1b349 Mon Sep 17 00:00:00 2001 From: oscarmartinez Date: Thu, 10 Jul 2025 11:27:31 +0200 Subject: [PATCH 22/34] Applying review suggestions --- .../force_torque_sensor_broadcaster.hpp | 11 +- .../src/force_torque_sensor_broadcaster.cpp | 115 ++++-------------- ..._torque_sensor_broadcaster_parameters.yaml | 12 +- 3 files changed, 34 insertions(+), 104 deletions(-) diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 5f92d48e42..81bfb25976 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -73,15 +73,13 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr std::shared_ptr param_listener_; Params params_; - std::string sensor_name_; - std::array interface_names_; - std::vector additional_frames_to_publish_; std::unique_ptr force_torque_sensor_; WrenchMsgType wrench_raw_; WrenchMsgType wrench_filtered_; std::unique_ptr> filter_chain_; + bool has_filter_chain_; using StatePublisher = rclcpp::Publisher::SharedPtr; using StateRTPublisher = realtime_tools::RealtimePublisher; @@ -89,13 +87,6 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr StatePublisher sensor_filtered_state_publisher_; std::unique_ptr realtime_raw_publisher_; std::unique_ptr realtime_filtered_publisher_; - - // Transformation variables - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - std::vector wrench_additional_frames_pubs_; - std::vector> wrench_additional_frames_publishers_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 0805d4acd3..2d769162d4 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -91,6 +91,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( torque_names.z)); } + // As sensor_filter_chain param is of type none, we cannot directly check if it present + // If the filter configuration fails, just continue without it try { filter_chain_ = @@ -100,20 +102,16 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( { fprintf( stderr, - "Exception thrown during filter chain creation at configure stage with message : %s \n", + "Exception thrown during filter chain creation with message : %s \n", e.what()); return CallbackReturn::ERROR; } - if (!filter_chain_->configure( + + // Refer to the params yaml for info on the prefix + has_filter_chain_ = filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), - get_node()->get_node_parameters_interface())) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Could not configure sensor filter chain, please check if the " - "parameters are provided correctly."); - return CallbackReturn::ERROR; - } + get_node()->get_node_parameters_interface()); + try { // register ft sensor data publisher @@ -121,11 +119,13 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_raw_publisher_ = std::make_unique(sensor_raw_state_publisher_); - sensor_filtered_state_publisher_ = - get_node()->create_publisher( - "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); - realtime_filtered_publisher_ = - std::make_unique(sensor_filtered_state_publisher_); + if (has_filter_chain_) { + sensor_filtered_state_publisher_ = + get_node()->create_publisher( + "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + realtime_filtered_publisher_ = + std::make_unique(sensor_filtered_state_publisher_); + } } catch (const std::exception & e) { @@ -142,31 +142,10 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; realtime_raw_publisher_->unlock(); - realtime_filtered_publisher_->lock(); - realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_filtered_publisher_->unlock(); - - // Add additional frames to publish if any exits - if (!params_.additional_frames_to_publish.empty()) - { - auto nr_frames = params_.additional_frames_to_publish.size(); - wrench_additional_frames_pubs_.reserve(nr_frames); - wrench_additional_frames_publishers_.reserve(nr_frames); - for (const auto & frame : params_.additional_frames_to_publish) - { - StatePublisher pub = get_node()->create_publisher( - "~/wrench_filtered_" + frame, rclcpp::SystemDefaultsQoS()); - wrench_additional_frames_pubs_.emplace_back(pub); - wrench_additional_frames_publishers_.emplace_back(std::make_unique(pub)); - - wrench_additional_frames_publishers_.back()->lock(); - wrench_additional_frames_publishers_.back()->msg_.header.frame_id = frame; - wrench_additional_frames_publishers_.back()->unlock(); - } - - // initialize buffer transforms - tf_buffer_ = std::make_shared(get_node()->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + if (has_filter_chain_){ + realtime_filtered_publisher_->lock(); + realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_filtered_publisher_->unlock(); } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -222,58 +201,16 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write } // Filter sensor data - auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); - if (!filtered) - { - wrench_filtered_.wrench.force.x = std::numeric_limits::quiet_NaN(); - wrench_filtered_.wrench.force.y = std::numeric_limits::quiet_NaN(); - wrench_filtered_.wrench.force.z = std::numeric_limits::quiet_NaN(); - wrench_filtered_.wrench.torque.x = std::numeric_limits::quiet_NaN(); - wrench_filtered_.wrench.torque.y = std::numeric_limits::quiet_NaN(); - wrench_filtered_.wrench.torque.z = std::numeric_limits::quiet_NaN(); - } - - if (realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) - { - realtime_filtered_publisher_->msg_.header.stamp = time; - realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; - realtime_filtered_publisher_->unlockAndPublish(); - } - - for (const auto & publisher : wrench_additional_frames_publishers_) - { - try - { - if (filtered) - { - auto transform = tf_buffer_->lookupTransform( - publisher->msg_.header.frame_id, params_.frame_id, tf2::TimePointZero); - tf2::doTransform(wrench_filtered_, publisher->msg_, transform); - } - else - { - throw tf2::TransformException("cannot transform, filtered message is invalid"); - } - } - catch (const tf2::TransformException & e) + if (has_filter_chain_) { + auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); + if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE( - get_node()->get_logger(), *(get_node()->get_clock()), 5000, - "LookupTransform failed from '%s' to '%s'. %s", params_.frame_id.c_str(), - publisher->msg_.header.frame_id.c_str(), e.what()); - publisher->msg_.wrench.force.x = std::numeric_limits::quiet_NaN(); - publisher->msg_.wrench.force.y = std::numeric_limits::quiet_NaN(); - publisher->msg_.wrench.force.z = std::numeric_limits::quiet_NaN(); - publisher->msg_.wrench.torque.x = std::numeric_limits::quiet_NaN(); - publisher->msg_.wrench.torque.y = std::numeric_limits::quiet_NaN(); - publisher->msg_.wrench.torque.z = std::numeric_limits::quiet_NaN(); - } - if (publisher && publisher->trylock()) - { - publisher->msg_.header.stamp = time; - publisher->unlockAndPublish(); + realtime_filtered_publisher_->msg_.header.stamp = time; + realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; + realtime_filtered_publisher_->unlockAndPublish(); } } + return controller_interface::return_type::OK; } diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index e0132127d0..fe1c7cedbb 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -46,11 +46,6 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } - additional_frames_to_publish: { - type: string_array, - default_value: [], - description: "Array of additional frames to publish filtered wrench to." - } offset: force: x: { @@ -117,3 +112,10 @@ force_torque_sensor_broadcaster: default_value: 1.0, description: "The multiplier of torque value around 'z' axis.", } + sensor_filter_chain: { + type: none, + description: "Map of parameters that defines a filter chain, containing filterN as key. The fields for each filter are: + type: The filter plugin to be loaded + name: Actual name semantically describing the filter, e.g., low_pass_filter + params: And underlying map of parameters needed for a specific filter, refer to the specific filter documentation." + } From b1de60ba041d225e457a0fbc7079e6683900dad8 Mon Sep 17 00:00:00 2001 From: oscarmartinez Date: Fri, 11 Jul 2025 10:43:26 +0200 Subject: [PATCH 23/34] Simplified testing --- .../CMakeLists.txt | 11 - .../force_torque_sensor_broadcaster.hpp | 1 - .../src/force_torque_sensor_broadcaster.cpp | 51 ++-- ...orce_torque_sensor_broadcaster_params.yaml | 14 + .../test_force_torque_sensor_broadcaster.cpp | 179 ++++++------ .../test_force_torque_sensor_broadcaster.hpp | 8 +- ...torque_sensor_broadcaster_with_filters.cpp | 269 ------------------ ...torque_sensor_broadcaster_with_filters.hpp | 72 ----- ...r_broadcaster_with_filters_parameters.yaml | 57 ---- 9 files changed, 141 insertions(+), 521 deletions(-) delete mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp delete mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp delete mode 100644 force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index b25e8af06a..3cd1dac720 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -80,17 +80,6 @@ if(BUILD_TESTING) force_torque_sensor_broadcaster_parameters ) - add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster_with_filters - test/test_force_torque_sensor_broadcaster_with_filters.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml - ) - target_include_directories(test_force_torque_sensor_broadcaster_with_filters PRIVATE include) - target_link_libraries(test_force_torque_sensor_broadcaster_with_filters - force_torque_sensor_broadcaster - force_torque_sensor_broadcaster_parameters - ros2_control_test_assets::ros2_control_test_assets - ) - endif() install( diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 81bfb25976..c20a59d9ec 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -79,7 +79,6 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr WrenchMsgType wrench_raw_; WrenchMsgType wrench_filtered_; std::unique_ptr> filter_chain_; - bool has_filter_chain_; using StatePublisher = rclcpp::Publisher::SharedPtr; using StateRTPublisher = realtime_tools::RealtimePublisher; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 2d769162d4..222cdd38c2 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include "tf2/utils.hpp" @@ -91,8 +92,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( torque_names.z)); } - // As sensor_filter_chain param is of type none, we cannot directly check if it present - // If the filter configuration fails, just continue without it try { filter_chain_ = @@ -107,11 +106,15 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - // Refer to the params yaml for info on the prefix - has_filter_chain_ = filter_chain_->configure( - "sensor_filter_chain", get_node()->get_node_logging_interface(), - get_node()->get_node_parameters_interface()); - + // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is present. + // Even if the sensor_filter_chain parameter is not specified, the filter chain will be correctly + // configured with an empty list of filters (https://github.com/ros/filters/issues/89). + if (!filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface())) { + return controller_interface::CallbackReturn::ERROR; + } + try { // register ft sensor data publisher @@ -119,13 +122,11 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_raw_publisher_ = std::make_unique(sensor_raw_state_publisher_); - if (has_filter_chain_) { - sensor_filtered_state_publisher_ = - get_node()->create_publisher( + sensor_filtered_state_publisher_ = + get_node()->create_publisher( "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); - realtime_filtered_publisher_ = - std::make_unique(sensor_filtered_state_publisher_); - } + realtime_filtered_publisher_ = + std::make_unique(sensor_filtered_state_publisher_); } catch (const std::exception & e) { @@ -142,11 +143,9 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; realtime_raw_publisher_->unlock(); - if (has_filter_chain_){ - realtime_filtered_publisher_->lock(); - realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_filtered_publisher_->unlock(); - } + realtime_filtered_publisher_->lock(); + realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_filtered_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -200,15 +199,13 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write realtime_raw_publisher_->unlockAndPublish(); } - // Filter sensor data - if (has_filter_chain_) { - auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); - if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) - { - realtime_filtered_publisher_->msg_.header.stamp = time; - realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; - realtime_filtered_publisher_->unlockAndPublish(); - } + // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_ + auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); + if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) + { + realtime_filtered_publisher_->msg_.header.stamp = time; + realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; + realtime_filtered_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; diff --git a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml index 58cc1c6b16..393e777dd5 100644 --- a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -2,3 +2,17 @@ test_force_torque_sensor_broadcaster: ros__parameters: frame_id: "fts_sensor_frame" +test_force_torque_sensor_broadcaster_with_chain: + ros__parameters: + frame_id: "fts_sensor_frame" + sensor_name: "fts_sensor" + sensor_filter_chain: + filter1: + name: gravity_compensation + type: control_filters/GravityCompensationWrench + params: + sensor_frame: ${WRIST_SIDE_PREFIX}_ft_sensor_link + world_frame: base_footprint + tool: + CoG: [-0.0001, 0.0000, 0.0319] + mass: 0.776 diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 19b5bc89ef..c80ae7c597 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -19,10 +19,12 @@ #include "test_force_torque_sensor_broadcaster.hpp" #include +#include #include #include #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "gtest/gtest.h" #include "hardware_interface/loaned_state_interface.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor.hpp" @@ -50,11 +52,10 @@ void ForceTorqueSensorBroadcasterTest::SetUp() void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); } -void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() +void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster(std::string node_name) { - const auto result = fts_broadcaster_->init( - "test_force_torque_sensor_broadcaster", "", 0, "", - fts_broadcaster_->define_custom_node_options()); + const auto result = + fts_broadcaster_->init(node_name, "", 0, "", fts_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -69,7 +70,7 @@ void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() } void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( - geometry_msgs::msg::WrenchStamped & wrench_msg) + geometry_msgs::msg::WrenchStamped & wrench_msg, std::string & topic_name) { // create a new subscriber geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; @@ -77,7 +78,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( - "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + topic_name, 10, subs_callback); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(test_subscription_node.get_node_base_interface()); @@ -97,6 +98,14 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( // check if message has been received if (received_msg.get()) { + std::cout << "Received message on topic '" << topic_name << "':" << std::endl; + std::cout << " frame_id: " << received_msg->header.frame_id << std::endl; + std::cout << " force.x: " << received_msg->wrench.force.x << std::endl; + std::cout << " force.y: " << received_msg->wrench.force.y << std::endl; + std::cout << " force.z: " << received_msg->wrench.force.z << std::endl; + std::cout << " torque.x: " << received_msg->wrench.torque.x << std::endl; + std::cout << " torque.y: " << received_msg->wrench.torque.y << std::endl; + std::cout << " torque.z: " << received_msg->wrench.torque.z << std::endl; break; } } @@ -108,40 +117,9 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( wrench_msg = *received_msg; } -void ForceTorqueSensorBroadcasterTest::subscribe_additional_frames_and_get_message( - const std::string & frame, geometry_msgs::msg::WrenchStamped & wrench_msg) -{ - // create a new subscriber - rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; - auto subscription = test_subscription_node.create_subscription( - "/test_force_torque_sensor_broadcaster/wrench_filtered_" + frame, 10, subs_callback); - - // call update to publish the test value - // since update doesn't guarantee a published message, republish until received - int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); - while (max_sub_check_loop_count--) - { - fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { - break; - } - } - ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - "controller/broadcaster update loop"; - - // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); -} - TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // configure failed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -149,7 +127,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -164,7 +142,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' empty fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); @@ -175,7 +153,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'interface_names' empty fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); @@ -187,7 +165,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -209,7 +187,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'interface_names' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -226,7 +204,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -258,7 +236,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -274,7 +252,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -290,7 +268,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -300,7 +278,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -313,7 +292,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); std::array force_offsets = {{10.0, 30.0, -50.0}}; std::array torque_offsets = {{1.0, -1.2, -5.2}}; @@ -331,7 +310,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); @@ -375,7 +355,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipliers) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // some non‐trivial multipliers std::array force_multipliers = {{2.0, 0.5, -1.0}}; @@ -399,7 +379,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipl // Publish & grab the message geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); // Check header ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); @@ -443,35 +424,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipl } } -TEST_F(ForceTorqueSensorBroadcasterTest, AdditionalFrame_Publish_Success) -{ - SetUpFTSBroadcaster(); - - // set the params 'sensor_name', 'frame_id', additional_frames_to_publish - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - std::string additional_frame = "fts_sensor_frame"; // permits to get a valid lookupTransform - fts_broadcaster_->get_node()->set_parameter( - {"additional_frames_to_publish", std::vector({additional_frame})}); - - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_additional_frames_and_get_message(additional_frame, wrench_msg); - - ASSERT_EQ(wrench_msg.header.frame_id, additional_frame); - ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); - ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1]); - ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2]); - ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); - ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); - ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); -} - TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -482,7 +437,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -508,7 +464,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set all the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -523,7 +479,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -556,6 +513,64 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) } } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + // configure passed + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + std::cout << "Finished" << std::endl; +} + +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_ActivateDeactivate_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + // configure and activate success + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // deactivate passed + ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Update_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 731530d642..56a02b68ee 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -39,6 +39,8 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); + + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success); }; class ForceTorqueSensorBroadcasterTest : public ::testing::Test @@ -50,7 +52,7 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpFTSBroadcaster(); + void SetUpFTSBroadcaster(std::string node_name); protected: const std::string sensor_name_ = "fts_sensor"; @@ -66,7 +68,9 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test std::unique_ptr fts_broadcaster_; - void subscribe_and_get_message(geometry_msgs::msg::WrenchStamped & wrench_msg); + void subscribe_and_get_message( + geometry_msgs::msg::WrenchStamped & wrench_msg, std::string & topic_name); + void subscribe_additional_frames_and_get_message( const std::string & frame, geometry_msgs::msg::WrenchStamped & wrench_msg); }; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp deleted file mode 100644 index 3d344ca651..0000000000 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.cpp +++ /dev/null @@ -1,269 +0,0 @@ -// 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. -// 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. - -/* - * Authors: Subhas Das, Denis Stogl - */ - -#include "test_force_torque_sensor_broadcaster_with_filters.hpp" - -#include -#include -#include - -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - -using hardware_interface::LoanedStateInterface; - -namespace -{ -constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -} // namespace - -void ForceTorqueSensorBroadcasterWithFilterTest::SetUpTestCase() {} - -void ForceTorqueSensorBroadcasterWithFilterTest::TearDownTestCase() {} - -void ForceTorqueSensorBroadcasterWithFilterTest::SetUp() -{ - // initialize controller - fts_broadcaster_ = std::make_unique(); -} - -void ForceTorqueSensorBroadcasterWithFilterTest::TearDown() { fts_broadcaster_.reset(nullptr); } - -void ForceTorqueSensorBroadcasterWithFilterTest::SetUpFTSBroadcaster(const std::string & name) -{ - const auto result = - fts_broadcaster_->init(name, "", 0, "", fts_broadcaster_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector state_ifs; - state_ifs.emplace_back(fts_force_x_); - state_ifs.emplace_back(fts_force_y_); - state_ifs.emplace_back(fts_force_z_); - state_ifs.emplace_back(fts_torque_x_); - state_ifs.emplace_back(fts_torque_y_); - state_ifs.emplace_back(fts_torque_z_); - - fts_broadcaster_->assign_interfaces({}, std::move(state_ifs)); -} - -TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success) -{ - SetUpFTSBroadcaster("FTS_NoFilter"); - - // configure passed (no filter means the filter_chain is still valid) - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; - wrench_in.wrench.force.x = 1.0; - - // update passed - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - // no change of the data - ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); - ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); - ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); - ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); -} - -TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Update) -{ - SetUpFTSBroadcaster("FTS_SingleFilterLP"); - - // debug helper, in case filters is not found - pluginlib::ClassLoader> filter_loader( - "filters", "filters::FilterBase"); - std::shared_ptr> filter; - std::stringstream sstr; - sstr << "available filters:" << std::endl; - for (const auto & available_class : filter_loader.getDeclaredClasses()) - { - sstr << " " << available_class << std::endl; - } - - // configure passed with one filter - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); - geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; - wrench_in.header.frame_id = "world"; - wrench_in.wrench.force.x = 1.0; - - // update and init with initial value - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); - ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); - ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); - ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); - - // update with different value - wrench_in.wrench.force.x = 10.0; - // update twice because currently the IIR has the same output at first iteration - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - // test if the correct low-pass filter was loaded - // sampling freq:200, damping_freq:50, damping_intensity:1 => a1 = 0.1284 , b1= 1-a1 =0.8616 - double sampling_freq = 200.0; - double damping_freq = 50.0; - double damping_intensity = 1.0; - 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; - // expect no change - ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); - ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z); - ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x); - ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); - // expect change - ASSERT_EQ(wrench_out.wrench.force.x, a1 * 1.0 + b1 * wrench_in.wrench.force.x); -} - -TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Update) -{ - SetUpFTSBroadcaster("FTS_SingleFilterGC"); - - // debug helper, in case filters is not found - pluginlib::ClassLoader> filter_loader( - "filters", "filters::FilterBase"); - std::shared_ptr> filter; - std::stringstream sstr; - sstr << "available filters:" << std::endl; - for (const auto & available_class : filter_loader.getDeclaredClasses()) - { - sstr << " " << available_class << std::endl; - } - - // configure passed with one filter - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); - geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; - wrench_in.header.frame_id = "world"; - wrench_in.wrench.force.x = 1.0; - - // update passed - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - // test if the correct gravity compensation filter was loaded (should match values in yaml file) - double gravity_acc = 9.81; - double mass = 5.0; - double cog_y = 0.1; - // with no tf different with world, sensor frame is world frame, calculation is simplified - double grav_force_z = gravity_acc * mass; - double grav_torque_x = cog_y * grav_force_z; - - // expect change - wrench_in.wrench.torque.x = 10.0; - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); - EXPECT_NEAR(wrench_out.wrench.force.z, wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); - EXPECT_NEAR( - wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); - ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); -} - -TEST_F(ForceTorqueSensorBroadcasterWithFilterTest, ChainedFilterLPGC_Configure_Update) -{ - SetUpFTSBroadcaster("FTS_ChainedFilterLPGC"); - - // debug helper, in case filters is not found - pluginlib::ClassLoader> filter_loader( - "filters", "filters::FilterBase"); - std::shared_ptr> filter; - std::stringstream sstr; - sstr << "available filters:" << std::endl; - for (const auto & available_class : filter_loader.getDeclaredClasses()) - { - sstr << " " << available_class << std::endl; - } - - // configure passed with two filters - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS) << sstr.str(); - geometry_msgs::msg::WrenchStamped wrench_in, wrench_out; - wrench_in.header.frame_id = "world"; - wrench_in.wrench.force.x = 1.0; - - // update passed - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - // test if the correct chained filter was loaded (should match values in yaml file) - double sampling_freq = 200.0; - double damping_freq = 50.0; - double damping_intensity = 1.0; - 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; - - double gravity_acc = 9.81; - double mass = 5.0; - double cog_y = 0.1; - // with no tf different with world, sensor frame is world frame, calculation is simplified - double grav_force_z = gravity_acc * mass; - double grav_torque_x = cog_y * grav_force_z; - - // expect no change from wrench_in - ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y); - ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z); - // expect change - EXPECT_NEAR(wrench_out.wrench.force.z, wrench_in.wrench.force.z + grav_force_z, COMMON_THRESHOLD); - EXPECT_NEAR( - wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + grav_torque_x, COMMON_THRESHOLD); - - // update - wrench_in.wrench.force.x = 1.0; - wrench_in.wrench.torque.x = 10.0; - // update twice because currently the IIR has the same output at first iteration - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - EXPECT_TRUE(fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out)); - - ASSERT_EQ(wrench_out.wrench.force.x, a1 * 1.0 + b1 * wrench_in.wrench.force.x); - ASSERT_EQ(wrench_out.wrench.force.y, a1 * 0.0 + b1 * wrench_in.wrench.force.y); - EXPECT_NEAR( - wrench_out.wrench.force.z, a1 * 0.0 + b1 * wrench_in.wrench.force.z + grav_force_z, - COMMON_THRESHOLD); - EXPECT_NEAR( - wrench_out.wrench.torque.x, a1 * 0.0 + b1 * wrench_in.wrench.torque.x + grav_torque_x, - COMMON_THRESHOLD); - ASSERT_EQ(wrench_out.wrench.torque.y, a1 * 0.0 + b1 * wrench_in.wrench.torque.y); - ASSERT_EQ(wrench_out.wrench.torque.z, a1 * 0.0 + b1 * wrench_in.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/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp deleted file mode 100644 index 334b243fcc..0000000000 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters.hpp +++ /dev/null @@ -1,72 +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. - -/* - * Authors: Subhas Das, Denis Stogl - */ - -#ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ -#define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ - -#include -#include - -#include "gmock/gmock.h" - -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" - -namespace -{ -const double COMMON_THRESHOLD = 0.001; -} - -// subclassing and friending so we can access member variables -class FriendForceTorqueSensorBroadcasterWithFilter -: public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster -{ - FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, NoFilter_Configure_Success); - FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterLP_Configure_Update); - FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, SingleFilterGC_Configure_Update); - FRIEND_TEST(ForceTorqueSensorBroadcasterWithFilterTest, ChainedFilterLPGC_Configure_Update); -}; - -class ForceTorqueSensorBroadcasterWithFilterTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpFTSBroadcaster(const std::string & name); - -protected: - const std::string sensor_name_ = "fts_sensor"; - const std::string frame_id_ = "fts_sensor_frame"; - std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; - - hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; - hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; - hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; - hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; - hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; - hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; - - std::unique_ptr fts_broadcaster_; - - void subscribe_and_get_message(geometry_msgs::msg::WrenchStamped & wrench_msg); -}; - -#endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_WITH_FILTERS_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml deleted file mode 100644 index 646a14d17b..0000000000 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster_with_filters_parameters.yaml +++ /dev/null @@ -1,57 +0,0 @@ -FTS_NoFilter: - ros__parameters: - sensor_name: "fts_sensor" - frame_id: "fts_sensor_frame" - sensor_filter_chain: "none" - -FTS_SingleFilterLP: - ros__parameters: - sensor_name: "fts_sensor" - frame_id: "fts_sensor_frame" - sensor_filter_chain: - filter1: - type: "control_filters/LowPassFilterWrench" - name: "low_pass_filter" - params: - sampling_frequency: 200.0 - damping_frequency: 50.0 - damping_intensity: 1.0 - divider: 1 - -FTS_SingleFilterGC: - ros__parameters: - sensor_name: "fts_sensor" - frame_id: "fts_sensor_frame" - sensor_filter_chain: - filter1: - type: "control_filters/GravityCompensationWrench" - name: "gravity_compensation" - params: - world_frame: "world" - sensor_frame: "world" - tool: - CoG: [0.0, 0.1, 0.0] - mass: 5.0 - -FTS_ChainedFilterLPGC: - ros__parameters: - sensor_name: "fts_sensor" - frame_id: "fts_sensor_frame" - sensor_filter_chain: - filter1: - type: "control_filters/LowPassFilterWrench" - name: "low_pass_filter" - params: - sampling_frequency: 200.0 - damping_frequency: 50.0 - damping_intensity: 1.0 - divider: 1 - filter2: - type: "control_filters/GravityCompensationWrench" - name: "gravity_compensation" - params: - world_frame: "world" - sensor_frame: "world" - tool: - CoG: [0.0, 0.1, 0.0] - mass: 5.0 From 4062e155b3118742b0de70b6c4c7b68bbe40c10f Mon Sep 17 00:00:00 2001 From: oscarmartinez Date: Fri, 11 Jul 2025 11:16:35 +0200 Subject: [PATCH 24/34] Improving logic to check if there's a filter chain --- .../force_torque_sensor_broadcaster.hpp | 1 + .../src/force_torque_sensor_broadcaster.cpp | 31 +++++++++++-------- .../test_force_torque_sensor_broadcaster.cpp | 1 + 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index c20a59d9ec..848ca14d9d 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -75,6 +75,7 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr Params params_; std::unique_ptr force_torque_sensor_; + bool has_filter_chain_ = false; WrenchMsgType wrench_raw_; WrenchMsgType wrench_filtered_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 222cdd38c2..3763d248b0 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -109,11 +109,11 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is present. // Even if the sensor_filter_chain parameter is not specified, the filter chain will be correctly // configured with an empty list of filters (https://github.com/ros/filters/issues/89). - if (!filter_chain_->configure( + has_filter_chain_ = filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), - get_node()->get_node_parameters_interface())) { - return controller_interface::CallbackReturn::ERROR; - } + get_node()->get_node_parameters_interface()); + + RCLCPP_INFO_EXPRESSION(get_node()->get_logger(), has_filter_chain_, "Filter chain is successfully configured!"); try { @@ -143,9 +143,11 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; realtime_raw_publisher_->unlock(); - realtime_filtered_publisher_->lock(); - realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_filtered_publisher_->unlock(); + if (has_filter_chain_){ + realtime_filtered_publisher_->lock(); + realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_filtered_publisher_->unlock(); + } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -199,13 +201,16 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write realtime_raw_publisher_->unlockAndPublish(); } - // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_ - auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); - if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) + if(has_filter_chain_) { - realtime_filtered_publisher_->msg_.header.stamp = time; - realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; - realtime_filtered_publisher_->unlockAndPublish(); + // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_ + auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); + if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock()) + { + realtime_filtered_publisher_->msg_.header.stamp = time; + realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; + realtime_filtered_publisher_->unlockAndPublish(); + } } return controller_interface::return_type::OK; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index c80ae7c597..fc1e59fc4d 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -519,6 +519,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success) // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); From ad4dcfaf51413dfee80297e3cd3a129eb85ee866 Mon Sep 17 00:00:00 2001 From: Oscar Martinez Date: Mon, 14 Jul 2025 09:27:51 +0200 Subject: [PATCH 25/34] Fixing with pre-commit --- .../src/force_torque_sensor_broadcaster.cpp | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 3763d248b0..398c03926b 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -20,9 +20,9 @@ #include #include -#include #include +#include "rclcpp/logging.hpp" #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -99,21 +99,19 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } catch (const std::exception & e) { - fprintf( - stderr, - "Exception thrown during filter chain creation with message : %s \n", - e.what()); + fprintf(stderr, "Exception thrown during filter chain creation with message : %s \n", e.what()); return CallbackReturn::ERROR; } - // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is present. - // Even if the sensor_filter_chain parameter is not specified, the filter chain will be correctly - // configured with an empty list of filters (https://github.com/ros/filters/issues/89). + // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is + // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be + // correctly configured with an empty list of filters (https://github.com/ros/filters/issues/89). has_filter_chain_ = filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface()); - RCLCPP_INFO_EXPRESSION(get_node()->get_logger(), has_filter_chain_, "Filter chain is successfully configured!"); + RCLCPP_INFO_EXPRESSION( + get_node()->get_logger(), has_filter_chain_, "Filter chain is successfully configured!"); try { @@ -124,7 +122,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( sensor_filtered_state_publisher_ = get_node()->create_publisher( - "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); realtime_filtered_publisher_ = std::make_unique(sensor_filtered_state_publisher_); } @@ -143,7 +141,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id; realtime_raw_publisher_->unlock(); - if (has_filter_chain_){ + if (has_filter_chain_) + { realtime_filtered_publisher_->lock(); realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id; realtime_filtered_publisher_->unlock(); @@ -201,7 +200,7 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write realtime_raw_publisher_->unlockAndPublish(); } - if(has_filter_chain_) + if (has_filter_chain_) { // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_ auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); From 61e9b553dad3220b25c29285453b20630d6a1610 Mon Sep 17 00:00:00 2001 From: Oscar Martinez Date: Tue, 15 Jul 2025 10:54:29 +0200 Subject: [PATCH 26/34] Removing unnecesarily added deps --- force_torque_sensor_broadcaster/CMakeLists.txt | 11 ++--------- .../force_torque_sensor_broadcaster.hpp | 3 --- force_torque_sensor_broadcaster/package.xml | 4 ---- .../src/force_torque_sensor_broadcaster.cpp | 2 -- ros2_controllers-not-released.humble.repos | 2 +- 5 files changed, 3 insertions(+), 19 deletions(-) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3cd1dac720..799d043c79 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -7,7 +7,6 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface - control_toolbox filters generate_parameter_library geometry_msgs @@ -16,9 +15,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools - tf2 - tf2_geometry_msgs - tf2_ros ) find_package(ament_cmake REQUIRED) @@ -48,9 +44,7 @@ target_link_libraries(force_torque_sensor_broadcaster PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle realtime_tools::realtime_tools - ${geometry_msgs_TARGETS} - ${tf2_geometry_msgs_TARGETS} -) + ${geometry_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) @@ -65,9 +59,9 @@ if(BUILD_TESTING) ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp) target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_force_torque_sensor_broadcaster - force_torque_sensor_broadcaster_parameters force_torque_sensor_broadcaster controller_manager::controller_manager + hardware_interface::hardware_interface ros2_control_test_assets::ros2_control_test_assets ) @@ -79,7 +73,6 @@ if(BUILD_TESTING) force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters ) - endif() install( diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 848ca14d9d..1f0b4484b7 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -29,9 +29,6 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index eac77baf84..4ca6bf7817 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -25,7 +25,6 @@ backward_ros controller_interface - control_toolbox filters geometry_msgs hardware_interface @@ -34,9 +33,6 @@ rclcpp_lifecycle realtime_tools generate_parameter_library - tf2 - tf2_geometry_msgs - tf2_ros ament_cmake_gmock controller_manager diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 398c03926b..42a9490f30 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -23,8 +23,6 @@ #include #include "rclcpp/logging.hpp" -#include "tf2/utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace force_torque_sensor_broadcaster { diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 2149abe505..1b3910e7e7 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -1,5 +1,5 @@ repositories: -## EXAMPLE DEPENDENCY + ## EXAMPLE DEPENDENCY # : # type: git # url: git@github.com:/.git From 3afca358ef12b67bd30cb69239ccc2c3828a92ad Mon Sep 17 00:00:00 2001 From: Oscar Martinez Date: Wed, 16 Jul 2025 10:15:49 +0200 Subject: [PATCH 27/34] Improving tests with dummy filter --- .../CMakeLists.txt | 25 +++++++++ .../test/dummy_filter.cpp | 56 +++++++++++++++++++ .../test/dummy_filter_plugin.xml | 9 +++ ...orce_torque_sensor_broadcaster_params.yaml | 11 +--- .../test_force_torque_sensor_broadcaster.cpp | 54 +++++++++++++++--- .../test_force_torque_sensor_broadcaster.hpp | 6 +- 6 files changed, 140 insertions(+), 21 deletions(-) create mode 100644 force_torque_sensor_broadcaster/test/dummy_filter.cpp create mode 100644 force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 799d043c79..6cec7088a3 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -73,6 +73,31 @@ if(BUILD_TESTING) force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters ) + + add_library(dummy_filter SHARED + test/dummy_filter.cpp + ) + target_compile_features(dummy_filter PUBLIC cxx_std_17) + target_include_directories(dummy_filter PUBLIC + $ + $ + ) + target_link_libraries(dummy_filter PUBLIC + filters::increment + ${geometry_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + ) + install( + TARGETS + dummy_filter + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + ) + + pluginlib_export_plugin_description_file(filters "test/dummy_filter_plugin.xml") endif() install( diff --git a/force_torque_sensor_broadcaster/test/dummy_filter.cpp b/force_torque_sensor_broadcaster/test/dummy_filter.cpp new file mode 100644 index 0000000000..3eef7fb3df --- /dev/null +++ b/force_torque_sensor_broadcaster/test/dummy_filter.cpp @@ -0,0 +1,56 @@ + +// Copyright (c) 2025, PAL Robotics +// +// 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. + +/* + * Authors: Oscar Martinez + */ + +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +#include "filters/increment.hpp" + +namespace filters +{ + +template <> +bool IncrementFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!this->configured_) + { + throw std::runtime_error("Filter is not configured"); + } + + // Just increment every value + data_out.wrench.force.x = data_in.wrench.force.x + 1; + data_out.wrench.force.y = data_in.wrench.force.y + 1; + data_out.wrench.force.z = data_in.wrench.force.z + 1; + data_out.wrench.torque.x = data_in.wrench.torque.x + 1; + data_out.wrench.torque.y = data_in.wrench.torque.y + 1; + data_out.wrench.torque.z = data_in.wrench.torque.z + 1; + + return true; +} + +} // namespace filters + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + filters::IncrementFilter, + filters::FilterBase) diff --git a/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml b/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml new file mode 100644 index 0000000000..17623c6e44 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml @@ -0,0 +1,9 @@ + + + + + This is a increment filter which works on a wrench message. + + + + diff --git a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml index 393e777dd5..1c09892ccb 100644 --- a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -1,6 +1,5 @@ test_force_torque_sensor_broadcaster: ros__parameters: - frame_id: "fts_sensor_frame" test_force_torque_sensor_broadcaster_with_chain: ros__parameters: @@ -8,11 +7,5 @@ test_force_torque_sensor_broadcaster_with_chain: sensor_name: "fts_sensor" sensor_filter_chain: filter1: - name: gravity_compensation - type: control_filters/GravityCompensationWrench - params: - sensor_frame: ${WRIST_SIDE_PREFIX}_ft_sensor_link - world_frame: base_footprint - tool: - CoG: [-0.0001, 0.0000, 0.0319] - mass: 0.776 + name: dummy + type: filters/IncrementFilterWrench diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index fc1e59fc4d..99fa93d845 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -24,12 +24,12 @@ #include #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "gtest/gtest.h" #include "hardware_interface/loaned_state_interface.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" + using hardware_interface::LoanedStateInterface; using testing::IsEmpty; using testing::SizeIs; @@ -98,14 +98,6 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( // check if message has been received if (received_msg.get()) { - std::cout << "Received message on topic '" << topic_name << "':" << std::endl; - std::cout << " frame_id: " << received_msg->header.frame_id << std::endl; - std::cout << " force.x: " << received_msg->wrench.force.x << std::endl; - std::cout << " force.y: " << received_msg->wrench.force.y << std::endl; - std::cout << " force.z: " << received_msg->wrench.force.z << std::endl; - std::cout << " torque.x: " << received_msg->wrench.torque.x << std::endl; - std::cout << " torque.y: " << received_msg->wrench.torque.y << std::endl; - std::cout << " torque.z: " << received_msg->wrench.torque.z << std::endl; break; } } @@ -538,6 +530,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_ActivateDeactivate_Su // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check interface configuration @@ -565,11 +558,54 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Update_Success) SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + // Dummy wrenches + geometry_msgs::msg::WrenchStamped wrench_in; + geometry_msgs::msg::WrenchStamped wrench_out; + + wrench_in.wrench.force.x = 1.0; + wrench_in.wrench.force.y = 2.0; + wrench_in.wrench.force.z = 3.0; + wrench_in.wrench.torque.x = 4.0; + wrench_in.wrench.torque.y = 5.0; + wrench_in.wrench.torque.z = 6.0; + + // Update the filter chain directly (the values from the update are just published) + fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out); + + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x + 1.0); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y + 1.0); + ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z + 1.0); +} + +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Publish_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg_filtered; + std::string topic_name = "/test_force_torque_sensor_broadcaster_with_chain/wrench_filtered"; + subscribe_and_get_message(wrench_msg_filtered, topic_name); + + ASSERT_EQ(wrench_msg_filtered.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg_filtered.wrench.force.x, sensor_values_[0] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.force.y, sensor_values_[1] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.force.z, sensor_values_[2] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.x, sensor_values_[3] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.y, sensor_values_[4] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.z, sensor_values_[5] + 1.0); } int main(int argc, char ** argv) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 56a02b68ee..5bd6b62452 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -41,6 +41,9 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_ActivateDeactivate_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Update_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Publish_Success); }; class ForceTorqueSensorBroadcasterTest : public ::testing::Test @@ -70,9 +73,6 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test void subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg, std::string & topic_name); - - void subscribe_additional_frames_and_get_message( - const std::string & frame, geometry_msgs::msg::WrenchStamped & wrench_msg); }; #endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ From 7306d25106b72f16f5cc1156d9a7a35aa19744d6 Mon Sep 17 00:00:00 2001 From: Oscar Martinez Date: Wed, 16 Jul 2025 10:38:34 +0200 Subject: [PATCH 28/34] Removing unnecessary temporary objects created while calling emplace_back --- .../src/force_torque_sensor_broadcaster.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 42a9490f30..cfde29c785 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -253,38 +253,32 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() if (!force_names[0].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[0], &realtime_raw_publisher_->msg_.wrench.force.x)); + export_prefix, force_names[0], &realtime_raw_publisher_->msg_.wrench.force.x); } if (!force_names[1].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[1], &realtime_raw_publisher_->msg_.wrench.force.y)); + export_prefix, force_names[1], &realtime_raw_publisher_->msg_.wrench.force.y); } if (!force_names[2].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[2], &realtime_raw_publisher_->msg_.wrench.force.z)); + export_prefix, force_names[2], &realtime_raw_publisher_->msg_.wrench.force.z); } if (!torque_names[0].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[0], &realtime_raw_publisher_->msg_.wrench.torque.x)); + export_prefix, torque_names[0], &realtime_raw_publisher_->msg_.wrench.torque.x); } if (!torque_names[1].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[1], &realtime_raw_publisher_->msg_.wrench.torque.y)); + export_prefix, torque_names[1], &realtime_raw_publisher_->msg_.wrench.torque.y); } if (!torque_names[2].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z)); + export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z); } return exported_state_interfaces; } From df9fe94b14a070f3f7341abb645e5295ce80ca2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=93scar=20Mart=C3=ADnez=20Mart=C3=ADnez?= Date: Tue, 12 Aug 2025 09:15:06 +0200 Subject: [PATCH 29/34] Remove unnecessary parameter dep MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- force_torque_sensor_broadcaster/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 6cec7088a3..9224a17001 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -71,7 +71,6 @@ if(BUILD_TESTING) target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster force_torque_sensor_broadcaster - force_torque_sensor_broadcaster_parameters ) add_library(dummy_filter SHARED From b8c1af3132071599a3d0390e05c6f8749f8878c8 Mon Sep 17 00:00:00 2001 From: oscarmartinez Date: Tue, 12 Aug 2025 09:56:26 +0200 Subject: [PATCH 30/34] Updated docs --- doc/release_notes.rst | 1 + force_torque_sensor_broadcaster/doc/userdoc.rst | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index cc4a2bd5d4..606d527293 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,6 +7,7 @@ This list summarizes the changes between Jazzy (previous) and Kilted (current) r force_torque_sensor_broadcaster ******************************* * Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__). +* Added support for filter chains, allowing users to configure a sequence of filter plugins with their parameters. The force/torque sensor readings are filtered sequentially and published on a separate topic. joint_trajectory_controller ******************************* diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index df0430e3bb..577f8e8857 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -7,8 +7,11 @@ Force Torque Sensor Broadcaster Broadcaster of messages from force/torque state interfaces of a robot or sensor. The published message type is ``geometry_msgs/msg/WrenchStamped``. -The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package). +The broadcaster also supports filtering the force/torque readings using a filter chain, enabling the sequential application of multiple filters. In this case, an additional topic with the +``_filtered`` suffix will be published containing the final result. For more details on filters, refer to the `filters package repository `_. +See the parameter section for instructions on configuring a filter chain with an arbitrary number of filters. +The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package). Parameters ^^^^^^^^^^^ @@ -17,6 +20,12 @@ This controller uses the `generate_parameter_library `_ library to load each filter at runtime, +passing the specified parameters. + +The chain processes the data sequentially, passing the output of one filter as the input to the next. + Full list of parameters: .. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml From 817c21bef8e03e67603040841aad8ece418d8708 Mon Sep 17 00:00:00 2001 From: Oscar Martinez Date: Mon, 1 Sep 2025 11:38:08 +0200 Subject: [PATCH 31/34] Properly checking chain length --- .../src/force_torque_sensor_broadcaster.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index cfde29c785..149a452809 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -104,12 +104,17 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be // correctly configured with an empty list of filters (https://github.com/ros/filters/issues/89). - has_filter_chain_ = filter_chain_->configure( + bool filter_chain_configured = filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface()); + // Even on successful configure, if empty, the chain won't be used + size_t filter_chain_length = filter_chain_->get_length(); + has_filter_chain_ = filter_chain_configured && filter_chain_length > 0; + RCLCPP_INFO_EXPRESSION( - get_node()->get_logger(), has_filter_chain_, "Filter chain is successfully configured!"); + get_node()->get_logger(), has_filter_chain_, "Filter active with %zu filters!", + filter_chain_length); try { From 06cdbab84e2928e5792dbd44d4a50d4b9f9275c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=93scar=20Mart=C3=ADnez=20Mart=C3=ADnez?= Date: Wed, 3 Sep 2025 09:35:47 +0200 Subject: [PATCH 32/34] Simplify filter check MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/force_torque_sensor_broadcaster.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 149a452809..d6988132e0 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -109,8 +109,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( get_node()->get_node_parameters_interface()); // Even on successful configure, if empty, the chain won't be used - size_t filter_chain_length = filter_chain_->get_length(); - has_filter_chain_ = filter_chain_configured && filter_chain_length > 0; + has_filter_chain_ = filter_chain_configured && filter_chain_->get_length() > 0; RCLCPP_INFO_EXPRESSION( get_node()->get_logger(), has_filter_chain_, "Filter active with %zu filters!", From 3e6d7d159aba227caf7767d55b4220c2cb4de4b4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 3 Sep 2025 10:59:54 +0200 Subject: [PATCH 33/34] Add the filter chain length clarifications MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/force_torque_sensor_broadcaster.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index d6988132e0..88dee22710 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -103,7 +103,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be - // correctly configured with an empty list of filters (https://github.com/ros/filters/issues/89). + // correctly configured with an empty list of filters. bool filter_chain_configured = filter_chain_->configure( "sensor_filter_chain", get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface()); @@ -113,7 +113,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( RCLCPP_INFO_EXPRESSION( get_node()->get_logger(), has_filter_chain_, "Filter active with %zu filters!", - filter_chain_length); + filter_chain_->get_length()); try { From 72916d3db65aebca1e07ffa87251363c50d333ec Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 3 Sep 2025 12:12:18 +0200 Subject: [PATCH 34/34] Fix pre-commit --- force_torque_sensor_broadcaster/doc/userdoc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index 577f8e8857..bfa82d77f6 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -20,8 +20,8 @@ This controller uses the `generate_parameter_library `_ library to load each filter at runtime, +The filter chain is configured as a sequential list of filters under the ``sensor_filter_chain`` parameter, where each filter is identified by the key ``filterN``, with ``N`` representing its position in the chain (e.g., ``filter1``, ``filter2``, etc.). +Each filter entry must specify the ``name`` and ``type`` of the plugin, along with any additional parameters required by that specific filter plugin. The chain will use the `pluginlib `_ library to load each filter at runtime, passing the specified parameters. The chain processes the data sequentially, passing the output of one filter as the input to the next.