Make linter happy

Juliaj · Juliaj · commit 254c53e6b8b5 · 2025-11-21T14:47:59.000-08:00
diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt
@@ -112,6 +112,7 @@ if(BUILD_TESTING)
   )
 
   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")
diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst
@@ -66,8 +66,3 @@ The node subscribes to:
 
 The node publishes:
 - ``<output_topic_prefix>_<target_frame>`` for each target frame specified in ``target_frames``
-
-Example parameter file:
-
-.. literalinclude:: ../test/test_wrench_transformer.yaml
-   :language: yaml
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
@@ -48,8 +48,8 @@ class WrenchTransformer : public rclcpp::Node
 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);
+    const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame,
+    geometry_msgs::msg::WrenchStamped & output_wrench);
 
   void setup_subscriber();
   void setup_publishers();
@@ -71,4 +71,3 @@ class WrenchTransformer : public rclcpp::Node
 }  // namespace force_torque_sensor_broadcaster
 
 #endif  // FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
-
diff --git a/force_torque_sensor_broadcaster/src/wrench_transformer.cpp b/force_torque_sensor_broadcaster/src/wrench_transformer.cpp
@@ -39,8 +39,8 @@ void WrenchTransformer::init()
 {
   try
   {
-    param_listener_ = std::make_shared<force_torque_wrench_transformer::ParamListener>(
-      shared_from_this());
+    param_listener_ =
+      std::make_shared<force_torque_wrench_transformer::ParamListener>(shared_from_this());
     params_ = param_listener_->get_params();
   }
   catch (const std::exception & e)
@@ -56,25 +56,32 @@ void WrenchTransformer::init()
 
 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");
+  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) {
+
+  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)) {
+    if (transform_wrench(*msg, target_frame, output_wrench))
+    {
       auto publisher = transformed_wrench_publishers_[target_frame];
-      if (publisher) {
+      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());
+    else
+    {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000,
+        "Failed to transform wrench for frame %s, skipping publication", target_frame.c_str());
     }
   }
 }
@@ -83,48 +90,62 @@ 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));
+  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());
+  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_ = params_.broadcaster_namespace.empty() ? "~/wrench" : params_.broadcaster_namespace + "/wrench";
-  if (params_.use_filtered_input) {
+  input_topic_ =
+    params_.broadcaster_namespace.empty() ? "~/wrench" : params_.broadcaster_namespace + "/wrench";
+  if (params_.use_filtered_input)
+  {
     input_topic_ += "_filtered";
   }
   wrench_subscriber_ = this->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    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");
+    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;
   }
 }
 
 void WrenchTransformer::setup_publishers()
 {
-  for (const auto & target_frame : params_.target_frames) {
+  for (const auto & target_frame : params_.target_frames)
+  {
     std::string topic_name = params_.output_topic_prefix + "_" + target_frame;
-    transformed_wrench_publishers_[target_frame] = this->create_publisher<geometry_msgs::msg::WrenchStamped>(
-      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());
+    transformed_wrench_publishers_[target_frame] =
+      this->create_publisher<geometry_msgs::msg::WrenchStamped>(
+        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;
     }
     RCLCPP_INFO(this->get_logger(), "Created publisher for target frame %s", target_frame.c_str());
   }
 }
 
 }  // 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
@@ -29,4 +29,3 @@ int main(int argc, char ** argv)
   rclcpp::shutdown();
   return 0;
 }
-
diff --git a/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml b/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml
@@ -0,0 +1,29 @@
+force_torque_wrench_transformer:
+  broadcaster_namespace: {
+    type: string,
+    default_value: "",
+    description: "Full namespace path to the force_torque_sensor_broadcaster controller node. The broadcaster publishes geometry_msgs::msg::WrenchStamped messages to topics 'wrench' (raw) and 'wrench_filtered' (filtered). Input topics will be constructed as '<broadcaster_namespace>/wrench' or '<broadcaster_namespace>/wrench_filtered'. If empty, assumes the broadcaster is in the same namespace as this node and uses '~/wrench'.",
+  }
+  use_filtered_input: {
+    type: bool,
+    default_value: false,
+    description: "If true, subscribes to the filtered WrenchStamped topic ('<broadcaster_namespace>/wrench_filtered'), otherwise subscribes to the raw WrenchStamped topic ('<broadcaster_namespace>/wrench'). The filtered topic is only available if the broadcaster has a sensor_filter_chain configured.",
+  }
+  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
+    }
+  }
+  output_topic_prefix: {
+    type: string,
+    default_value: "~/wrench_transformed",
+    description: "Prefix for output topic names. Each output topic publishes transformed WrenchStamped messages in a different frame. Output topics will be named as '<output_topic_prefix>_<frame_name>'. Default uses node namespace.",
+  }
+  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
@@ -18,6 +18,8 @@
 
 #include <gmock/gmock.h>
 #include <chrono>
+#include <filesystem>
+#include <fstream>
 #include <memory>
 #include <string>
 #include <thread>
@@ -45,9 +47,9 @@ class TestWrenchTransformer : public ::testing::Test
   std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> 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)
+    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<rclcpp::Node>("static_tf_broadcaster");
     executor_->add_node(tf_node);
@@ -71,11 +73,9 @@ class TestWrenchTransformer : public ::testing::Test
   }
 
   std::shared_ptr<force_torque_sensor_broadcaster::WrenchTransformer> create_transformer_node(
-    const std::string & broadcaster_namespace = "test_broadcaster",
-    bool use_filtered_input = false,
+    const std::string & broadcaster_namespace = "test_broadcaster", bool use_filtered_input = false,
     const std::vector<std::string> & target_frames = {"base_link"},
-    const std::string & output_topic_prefix = "~/wrench_transformed",
-    double tf_timeout = 0.1)
+    const std::string & output_topic_prefix = "~/wrench_transformed", double tf_timeout = 0.1)
   {
     rclcpp::NodeOptions options;
     std::vector<rclcpp::Parameter> parameters;
@@ -130,7 +130,7 @@ TEST_F(TestWrenchTransformer, NodeInitialization)
   rclcpp::NodeOptions options;
   std::vector<rclcpp::Parameter> parameters;
   parameters.emplace_back("target_frames", std::vector<std::string>{"base_link"});
-  
+
   options.parameter_overrides(parameters);
 
   auto node = std::make_shared<force_torque_sensor_broadcaster::WrenchTransformer>(options);
@@ -143,14 +143,23 @@ TEST_F(TestWrenchTransformer, NodeInitialization)
 
 TEST_F(TestWrenchTransformer, ParameterLoadingFromYamlFile)
 {
+  // Construct absolute path to YAML file
+  std::filesystem::path yaml_path =
+    std::filesystem::path(TEST_FILES_DIRECTORY) / "test_wrench_transformer.yaml";
+  yaml_path = std::filesystem::canonical(yaml_path);
+  const std::string yaml_file_path = yaml_path.string();
+
+  // Verify file exists before trying to load it
+  ASSERT_TRUE(std::filesystem::exists(yaml_path)) << "YAML file not found at: " << yaml_file_path;
+
   rclcpp::NodeOptions options;
-  options.arguments({"--ros-args", "--params-file", std::string(TEST_FILES_DIRECTORY) + "/test_wrench_transformer.yaml"});
+  options.arguments({"--ros-args", "--params-file", yaml_file_path});
   auto node = std::make_shared<force_torque_sensor_broadcaster::WrenchTransformer>(options);
   executor_->add_node(node);
   node->init();
-  
+
   ASSERT_NE(node, nullptr);
-  
+
   // Verify parameters were loaded from YAML
   EXPECT_EQ(node->get_parameter("broadcaster_namespace").as_string(), "test_broadcaster");
   EXPECT_EQ(node->get_parameter("use_filtered_input").as_bool(), false);
@@ -164,9 +173,7 @@ TEST_F(TestWrenchTransformer, ParameterLoadingFromYamlFile)
 
 TEST_F(TestWrenchTransformer, MultipleTargetFrames)
 {
-  auto node = create_transformer_node(
-    "test_broadcaster", false,
-    {"base_link", "end_effector"});
+  auto node = create_transformer_node("test_broadcaster", false, {"base_link", "end_effector"});
 
   executor_->spin_some(std::chrono::milliseconds(100));
 
@@ -188,8 +195,10 @@ TEST_F(TestWrenchTransformer, MultipleTargetFrames)
   wait_for_publisher(end_effector_sub);
 
   ASSERT_NE(node, nullptr);
-  EXPECT_GT(base_link_sub->get_publisher_count(), 0u) << "Publisher not found on /fts_wrench_transformer/wrench_transformed_base_link";
-  EXPECT_GT(end_effector_sub->get_publisher_count(), 0u) << "Publisher not found on /fts_wrench_transformer/wrench_transformed_end_effector";
+  EXPECT_GT(base_link_sub->get_publisher_count(), 0u)
+    << "Publisher not found on /fts_wrench_transformer/wrench_transformed_base_link";
+  EXPECT_GT(end_effector_sub->get_publisher_count(), 0u)
+    << "Publisher not found on /fts_wrench_transformer/wrench_transformed_end_effector";
 }
 
 TEST_F(TestWrenchTransformer, PublishSubscribeFlow)
@@ -216,7 +225,8 @@ TEST_F(TestWrenchTransformer, PublishSubscribeFlow)
   auto subscriber_node = std::make_shared<rclcpp::Node>("test_subscriber");
   auto output_subscriber = subscriber_node->create_subscription<geometry_msgs::msg::WrenchStamped>(
     "/fts_wrench_transformer/wrench_transformed_base_link", rclcpp::SystemDefaultsQoS(),
-    [&received_msg](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { received_msg = msg; });
+    [&received_msg](const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
+    { received_msg = msg; });
   executor_->add_node(subscriber_node);
 
   wait_for_discovery();
@@ -252,8 +262,8 @@ TEST_F(TestWrenchTransformer, PublishSubscribeFlow)
 TEST_F(TestWrenchTransformer, PublishSubscribeMultipleFrames)
 {
   // Create transformer node first so its TF listener can receive transforms
-  auto transformer_node = create_transformer_node(
-    "test_broadcaster", false, {"base_link", "end_effector"});
+  auto transformer_node =
+    create_transformer_node("test_broadcaster", false, {"base_link", "end_effector"});
   executor_->spin_some(std::chrono::milliseconds(100));
 
   // Setup static transforms
@@ -273,14 +283,18 @@ TEST_F(TestWrenchTransformer, PublishSubscribeMultipleFrames)
   // Create subscriber nodes to receive output messages for both frames
   geometry_msgs::msg::WrenchStamped::SharedPtr received_base_link;
   geometry_msgs::msg::WrenchStamped::SharedPtr received_end_effector;
-  
+
   auto subscriber_node = std::make_shared<rclcpp::Node>("test_subscriber");
-  auto base_link_subscriber = subscriber_node->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    "/fts_wrench_transformer/wrench_transformed_base_link", rclcpp::SystemDefaultsQoS(),
-    [&received_base_link](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { received_base_link = msg; });
-  auto end_effector_subscriber = subscriber_node->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    "/fts_wrench_transformer/wrench_transformed_end_effector", rclcpp::SystemDefaultsQoS(),
-    [&received_end_effector](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { received_end_effector = msg; });
+  auto base_link_subscriber =
+    subscriber_node->create_subscription<geometry_msgs::msg::WrenchStamped>(
+      "/fts_wrench_transformer/wrench_transformed_base_link", rclcpp::SystemDefaultsQoS(),
+      [&received_base_link](const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
+      { received_base_link = msg; });
+  auto end_effector_subscriber =
+    subscriber_node->create_subscription<geometry_msgs::msg::WrenchStamped>(
+      "/fts_wrench_transformer/wrench_transformed_end_effector", rclcpp::SystemDefaultsQoS(),
+      [&received_end_effector](const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
+      { received_end_effector = msg; });
   executor_->add_node(subscriber_node);
 
   wait_for_discovery();
@@ -299,8 +313,10 @@ TEST_F(TestWrenchTransformer, PublishSubscribeMultipleFrames)
 
   // 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";
+  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");
 }

Original file line number	Diff line number	Diff line change
`@@ -112,6 +112,7 @@ if(BUILD_TESTING)`
`112`	`112`	`)`
`113`	`113`
`114`	`114`	`ament_add_gmock(test_wrench_transformer test/test_wrench_transformer.cpp)`
	`115`	`+ target_compile_features(test_wrench_transformer PUBLIC cxx_std_17)`
`115`	`116`	`target_include_directories(test_wrench_transformer PRIVATE include)`
`116`	`117`	`target_compile_definitions(test_wrench_transformer PRIVATE`
`117`	`118`	`TEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")`
Original file line number	Diff line number	Diff line change
`@@ -29,4 +29,3 @@ int main(int argc, char ** argv)`
`29`	`29`	`rclcpp::shutdown();`
`30`	`30`	`return 0;`
`31`	`31`	`}`
`32`		`-`