Update per review feedback to simplify param handling

Author: Juliaj

force_torque_sensor_broadcaster/doc/userdoc.rst

@@ -62,8 +62,12 @@ Topics
 ^^^^^^
 The node subscribes to:
 
-- ``<broadcaster_namespace>/wrench`` (raw wrench messages) or ``<broadcaster_namespace>/wrench_filtered`` (filtered wrench messages) depending on the ``use_filtered_input`` parameter
+- ``~/wrench`` (raw wrench messages). To subscribe to filtered wrench messages, use topic remapping: ``ros2 run ... --ros-args -r ~/wrench:=<namespace>/wrench_filtered``
 
 The node publishes:
 
-- ``<output_topic_prefix>_<target_frame>`` for each target frame specified in ``target_frames``
+- ``<namespace>/<target_frame>/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/<target_frame>/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., ``<namespace>/<target_frame>/wrench_filtered``)
+  - This allows users to distinguish between transformed raw wrench data and transformed filtered wrench data

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp

@@ -54,6 +54,14 @@ class WrenchTransformer : public rclcpp::Node
   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<force_torque_wrench_transformer::ParamListener> param_listener_;
   force_torque_wrench_transformer::Params params_;
 
@@ -65,6 +73,7 @@ class WrenchTransformer : public rclcpp::Node
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
   std::string input_topic_;
+  std::string output_topic_suffix_;  // e.g., "" or "_filtered" based on input topic
   std::vector<std::string> target_frames_;
 };
 

force_torque_sensor_broadcaster/src/wrench_transformer.cpp

@@ -112,12 +112,7 @@ bool WrenchTransformer::transform_wrench(
 
 void WrenchTransformer::setup_subscriber()
 {
-  input_topic_ =
-    params_.broadcaster_namespace.empty() ? "~/wrench" : params_.broadcaster_namespace + "/wrench";
-  if (params_.use_filtered_input)
-  {
-    input_topic_ += "_filtered";
-  }
+  input_topic_ = "~/wrench";
   wrench_subscriber_ = this->create_subscription<geometry_msgs::msg::WrenchStamped>(
     input_topic_, rclcpp::SystemDefaultsQoS(),
     std::bind(&WrenchTransformer::wrench_callback, this, std::placeholders::_1));
@@ -127,13 +122,55 @@ void WrenchTransformer::setup_subscriber()
       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 = params_.output_topic_prefix + "_" + target_frame;
+    std::string topic_name = ns + "/" + target_frame + "/wrench" + output_topic_suffix_;
     transformed_wrench_publishers_[target_frame] =
       this->create_publisher<geometry_msgs::msg::WrenchStamped>(
         topic_name, rclcpp::SystemDefaultsQoS());

force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml

@@ -1,14 +1,4 @@
 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: [],
@@ -17,11 +7,6 @@ force_torque_wrench_transformer:
       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,