feat: parameterize controller and broadcaster topics

src/cartesian_admittance_controller.cpp

@@ -460,57 +460,68 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr
   ft_wrench_ = Eigen::VectorXd::Zero(6);
   topic_adm_stiffness_ = Eigen::Matrix<double, 6, 6>::Zero();
 
+  const auto target_pose_topic =
+    params_.topics.target_pose.empty() ? "target_pose" : params_.topics.target_pose;
+  const auto target_joint_topic =
+    params_.topics.target_joint.empty() ? "target_joint" : params_.topics.target_joint;
+  const auto target_wrench_topic =
+    params_.topics.target_wrench.empty() ? "target_wrench" : params_.topics.target_wrench;
+
   // --- Subscriptions (same as CartesianController) ---
   auto target_pose_callback =
-    [this](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {
-    if (!check_topic_publisher_count("target_pose")) {
+    [this, target_pose_topic](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {
+    if (!check_topic_publisher_count(target_pose_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_pose message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_pose_topic.c_str());
       return;
     }
     target_pose_buffer_.writeFromNonRT(msg);
     new_target_pose_ = true;
   };
 
   auto target_joint_callback =
-    [this](const std::shared_ptr<sensor_msgs::msg::JointState> msg) -> void {
-    if (!check_topic_publisher_count("target_joint")) {
+    [this, target_joint_topic](const std::shared_ptr<sensor_msgs::msg::JointState> msg) -> void {
+    if (!check_topic_publisher_count(target_joint_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_joint message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_joint_topic.c_str());
       return;
     }
     target_joint_buffer_.writeFromNonRT(msg);
     new_target_joint_ = true;
   };
 
   auto target_wrench_callback =
-    [this](const std::shared_ptr<geometry_msgs::msg::WrenchStamped> msg) -> void {
-    if (!check_topic_publisher_count("target_wrench")) {
+    [this, target_wrench_topic](
+      const std::shared_ptr<geometry_msgs::msg::WrenchStamped> msg) -> void {
+    if (!check_topic_publisher_count(target_wrench_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_wrench message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_wrench_topic.c_str());
       return;
     }
     target_wrench_buffer_.writeFromNonRT(msg);
     new_target_wrench_ = true;
   };
 
   pose_sub_ = get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
-    "target_pose", rclcpp::QoS(1), target_pose_callback);
+    target_pose_topic, rclcpp::QoS(1), target_pose_callback);
 
   joint_sub_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
-    "target_joint", rclcpp::QoS(1), target_joint_callback);
+    target_joint_topic, rclcpp::QoS(1), target_joint_callback);
 
   wrench_sub_ = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    "target_wrench", rclcpp::QoS(1), target_wrench_callback);
+    target_wrench_topic, rclcpp::QoS(1), target_wrench_callback);
 
   if (params_.variable_stiffness.enabled) {
     auto target_stiffness_callback =

src/cartesian_admittance_controller.yaml

@@ -194,6 +194,20 @@ cartesian_admittance_controller:
     default_value: false
     description: "If true, we use the local jacobian for computations, otherwise we use the world jacobian."
 
+  topics:
+    target_pose:
+      type: string
+      default_value: "target_pose"
+      description: "Topic name for target pose commands."
+    target_joint:
+      type: string
+      default_value: "target_joint"
+      description: "Topic name for target joint commands."
+    target_wrench:
+      type: string
+      default_value: "target_wrench"
+      description: "Topic name for target wrench commands."
+
   log:
     enabled:
       type: bool

src/cartesian_controller.cpp

@@ -364,56 +364,67 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   multiple_publishers_detected_ = false;
   max_allowed_publishers_ = 1;
 
+  const auto target_pose_topic =
+    params_.topics.target_pose.empty() ? "target_pose" : params_.topics.target_pose;
+  const auto target_joint_topic =
+    params_.topics.target_joint.empty() ? "target_joint" : params_.topics.target_joint;
+  const auto target_wrench_topic =
+    params_.topics.target_wrench.empty() ? "target_wrench" : params_.topics.target_wrench;
+
   auto target_pose_callback =
-    [this](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {
-    if (!check_topic_publisher_count("target_pose")) {
+    [this, target_pose_topic](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {
+    if (!check_topic_publisher_count(target_pose_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_pose message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_pose_topic.c_str());
       return;
     }
     target_pose_buffer_.writeFromNonRT(msg);
     new_target_pose_ = true;
   };
 
   auto target_joint_callback =
-    [this](const std::shared_ptr<sensor_msgs::msg::JointState> msg) -> void {
-    if (!check_topic_publisher_count("target_joint")) {
+    [this, target_joint_topic](const std::shared_ptr<sensor_msgs::msg::JointState> msg) -> void {
+    if (!check_topic_publisher_count(target_joint_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_joint message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_joint_topic.c_str());
       return;
     }
     target_joint_buffer_.writeFromNonRT(msg);
     new_target_joint_ = true;
   };
 
   auto target_wrench_callback =
-    [this](const std::shared_ptr<geometry_msgs::msg::WrenchStamped> msg) -> void {
-    if (!check_topic_publisher_count("target_wrench")) {
+    [this, target_wrench_topic](
+      const std::shared_ptr<geometry_msgs::msg::WrenchStamped> msg) -> void {
+    if (!check_topic_publisher_count(target_wrench_topic)) {
       RCLCPP_WARN_THROTTLE(
         get_node()->get_logger(),
         *get_node()->get_clock(),
         1000,
-        "Ignoring target_wrench message due to multiple publishers detected!");
+        "Ignoring message on '%s' due to multiple publishers detected!",
+        target_wrench_topic.c_str());
       return;
     }
     target_wrench_buffer_.writeFromNonRT(msg);
     new_target_wrench_ = true;
   };
 
   pose_sub_ = get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
-    "target_pose", rclcpp::QoS(1), target_pose_callback);
+    target_pose_topic, rclcpp::QoS(1), target_pose_callback);
 
   joint_sub_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
-    "target_joint", rclcpp::QoS(1), target_joint_callback);
+    target_joint_topic, rclcpp::QoS(1), target_joint_callback);
 
   wrench_sub_ = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    "target_wrench", rclcpp::QoS(1), target_wrench_callback);
+    target_wrench_topic, rclcpp::QoS(1), target_wrench_callback);
 
   auto target_stiffness_callback =
     [this](const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) -> void {

src/cartesian_controller.yaml

@@ -194,6 +194,20 @@ cartesian_controller:
     default_value: false
     description: "If true, we use the local jacobian for computations, otherwise we use the world jacobian."
 
+  topics:
+    target_pose:
+      type: string
+      default_value: "target_pose"
+      description: "Topic name for target pose commands."
+    target_joint:
+      type: string
+      default_value: "target_joint"
+      description: "Topic name for target joint commands."
+    target_wrench:
+      type: string
+      default_value: "target_wrench"
+      description: "Topic name for target wrench commands."
+
   log:
     enabled:
       type: bool

src/pose_broadcaster.cpp

@@ -191,8 +191,11 @@ CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*p
   end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
   q = Eigen::VectorXd::Zero(model_.nv);
 
+  const auto current_pose_topic =
+    params_.topics.current_pose.empty() ? "current_pose" : params_.topics.current_pose;
+
   pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
-    "current_pose", rclcpp::SystemDefaultsQoS());
+    current_pose_topic, rclcpp::SystemDefaultsQoS());
   realtime_pose_publisher_ =
     std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>(
       pose_publisher_);

src/pose_broadcaster.yaml

@@ -9,6 +9,11 @@ pose_broadcaster:
   base_frame:
     type: string
     description: "Name of the base frame"
+  topics:
+    current_pose:
+      type: string
+      default_value: "current_pose"
+      description: "Topic name for published end-effector poses."
   publish_frequency:
     type: double
     default_value: 250.0

src/twist_broadcaster.cpp

@@ -198,8 +198,11 @@ CallbackReturn TwistBroadcaster::on_configure(const rclcpp_lifecycle::State & /*
   q = Eigen::VectorXd::Zero(model_.nv);
   q_dot = Eigen::VectorXd::Zero(model_.nv);
 
+  const auto current_twist_topic =
+    params_.topics.current_twist.empty() ? "current_twist" : params_.topics.current_twist;
+
   twist_publisher_ = get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
-    "current_twist", rclcpp::SystemDefaultsQoS());
+    current_twist_topic, rclcpp::SystemDefaultsQoS());
   realtime_twist_publisher_ =
     std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>>(
       twist_publisher_);

src/twist_broadcaster.yaml

@@ -9,6 +9,11 @@ twist_broadcaster:
   base_frame:
     type: string
     description: "Name of the base frame"
+  topics:
+    current_twist:
+      type: string
+      default_value: "current_twist"
+      description: "Topic name for published end-effector twists."
   publish_frequency:
     type: double
     default_value: 250.0