diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index a311e03..7d411ee 100644 --- a/src/cartesian_admittance_controller.cpp +++ b/src/cartesian_admittance_controller.cpp @@ -460,15 +460,23 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr ft_wrench_ = Eigen::VectorXd::Zero(6); topic_adm_stiffness_ = Eigen::Matrix::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 msg) -> void { - if (!check_topic_publisher_count("target_pose")) { + [this, target_pose_topic](const std::shared_ptr 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); @@ -476,13 +484,14 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr }; auto target_joint_callback = - [this](const std::shared_ptr msg) -> void { - if (!check_topic_publisher_count("target_joint")) { + [this, target_joint_topic](const std::shared_ptr 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); @@ -490,13 +499,15 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr }; auto target_wrench_callback = - [this](const std::shared_ptr msg) -> void { - if (!check_topic_publisher_count("target_wrench")) { + [this, target_wrench_topic]( + const std::shared_ptr 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); @@ -504,13 +515,13 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr }; pose_sub_ = get_node()->create_subscription( - "target_pose", rclcpp::QoS(1), target_pose_callback); + target_pose_topic, rclcpp::QoS(1), target_pose_callback); joint_sub_ = get_node()->create_subscription( - "target_joint", rclcpp::QoS(1), target_joint_callback); + target_joint_topic, rclcpp::QoS(1), target_joint_callback); wrench_sub_ = get_node()->create_subscription( - "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 = diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml index 4636a55..8bd6366 100644 --- a/src/cartesian_admittance_controller.yaml +++ b/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 diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 18a4668..e8caa3c 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -364,14 +364,22 @@ 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 msg) -> void { - if (!check_topic_publisher_count("target_pose")) { + [this, target_pose_topic](const std::shared_ptr 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); @@ -379,13 +387,14 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta }; auto target_joint_callback = - [this](const std::shared_ptr msg) -> void { - if (!check_topic_publisher_count("target_joint")) { + [this, target_joint_topic](const std::shared_ptr 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); @@ -393,13 +402,15 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta }; auto target_wrench_callback = - [this](const std::shared_ptr msg) -> void { - if (!check_topic_publisher_count("target_wrench")) { + [this, target_wrench_topic]( + const std::shared_ptr 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); @@ -407,13 +418,13 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta }; pose_sub_ = get_node()->create_subscription( - "target_pose", rclcpp::QoS(1), target_pose_callback); + target_pose_topic, rclcpp::QoS(1), target_pose_callback); joint_sub_ = get_node()->create_subscription( - "target_joint", rclcpp::QoS(1), target_joint_callback); + target_joint_topic, rclcpp::QoS(1), target_joint_callback); wrench_sub_ = get_node()->create_subscription( - "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 msg) -> void { diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index a19f68d..01a17c8 100644 --- a/src/cartesian_controller.yaml +++ b/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 diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index a88d460..e0101ed 100644 --- a/src/pose_broadcaster.cpp +++ b/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( - "current_pose", rclcpp::SystemDefaultsQoS()); + current_pose_topic, rclcpp::SystemDefaultsQoS()); realtime_pose_publisher_ = std::make_shared>( pose_publisher_); diff --git a/src/pose_broadcaster.yaml b/src/pose_broadcaster.yaml index 3c687d6..073bf1b 100644 --- a/src/pose_broadcaster.yaml +++ b/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 diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 9e242d2..358758e 100644 --- a/src/twist_broadcaster.cpp +++ b/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( - "current_twist", rclcpp::SystemDefaultsQoS()); + current_twist_topic, rclcpp::SystemDefaultsQoS()); realtime_twist_publisher_ = std::make_shared>( twist_publisher_); diff --git a/src/twist_broadcaster.yaml b/src/twist_broadcaster.yaml index a455667..b636af0 100644 --- a/src/twist_broadcaster.yaml +++ b/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