diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 697af84971..2f9ae8ebab 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -11,6 +11,7 @@ chained_filter_controller 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. imu_sensor_broadcaster ******************************* diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index c7d8e3beb8..9224a17001 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -7,6 +7,7 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + filters generate_parameter_library geometry_msgs hardware_interface @@ -37,6 +38,7 @@ target_include_directories(force_torque_sensor_broadcaster PUBLIC target_link_libraries(force_torque_sensor_broadcaster PUBLIC force_torque_sensor_broadcaster_parameters controller_interface::controller_interface + filters::filter_chain hardware_interface::hardware_interface pluginlib::pluginlib rclcpp::rclcpp @@ -70,6 +72,31 @@ if(BUILD_TESTING) target_link_libraries(test_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) + + 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/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index df0430e3bb..bfa82d77f6 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 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 0c8ce438f1..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 @@ -20,18 +20,23 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include #include #include "controller_interface/chainable_controller_interface.hpp" -// auto-generated by generate_parameter_library -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" +// auto-generated by generate_parameter_library +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" + namespace force_torque_sensor_broadcaster { +using WrenchMsgType = geometry_msgs::msg::WrenchStamped; + class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: @@ -67,10 +72,18 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr Params params_; std::unique_ptr force_torque_sensor_; - - using StatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr sensor_state_publisher_; - std::unique_ptr realtime_publisher_; + bool has_filter_chain_ = false; + + 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_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4fbe884762..48c32ce77f 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -25,6 +25,7 @@ backward_ros controller_interface + 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 6ddd94ca72..88dee22710 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -18,9 +18,12 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include #include #include +#include "rclcpp/logging.hpp" + namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() @@ -87,12 +90,43 @@ 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 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. + 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 + 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!", + filter_chain_->get_length()); + 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) { @@ -102,9 +136,19 @@ 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 = 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; + 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(); + } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -146,13 +190,28 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write { param_listener_->try_get_params(params_); - if (realtime_publisher_ && realtime_publisher_->trylock()) + wrench_raw_.header.stamp = time; + force_torque_sensor_->get_values_as_message(wrench_raw_.wrench); + this->apply_sensor_offset(params_, wrench_raw_); + this->apply_sensor_multiplier(params_, wrench_raw_); + + if (realtime_raw_publisher_ && realtime_raw_publisher_->trylock()) + { + realtime_raw_publisher_->msg_.header.stamp = time; + realtime_raw_publisher_->msg_.wrench = wrench_raw_.wrench; + realtime_raw_publisher_->unlockAndPublish(); + } + + if (has_filter_chain_) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - this->apply_sensor_offset(params_, realtime_publisher_->msg_); - this->apply_sensor_multiplier(params_, realtime_publisher_->msg_); - realtime_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; @@ -198,38 +257,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_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_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_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_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_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_publisher_->msg_.wrench.torque.z)); + export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z); } return exported_state_interfaces; } 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 47c36b3b14..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 @@ -112,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." + } 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 58cc1c6b16..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,4 +1,11 @@ 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: 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 4d3e62379b..017928888f 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,6 +19,7 @@ #include "test_force_torque_sensor_broadcaster.hpp" #include +#include #include #include @@ -28,6 +29,7 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" + using hardware_interface::LoanedStateInterface; using testing::IsEmpty; using testing::SizeIs; @@ -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()); @@ -110,7 +111,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( 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); @@ -118,7 +119,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_}); @@ -133,7 +134,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", ""}); @@ -144,7 +145,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", ""}); @@ -156,7 +157,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_}); @@ -178,7 +179,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"}); @@ -195,7 +196,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_}); @@ -227,7 +228,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_}); @@ -243,7 +244,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"}); @@ -259,7 +260,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_}); @@ -269,7 +270,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]); @@ -282,7 +284,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}}; @@ -300,7 +302,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]); @@ -344,7 +347,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}}; @@ -368,7 +371,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_); @@ -414,7 +418,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipl 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"}); @@ -425,7 +429,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]); @@ -451,7 +456,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"}); @@ -466,7 +471,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]); @@ -499,6 +505,109 @@ 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); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + + // 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_->has_filter_chain_, true); + 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_->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) { ::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 5d485d9d12..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 @@ -39,6 +39,11 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); 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 @@ -50,7 +55,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 +71,8 @@ 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); }; #endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_