diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 2f9ae8ebab..defc4f8c84 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -12,6 +12,7 @@ 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. +* Added support for transforming Wrench messages to a given list of target frames. This is useful when applications need force/torque data in their preferred coordinate frames. (`#2021 `__). imu_sensor_broadcaster ******************************* diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 9224a17001..eab7c8dc1a 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -15,6 +15,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + tf2 + tf2_ros + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -27,6 +30,10 @@ generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) +generate_parameter_library(wrench_transformer_parameters + src/wrench_transformer_parameters.yaml +) + add_library(force_torque_sensor_broadcaster SHARED src/force_torque_sensor_broadcaster.cpp ) @@ -49,6 +56,37 @@ target_link_libraries(force_torque_sensor_broadcaster PUBLIC pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) +# Wrench transformer library +add_library(wrench_transformer SHARED + src/wrench_transformer.cpp +) +target_compile_features(wrench_transformer PUBLIC cxx_std_17) +target_include_directories(wrench_transformer PUBLIC + $ + $ +) +target_link_libraries(wrench_transformer PUBLIC + wrench_transformer_parameters + rclcpp::rclcpp + tf2::tf2 + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs + ${geometry_msgs_TARGETS} +) + +# Wrench transformer executable +add_executable(wrench_transformer_node + src/wrench_transformer_main.cpp +) +target_compile_features(wrench_transformer_node PUBLIC cxx_std_17) +target_include_directories(wrench_transformer_node PUBLIC + $ + $ +) +target_link_libraries(wrench_transformer_node PUBLIC + wrench_transformer +) + if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -73,6 +111,16 @@ if(BUILD_TESTING) force_torque_sensor_broadcaster ) + ament_add_gmock(test_wrench_transformer test/test_wrench_transformer.cpp) + target_compile_features(test_wrench_transformer PUBLIC cxx_std_17) + target_include_directories(test_wrench_transformer PRIVATE include) + target_compile_definitions(test_wrench_transformer PRIVATE + TEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + target_link_libraries(test_wrench_transformer + wrench_transformer + tf2_ros::tf2_ros + ) + add_library(dummy_filter SHARED test/dummy_filter.cpp ) @@ -107,12 +155,22 @@ install( TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters + wrench_transformer + wrench_transformer_parameters EXPORT export_force_torque_sensor_broadcaster RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) +# Install executable to lib/ for launch files to find it +install( + TARGETS + wrench_transformer_node + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION lib/force_torque_sensor_broadcaster +) + ament_export_targets(export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/force_torque_sensor_broadcaster/README.md b/force_torque_sensor_broadcaster/README.md index ea8782f183..56e85de50d 100644 --- a/force_torque_sensor_broadcaster/README.md +++ b/force_torque_sensor_broadcaster/README.md @@ -6,3 +6,9 @@ Controller to publish state of force-torque sensors. Pluginlib-Library: force_torque_sensor_broadcaster Plugin: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster (controller_interface::ControllerInterface) + +Wrench Transformer Node +----------------------- +The package also provides a standalone ROS 2 node ``wrench_transformer_node`` that transforms wrench messages from the force_torque_sensor_broadcaster to different target frames using TF2. This allows applications to receive force/torque data in their preferred coordinate frames. + +See the user documentation for details on configuration and usage. diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index bfa82d77f6..cad318633c 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -35,3 +35,39 @@ An example parameter file for this controller can be found in `the test director .. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml :language: yaml + +Wrench Transformer Node +----------------------- +The package provides a standalone ROS 2 node ``wrench_transformer_node`` that transforms wrench messages published by the ``ForceTorqueSensorBroadcaster`` controller to different target frames using TF2. This is useful when applications need force/torque data in coordinate frames other than the sensor frame. + +The node subscribes to wrench messages from the broadcaster (either raw or filtered) and publishes transformed versions to separate topics for each target frame. + +Usage +^^^^^ +The wrench transformer node can be launched as a standalone executable: + +.. code-block:: bash + + ros2 run force_torque_sensor_broadcaster wrench_transformer_node + +Wrench Transformer Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The wrench transformer uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file for the wrench transformer `_ contains descriptions for all the parameters. + +Full list of parameters: + +.. generate_parameter_library_details:: ../src/wrench_transformer_parameters.yaml + +Topics +^^^^^^ +The node subscribes to: + +- ``~/wrench`` (raw wrench messages). To subscribe to filtered wrench messages, use topic remapping: ``ros2 run ... --ros-args -r ~/wrench:=/wrench_filtered`` + +The node publishes: + +- ``//wrench`` for each target frame specified in ``target_frames`` + + - If the node is in the root namespace (``/``), the namespace defaults to the node name (e.g., ``/fts_wrench_transformer//wrench``) + - If the input topic is remapped to a filtered topic (contains "filtered" in the name), the output topics automatically append ``_filtered`` suffix (e.g., ``//wrench_filtered``) + - This allows users to distinguish between transformed raw wrench data and transformed filtered wrench data diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp new file mode 100644 index 0000000000..1fdca54e7c --- /dev/null +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2025, ros2_control development team +// +// 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: Julia Jia + */ + +#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_ +#define FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_ + +#include +#include +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +// auto-generated by generate_parameter_library +#include "force_torque_sensor_broadcaster/wrench_transformer_parameters.hpp" + +namespace force_torque_sensor_broadcaster +{ + +class WrenchTransformer : public rclcpp::Node +{ +public: + explicit WrenchTransformer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + void init(); + + ~WrenchTransformer() = default; + +private: + void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + bool transform_wrench( + const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame, + geometry_msgs::msg::WrenchStamped & output_wrench); + + void setup_subscriber(); + void setup_publishers(); + + /** + * Normalize namespace for topic construction. + * If namespace is empty or root ("/"), returns node name with leading "/". + * Otherwise, normalizes namespace to start with "/" and not end with "/". + * @return Normalized namespace string + */ + std::string normalize_namespace_for_topics() const; + + std::shared_ptr param_listener_; + force_torque_wrench_transformer::Params params_; + + rclcpp::Subscription::SharedPtr wrench_subscriber_; + std::unordered_map::SharedPtr> + transformed_wrench_publishers_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string input_topic_; + std::string output_topic_suffix_; // e.g., "" or "_filtered" based on input topic + std::vector target_frames_; +}; + +// Function to run wrench transformer (extracted from main for testability) +int run_wrench_transformer(int argc, char ** argv); + +} // namespace force_torque_sensor_broadcaster + +#endif // FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index eaf527d155..a0d308e535 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -33,6 +33,9 @@ rclcpp_lifecycle realtime_tools generate_parameter_library + tf2 + tf2_ros + tf2_geometry_msgs ament_cmake_gmock controller_manager diff --git a/force_torque_sensor_broadcaster/src/wrench_transformer.cpp b/force_torque_sensor_broadcaster/src/wrench_transformer.cpp new file mode 100644 index 0000000000..74befb8c00 --- /dev/null +++ b/force_torque_sensor_broadcaster/src/wrench_transformer.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2025, ros2_control development team +// +// 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: Julia Jia + */ + +#include "force_torque_sensor_broadcaster/wrench_transformer.hpp" + +#include +#include +#include + +#include "tf2/utils.hpp" + +namespace force_torque_sensor_broadcaster +{ + +WrenchTransformer::WrenchTransformer(const rclcpp::NodeOptions & options) +: rclcpp::Node("fts_wrench_transformer", options) +{ + // Initialize TF2 + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +void WrenchTransformer::init() +{ + try + { + param_listener_ = + std::make_shared(shared_from_this()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(this->get_logger(), "Exception thrown during initialization: %s", e.what()); + return; + } + + // Setup subscriber and publishers + setup_subscriber(); + setup_publishers(); +} + +void WrenchTransformer::wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) +{ + if (!msg || msg->header.frame_id.empty()) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Invalid wrench message or frame_id is empty"); + return; + } + + for (const auto & target_frame : params_.target_frames) + { + geometry_msgs::msg::WrenchStamped output_wrench; + // preserve timestamp + output_wrench.header.stamp = msg->header.stamp; + output_wrench.header.frame_id = target_frame; + if (transform_wrench(*msg, target_frame, output_wrench)) + { + auto publisher = transformed_wrench_publishers_[target_frame]; + if (publisher) + { + publisher->publish(output_wrench); + } + } + else + { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Failed to transform wrench for frame %s, skipping publication", target_frame.c_str()); + } + } +} + +bool WrenchTransformer::transform_wrench( + const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame, + geometry_msgs::msg::WrenchStamped & output_wrench) +{ + try + { + auto transform = tf_buffer_->lookupTransform( + target_frame, input_wrench.header.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(params_.tf_timeout)); + output_wrench.header.frame_id = target_frame; + tf2::doTransform(input_wrench, output_wrench, transform); + // Preserve original timestamp after transform (doTransform may modify it) + output_wrench.header.stamp = input_wrench.header.stamp; + return true; + } + catch (const tf2::TransformException & e) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Transform exception: %s", e.what()); + return false; + } + return false; +} + +void WrenchTransformer::setup_subscriber() +{ + input_topic_ = "~/wrench"; + wrench_subscriber_ = this->create_subscription( + input_topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&WrenchTransformer::wrench_callback, this, std::placeholders::_1)); + if (wrench_subscriber_ == nullptr) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Failed to create wrench subscriber"); + return; + } + + // Detect if input topic is filtered by checking the actual subscribed topic name + // This handles remapping: if ~/wrench is remapped to wrench_filtered, output should reflect it + try + { + const std::string actual_topic = wrench_subscriber_->get_topic_name(); + if (actual_topic.find("filtered") != std::string::npos) + { + output_topic_suffix_ = "_filtered"; + } + else + { + output_topic_suffix_ = ""; + } + } + catch (const std::exception &) + { + // If we can't get topic name yet, default to empty suffix + output_topic_suffix_ = ""; + } +} + +std::string WrenchTransformer::normalize_namespace_for_topics() const +{ + std::string ns = this->get_namespace(); + // If namespace is empty or root ("/"), use node name as namespace + if (ns.empty() || ns == "/") + { + return "/" + std::string(this->get_name()); + } + // Otherwise, normalize the namespace (ensure it starts with / and doesn't end with /) + if (ns.front() != '/') + { + ns = "/" + ns; + } + if (ns.back() == '/') + { + ns.pop_back(); + } + return ns; +} + +void WrenchTransformer::setup_publishers() +{ + const std::string ns = normalize_namespace_for_topics(); + + for (const auto & target_frame : params_.target_frames) + { + std::string topic_name = ns + "/" + target_frame + "/wrench" + output_topic_suffix_; + transformed_wrench_publishers_[target_frame] = + this->create_publisher( + topic_name, rclcpp::SystemDefaultsQoS()); + if (transformed_wrench_publishers_[target_frame] == nullptr) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Failed to create publisher for target frame %s", target_frame.c_str()); + return; + } + } +} + +int run_wrench_transformer(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + node->init(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} + +} // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/src/wrench_transformer_main.cpp b/force_torque_sensor_broadcaster/src/wrench_transformer_main.cpp new file mode 100644 index 0000000000..46ab262f61 --- /dev/null +++ b/force_torque_sensor_broadcaster/src/wrench_transformer_main.cpp @@ -0,0 +1,24 @@ +// Copyright (c) 2025, ros2_control development team +// +// 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: Julia Jia + */ + +#include "force_torque_sensor_broadcaster/wrench_transformer.hpp" + +int main(int argc, char ** argv) +{ + return force_torque_sensor_broadcaster::run_wrench_transformer(argc, argv); +} diff --git a/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml b/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml new file mode 100644 index 0000000000..fc9dde1694 --- /dev/null +++ b/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml @@ -0,0 +1,14 @@ +force_torque_wrench_transformer: + target_frames: { + type: string_array, + default_value: [], + description: "Array of target frame names to transform the input wrench to. For each frame, a separate output topic will be published.", + validation: { + not_empty<>: null + } + } + tf_timeout: { + type: double, + default_value: 0.1, + description: "Timeout in seconds for TF lookups when transforming wrenches between frames.", + } diff --git a/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp b/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp new file mode 100644 index 0000000000..b837bf2f8d --- /dev/null +++ b/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp @@ -0,0 +1,696 @@ +// Copyright (c) 2025, ros2_control development team +// +// 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: Julia Jia + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "force_torque_sensor_broadcaster/wrench_transformer.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(18, 0, 0) +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#endif +#include "tf2/exceptions.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" + +class TestWrenchTransformer : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + } + + void TearDown() override { rclcpp::shutdown(); } + + std::shared_ptr executor_; + + void setup_static_transform( + const std::string & parent_frame, const std::string & child_frame, double x = 0.0, + double y = 0.0, double z = 0.0, double qx = 0.0, double qy = 0.0, double qz = 0.0, + double qw = 1.0) + { + auto tf_node = std::make_shared("static_tf_broadcaster"); + executor_->add_node(tf_node); +#if RCLCPP_VERSION_GTE(18, 0, 0) + tf2_ros::StaticTransformBroadcaster tf_broadcaster( + rclcpp::node_interfaces::NodeInterfaces( + tf_node->get_node_parameters_interface(), tf_node->get_node_topics_interface())); +#else + tf2_ros::StaticTransformBroadcaster tf_broadcaster(tf_node); +#endif + + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = tf_node->get_clock()->now(); + transform.header.frame_id = parent_frame; + transform.child_frame_id = child_frame; + transform.transform.translation.x = x; + transform.transform.translation.y = y; + transform.transform.translation.z = z; + transform.transform.rotation.x = qx; + transform.transform.rotation.y = qy; + transform.transform.rotation.z = qz; + transform.transform.rotation.w = qw; + + tf_broadcaster.sendTransform(transform); + executor_->spin_some(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + std::shared_ptr create_transformer_node( + const std::vector & target_frames = {"base_link"}, double tf_timeout = 0.1, + const std::string & input_topic_remap = "test_broadcaster/wrench", + const std::string & node_namespace = "") + { + rclcpp::NodeOptions options; + std::vector parameters; + parameters.emplace_back("target_frames", target_frames); + parameters.emplace_back("tf_timeout", tf_timeout); + options.parameter_overrides(parameters); + + // Build arguments for namespace and topic remapping + std::vector args = {"--ros-args"}; + if (!node_namespace.empty()) + { + args.push_back("-r"); + args.push_back("__ns:=" + node_namespace); + } + // Apply topic remapping - when node is namespaced, ~/wrench expands to /wrench + // For tests, we remap to the broadcaster's topic. For filtered topics, remap to wrench_filtered + if (!input_topic_remap.empty()) + { + args.push_back("-r"); + args.push_back("~/wrench:=" + input_topic_remap); + } + if (args.size() > 1) + { + options.arguments(args); + } + + auto node = std::make_shared(options); + executor_->add_node(node); + node->init(); + return node; + } + + void wait_for_discovery() + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(std::chrono::milliseconds(100)); + } + + bool check_publisher_exists_via_graph( + rclcpp::Node::SharedPtr node, const std::string & topic_name) + { + try + { + auto publishers_info = node->get_publishers_info_by_topic(topic_name); + return publishers_info.size() > 0; + } + catch (const std::exception &) + { + return false; + } + } + + void wait_for_publisher( + rclcpp::Subscription::SharedPtr subscriber, + int max_attempts = 20) + { + int attempts = 0; + while (attempts < max_attempts && subscriber->get_publisher_count() == 0) + { + executor_->spin_some(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + attempts++; + } + } + + void wait_for_publisher( + rclcpp::Subscription::SharedPtr subscriber, + rclcpp::Node::SharedPtr query_node, const std::string & topic_name, int max_attempts = 20) + { + int attempts = 0; + while (attempts < max_attempts) + { + // Check both methods: subscription count and graph API + if ( + subscriber->get_publisher_count() > 0 || + check_publisher_exists_via_graph(query_node, topic_name)) + { + return; + } + executor_->spin_some(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + attempts++; + } + } + + geometry_msgs::msg::WrenchStamped create_test_wrench( + rclcpp::Clock::SharedPtr clock, const std::string & frame_id = "sensor_frame") + { + geometry_msgs::msg::WrenchStamped test_msg; + test_msg.header.stamp = clock->now(); + test_msg.header.frame_id = frame_id; + test_msg.wrench.force.x = 1.0; + test_msg.wrench.force.y = 2.0; + test_msg.wrench.force.z = 3.0; + test_msg.wrench.torque.x = 0.1; + test_msg.wrench.torque.y = 0.2; + test_msg.wrench.torque.z = 0.3; + return test_msg; + } + + rclcpp::Publisher::SharedPtr create_input_publisher( + const std::string & topic_name) + { + auto publisher_node = std::make_shared("test_publisher"); + auto publisher = publisher_node->create_publisher( + topic_name, rclcpp::SystemDefaultsQoS()); + executor_->add_node(publisher_node); + return publisher; + } + + rclcpp::Subscription::SharedPtr create_output_subscriber( + const std::string & topic_name, + std::function callback) + { + auto subscriber_node = std::make_shared("test_subscriber"); + auto subscriber = subscriber_node->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), callback); + executor_->add_node(subscriber_node); + return subscriber; + } +}; + +TEST_F(TestWrenchTransformer, NodeInitialization) +{ + rclcpp::NodeOptions options; + std::vector parameters; + parameters.emplace_back("target_frames", std::vector{"base_link"}); + + options.parameter_overrides(parameters); + + auto node = std::make_shared(options); + executor_->add_node(node); + node->init(); + + ASSERT_NE(node, nullptr); + EXPECT_EQ(std::string(node->get_name()), "fts_wrench_transformer"); +} + +TEST_F(TestWrenchTransformer, MultipleTargetFrames) +{ + auto node = create_transformer_node({"base_link", "end_effector"}); + + executor_->spin_some(std::chrono::milliseconds(100)); + + // Verify publishers exist by creating test subscribers + // Use fully qualified topic name since ~/ expands to the transformer node's namespace + auto test_sub_node = std::make_shared("test_sub_check"); + auto base_link_sub = test_sub_node->create_subscription( + "/fts_wrench_transformer/base_link/wrench", rclcpp::SystemDefaultsQoS(), + [](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}); + auto end_effector_sub = test_sub_node->create_subscription( + "/fts_wrench_transformer/end_effector/wrench", rclcpp::SystemDefaultsQoS(), + [](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}); + executor_->add_node(test_sub_node); + + // Spin more aggressively to allow discovery + wait_for_discovery(); + executor_->spin_some(std::chrono::milliseconds(200)); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Wait for each publisher to be discovered + wait_for_publisher(base_link_sub, test_sub_node, "/fts_wrench_transformer/base_link/wrench"); + wait_for_publisher( + end_effector_sub, test_sub_node, "/fts_wrench_transformer/end_effector/wrench"); + + // Final spin to ensure discovery is complete + executor_->spin_some(std::chrono::milliseconds(200)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + ASSERT_NE(node, nullptr); + + // Dump available publishers for debugging if publishers are not found + EXPECT_GT(base_link_sub->get_publisher_count(), 0u) + << "Publisher not found on /fts_wrench_transformer/base_link/wrench"; + EXPECT_GT(end_effector_sub->get_publisher_count(), 0u) + << "Publisher not found on /fts_wrench_transformer/end_effector/wrench"; +} + +TEST_F(TestWrenchTransformer, PublishSubscribeFlow) +{ + // Create transformer node first so its TF listener can receive transforms + auto transformer_node = create_transformer_node(); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Setup static transform from sensor_frame to base_link + setup_static_transform("base_link", "sensor_frame", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + + // Wait longer for TF buffer to receive the transform + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + executor_->spin_some(std::chrono::milliseconds(200)); + + // Create publisher and subscriber using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench"); + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; + auto output_subscriber = create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench", + [&received_msg](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }); + + wait_for_discovery(); + + // Wait for publisher to be discovered by the output subscriber + wait_for_publisher(output_subscriber); + + // Publish test message + auto test_msg = create_test_wrench(transformer_node->get_clock()); + input_publisher->publish(test_msg); + + // Wait for message to be processed + int max_attempts = 20; + while (max_attempts-- > 0 && !received_msg) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Verify message was received and transformed + ASSERT_NE(transformer_node, nullptr); + ASSERT_TRUE(received_msg != nullptr) << "Transformed message was not received"; + EXPECT_EQ(received_msg->header.frame_id, "base_link"); + // Compare timestamp components (tf2::doTransform may modify timestamp slightly) + EXPECT_EQ(received_msg->header.stamp.sec, test_msg.header.stamp.sec); + EXPECT_EQ(received_msg->header.stamp.nanosec, test_msg.header.stamp.nanosec); + // With identity transform, values should be the same + EXPECT_DOUBLE_EQ(received_msg->wrench.force.x, test_msg.wrench.force.x); + EXPECT_DOUBLE_EQ(received_msg->wrench.force.y, test_msg.wrench.force.y); + EXPECT_DOUBLE_EQ(received_msg->wrench.force.z, test_msg.wrench.force.z); + EXPECT_DOUBLE_EQ(received_msg->wrench.torque.x, test_msg.wrench.torque.x); + EXPECT_DOUBLE_EQ(received_msg->wrench.torque.y, test_msg.wrench.torque.y); + EXPECT_DOUBLE_EQ(received_msg->wrench.torque.z, test_msg.wrench.torque.z); +} + +TEST_F(TestWrenchTransformer, NonIdentityTransform) +{ + // Create transformer node first so its TF listener can receive transforms + auto transformer_node = create_transformer_node(); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Setup non-identity transform: 90-degree rotation around z-axis + // Use case: sensors can be mounted rotated (e.g., 90° around z) on links, + // grippers, or end-effectors, requiring rotation transforms. + // Quaternion for 90° rotation around z-axis: For a rotation of angle θ around axis (x,y,z), + // the quaternion is (x*sin(θ/2), y*sin(θ/2), z*sin(θ/2), cos(θ/2)). + // For 90° around z-axis: θ=90°, axis=(0,0,1), so (0, 0, sin(45°), cos(45°)) = (0, 0, 0.7071, + // 0.7071) + setup_static_transform("base_link", "sensor_frame", 0.0, 0.0, 0.0, 0.0, 0.0, 0.7071, 0.7071); + + // Wait longer for TF buffer to receive the transform + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + executor_->spin_some(std::chrono::milliseconds(200)); + + // Create publisher and subscriber using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench"); + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; + auto output_subscriber = create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench", + [&received_msg](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }); + + wait_for_discovery(); + + // Wait for publisher to be discovered by the output subscriber + wait_for_publisher(output_subscriber); + + // Publish test message with known values + auto test_msg = create_test_wrench(transformer_node->get_clock()); + input_publisher->publish(test_msg); + + // Wait for message to be processed + int max_attempts = 20; + while (max_attempts-- > 0 && !received_msg) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Verify message was received and transformed + ASSERT_NE(transformer_node, nullptr); + ASSERT_TRUE(received_msg != nullptr) << "Transformed message was not received"; + EXPECT_EQ(received_msg->header.frame_id, "base_link"); + + // With 90° counterclockwise rotation around z-axis, the transformation is: + // x' = -y, y' = x, z' = z (z-component unchanged since rotation is around z-axis) + // Force: (1.0, 2.0, 3.0) -> x'=-2.0, y'=1.0, z'=3.0 -> (-2.0, 1.0, 3.0) + // Torque: (0.1, 0.2, 0.3) -> x'=-0.2, y'=0.1, z'=0.3 -> (-0.2, 0.1, 0.3) + // Use 1e-4 tolerance to account for floating-point precision in quaternion/transform calculations + EXPECT_NEAR(received_msg->wrench.force.x, -test_msg.wrench.force.y, 1e-4); + EXPECT_NEAR(received_msg->wrench.force.y, test_msg.wrench.force.x, 1e-4); + EXPECT_NEAR(received_msg->wrench.force.z, test_msg.wrench.force.z, 1e-4); + EXPECT_NEAR(received_msg->wrench.torque.x, -test_msg.wrench.torque.y, 1e-4); + EXPECT_NEAR(received_msg->wrench.torque.y, test_msg.wrench.torque.x, 1e-4); + EXPECT_NEAR(received_msg->wrench.torque.z, test_msg.wrench.torque.z, 1e-4); +} + +TEST_F(TestWrenchTransformer, PublishSubscribeMultipleFrames) +{ + // Create transformer node first so its TF listener can receive transforms + auto transformer_node = create_transformer_node({"base_link", "end_effector"}); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Setup static transforms + setup_static_transform("base_link", "sensor_frame", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + setup_static_transform("base_link", "end_effector", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + + // Wait longer for TF buffer to receive the transforms + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + executor_->spin_some(std::chrono::milliseconds(200)); + + // Create publisher and subscribers using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench"); + geometry_msgs::msg::WrenchStamped::SharedPtr received_base_link; + geometry_msgs::msg::WrenchStamped::SharedPtr received_end_effector; + auto base_link_subscriber = create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench", + [&received_base_link](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_base_link = msg; }); + auto end_effector_subscriber = create_output_subscriber( + "/fts_wrench_transformer/end_effector/wrench", + [&received_end_effector](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_end_effector = msg; }); + + wait_for_discovery(); + + // Wait for publishers to be discovered by both subscribers + wait_for_publisher(base_link_subscriber); + wait_for_publisher(end_effector_subscriber); + + // Publish test message + auto test_msg = create_test_wrench(transformer_node->get_clock()); + input_publisher->publish(test_msg); + + // Wait for messages to be processed + int max_attempts = 20; + while (max_attempts-- > 0 && (!received_base_link || !received_end_effector)) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Verify messages were received and transformed + ASSERT_NE(transformer_node, nullptr); + ASSERT_TRUE(received_base_link != nullptr) + << "Transformed message for base_link was not received"; + ASSERT_TRUE(received_end_effector != nullptr) + << "Transformed message for end_effector was not received"; + EXPECT_EQ(received_base_link->header.frame_id, "base_link"); + EXPECT_EQ(received_end_effector->header.frame_id, "end_effector"); +} + +TEST_F(TestWrenchTransformer, TransformFailureNoPublication) +{ + // Create transformer node with target frame + auto transformer_node = create_transformer_node(); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Skip setting up transform - this will cause transform_wrench to return false + // Wait a bit to ensure TF buffer is ready + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Create publisher and subscriber using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench"); + bool message_received = false; + create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench", + [&message_received](const geometry_msgs::msg::WrenchStamped::SharedPtr) + { message_received = true; }); + + wait_for_discovery(); + + // Publish test message with frame_id that has no transform available + auto test_msg = create_test_wrench(transformer_node->get_clock(), "unknown_frame"); + input_publisher->publish(test_msg); + + // Wait for message processing - should NOT receive any message + int max_attempts = 10; + while (max_attempts-- > 0) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Verify transform_wrench would return false by checking transform doesn't exist + auto tf_check_node = std::make_shared("tf_check_node"); + auto tf_buffer = std::make_shared(tf_check_node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + executor_->add_node(tf_check_node); + executor_->spin_some(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + bool transform_exists = false; + try + { + tf_buffer->lookupTransform( + "base_link", "unknown_frame", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + transform_exists = true; + } + catch (const tf2::TransformException &) + { + transform_exists = false; + } + + // Verify no message was received and transform doesn't exist (transform_wrench returned false) + ASSERT_NE(transformer_node, nullptr); + EXPECT_FALSE(transform_exists) + << "Transform should not exist, confirming transform_wrench would return false"; + EXPECT_FALSE(message_received) << "Message should not be published when transform fails"; +} + +TEST_F(TestWrenchTransformer, InitExceptionHandling) +{ + // Test that init() handles exceptions gracefully when ParamListener throws + // Use invalid parameter type to trigger exception in ParamListener + rclcpp::NodeOptions options; + std::vector parameters; + // Provide wrong type for target_frames (string instead of vector) + // This should cause ParamListener to throw when getting parameters + parameters.emplace_back("target_frames", std::string("invalid_type")); + + options.parameter_overrides(parameters); + + auto node = std::make_shared(options); + executor_->add_node(node); + + // init() should catch the exception and return early without crashing + ASSERT_NE(node, nullptr); + EXPECT_NO_THROW(node->init()); +} + +TEST_F(TestWrenchTransformer, InvalidMessageHandling) +{ + // Create transformer node + auto transformer_node = create_transformer_node(); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Setup static transform + setup_static_transform("base_link", "sensor_frame", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + executor_->spin_some(std::chrono::milliseconds(200)); + + // Create publisher and subscriber using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench"); + bool message_received = false; + create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench", + [&message_received](const geometry_msgs::msg::WrenchStamped::SharedPtr) + { message_received = true; }); + + wait_for_discovery(); + + // Test case 1: Publish message with empty frame_id + auto invalid_msg = create_test_wrench(transformer_node->get_clock(), ""); + input_publisher->publish(invalid_msg); + + // Wait for message processing + int max_attempts = 10; + while (max_attempts-- > 0) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Verify no message was published (callback should return early) + ASSERT_NE(transformer_node, nullptr); + EXPECT_FALSE(message_received) << "Message with empty frame_id should not trigger publication"; +} + +TEST_F(TestWrenchTransformer, TopicRemappingFilteredInput) +{ + // Test that transformer subscribes to wrench_filtered topic using topic remapping + auto transformer_node = + create_transformer_node({"base_link"}, 0.1, "test_broadcaster/wrench_filtered"); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Setup static transform + setup_static_transform("base_link", "sensor_frame", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + executor_->spin_some(std::chrono::milliseconds(200)); + + // Create publisher and subscriber using helper functions + auto input_publisher = create_input_publisher("test_broadcaster/wrench_filtered"); + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; + auto output_subscriber = create_output_subscriber( + "/fts_wrench_transformer/base_link/wrench_filtered", + [&received_msg](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }); + + wait_for_discovery(); + + // Wait for publisher to be discovered by the transformer's subscriber + wait_for_publisher(output_subscriber); + + // Publish test message to filtered topic + auto test_msg = create_test_wrench(transformer_node->get_clock()); + input_publisher->publish(test_msg); + + // Wait for message to be processed + int max_attempts = 20; + while (max_attempts-- > 0 && !received_msg) + { + executor_->spin_some(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Verify message was received and transformed from filtered topic + ASSERT_NE(transformer_node, nullptr); + ASSERT_TRUE(received_msg != nullptr) + << "Transformed message from filtered topic was not received"; + EXPECT_EQ(received_msg->header.frame_id, "base_link"); +} + +TEST_F(TestWrenchTransformer, NamespaceNormalizationRootNamespace) +{ + // Test with root namespace ("/") - should use node name + auto node = create_transformer_node({"base_link"}, 0.1, "test_broadcaster/wrench", "/"); + executor_->spin_some(std::chrono::milliseconds(100)); + + // Get the publisher and check its topic name directly + auto topic_names_and_types = node->get_topic_names_and_types(); + bool found = false; + for (const auto & [topic_name, topic_types] : topic_names_and_types) + { + if (topic_name == "/fts_wrench_transformer/base_link/wrench") + { + found = true; + break; + } + } + EXPECT_TRUE(found) << "Publisher topic should be /fts_wrench_transformer/base_link/wrench"; +} + +TEST_F(TestWrenchTransformer, NamespaceNormalizationCustomNamespace) +{ + // Test with custom namespace - should use the namespace + auto node = create_transformer_node({"base_link"}, 0.1, "test_broadcaster/wrench", "/my_robot"); + executor_->spin_some(std::chrono::milliseconds(100)); + + auto topic_names_and_types = node->get_topic_names_and_types(); + bool found = false; + for (const auto & [topic_name, topic_types] : topic_names_and_types) + { + if (topic_name == "/my_robot/base_link/wrench") + { + found = true; + break; + } + } + EXPECT_TRUE(found) << "Publisher topic should be /my_robot/base_link/wrench"; +} + +TEST_F(TestWrenchTransformer, RunWrenchTransformerFunction) +{ + // Directly test the run_wrench_transformer() function extracted from main() + // This tests the actual code path that main() executes + + // Shutdown ROS2 first since SetUp already initialized it + // run_wrench_transformer calls rclcpp::init() internally + rclcpp::shutdown(); + + // Prepare test arguments with parameters (simulating launch with --ros-args) + // This simulates how the node would be launched: ros2 run ... --ros-args -p + // target_frames:=[base_link] + int argc = 4; + char arg0[] = "test_wrench_transformer"; + char arg1[] = "--ros-args"; + char arg2[] = "-p"; + char arg3[] = "target_frames:=[base_link]"; + char * argv[] = {arg0, arg1, arg2, arg3, nullptr}; + + // Test that run_wrench_transformer executes without crashing + // Since it calls rclcpp::spin() which blocks, run it in a separate thread + // and verify it initializes correctly + std::atomic function_started{false}; + std::thread test_thread( + [&]() + { + function_started = true; + // This will block at rclcpp::spin(), but initialization should complete first + force_torque_sensor_broadcaster::run_wrench_transformer(argc, argv); + }); + + // Wait for function to start and initialize (node creation and init() call) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + EXPECT_TRUE(function_started); + + // Shutdown ROS2 to unblock the spin in the thread + // This will cause rclcpp::spin() to return and the function to complete + rclcpp::shutdown(); + + // Wait for thread to complete (run_wrench_transformer should finish after shutdown) + if (test_thread.joinable()) + { + test_thread.join(); + } + + // Re-initialize ROS2 for other tests (TearDown will handle final shutdown) + rclcpp::init(0, nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + int result = RUN_ALL_TESTS(); + return result; +}