From fa984c934882570979479d77c7be33efa1ac0567 Mon Sep 17 00:00:00 2001 From: Demon_masterlqx <183842220@qq.com> Date: Sun, 16 Nov 2025 18:26:47 +0800 Subject: [PATCH 1/2] feature: add support for prefixed joint names in joint_trajectory_controller for chainable controllers situation --- .../joint_trajectory_controller.hpp | 6 ++ .../src/joint_trajectory_controller.cpp | 56 +++++++++++++++++-- ...oint_trajectory_controller_parameters.yaml | 20 +++++++ 3 files changed, 78 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 05d5fe992c..ea50fe7eb5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -101,6 +101,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Storing command joint names for interfaces std::vector command_joint_names_; + // Storing prefixed joint names for command + std::vector command_full_joint_names_; + + // Storing prefixed joint names for state + std::vector state_full_joint_names_; + // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6661bc802b..076e707a67 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -106,7 +106,7 @@ JointTrajectoryController::command_interface_configuration() const controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size()); - for (const auto & joint_name : command_joint_names_) + for (const auto & joint_name : command_full_joint_names_) { for (const auto & interface_type : params_.command_interfaces) { @@ -122,7 +122,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(dof_ * params_.state_interfaces.size()); - for (const auto & joint_name : params_.joints) + for (const auto & joint_name : state_full_joint_names_) { for (const auto & interface_type : params_.state_interfaces) { @@ -763,6 +763,39 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } + // check command and state prefixes + if(params_.command_prefix.empty()) + { + RCLCPP_INFO( + logger, + "No command prefix specified, will not set prefix for command interfaces."); + params_.command_prefix = std::vector(params_.joints.size(), ""); + } + else if(params_.command_prefix.size() != params_.joints.size()) + { + RCLCPP_ERROR( + logger, + "Size of command prefix (%zu) does not match number of joints (%zu).", + params_.command_prefix.size(), params_.joints.size()); + return CallbackReturn::FAILURE; + } + + if(params_.state_prefix.empty()) + { + RCLCPP_INFO( + logger, + "No state prefix specified, will not set prefix for state interfaces."); + params_.state_prefix = std::vector(params_.joints.size(), ""); + } + else if(params_.state_prefix.size() != params_.joints.size()) + { + RCLCPP_ERROR( + logger, + "Size of state prefix (%zu) does not match number of joints (%zu).", + params_.state_prefix.size(), params_.joints.size()); + return CallbackReturn::FAILURE; + } + command_joint_names_ = params_.command_joints; if (command_joint_names_.empty()) @@ -771,6 +804,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); } + + // construct command_full_joint_names_ / state_full_joint_names_ + command_full_joint_names_.resize(command_joint_names_.size()); + state_full_joint_names_.resize(params_.joints.size()); + for (size_t i = 0; i < command_joint_names_.size(); i++) + { + command_full_joint_names_[i] = (params_.command_prefix[i].empty() ? "" : params_.command_prefix[i] + "/") + + command_joint_names_[i]; + } + for (size_t i = 0; i < params_.joints.size(); i++) + { + state_full_joint_names_[i] = (params_.state_prefix[i].empty() ? "" : params_.state_prefix[i] + "/") + + params_.joints[i]; + } + num_cmd_joints_ = command_joint_names_.size(); if (num_cmd_joints_ > dof_) @@ -1034,7 +1082,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_full_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( logger, "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_, @@ -1048,7 +1096,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( - state_interfaces_, params_.joints, interface, joint_state_interface_[index])) + state_interfaces_, state_full_joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b06c8c2e26..0fe13f53fb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -9,6 +9,26 @@ joint_trajectory_controller: size_gt<>: [0], } } + command_prefix: { + type: string_array, + default_value: [], + description: "Optional prefixes to add to each command interface name. + The size of this array must either be zero or equal to the number of command_joints. + If the size is zero, no prefixing is done. + If the size is 1, the single entry is prepended to all command joint names to form the full command interface name. + If the size is equal to the number of command_joints, each entry is prepended to the corresponding command joint name to form the full command interface name. + This config will not affect the command message, only the interface names used to read the state from the hardware.", + } + state_prefix: { + type: string_array, + default_value: [], + description: "Optional prefixes to add to each state interface name. + The size of this array must either be zero or equal to the number of joints. + If the size is zero, no prefixing is done. + If the size is 1, the single entry is prepended to all joint names to form the full state interface name. + If the size is equal to the number of joints, each entry is prepended to the corresponding joint name to form the full state interface name. + This config will not affect the command message, only the interface names used to read the state from the hardware.", + } command_joints: { type: string_array, default_value: [], From f7da87e78c1290ab711e0752151fecce1e7b4262 Mon Sep 17 00:00:00 2001 From: Demon_masterlqx <183842220@qq.com> Date: Mon, 17 Nov 2025 17:23:33 +0800 Subject: [PATCH 2/2] fix: correctly prepend prefixes to full joint names and pass test --- .../src/joint_trajectory_controller.cpp | 40 ++++++++++++------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 076e707a67..ef73b57e44 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -763,20 +763,35 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } + command_joint_names_ = params_.command_joints; + + if (command_joint_names_.empty()) + { + command_joint_names_ = params_.joints; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + // check command and state prefixes if(params_.command_prefix.empty()) { RCLCPP_INFO( logger, "No command prefix specified, will not set prefix for command interfaces."); - params_.command_prefix = std::vector(params_.joints.size(), ""); + params_.command_prefix = std::vector(command_joint_names_.size(), ""); + } + else if(params_.command_prefix.size() == 1){ + RCLCPP_INFO( + logger, + "Only one command prefix specified, will use it for all command interfaces."); + params_.command_prefix = std::vector(command_joint_names_.size(), params_.command_prefix[0]); } - else if(params_.command_prefix.size() != params_.joints.size()) + else if(params_.command_prefix.size() != command_joint_names_.size()) { RCLCPP_ERROR( logger, "Size of command prefix (%zu) does not match number of joints (%zu).", - params_.command_prefix.size(), params_.joints.size()); + params_.command_prefix.size(), command_joint_names_.size()); return CallbackReturn::FAILURE; } @@ -787,6 +802,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "No state prefix specified, will not set prefix for state interfaces."); params_.state_prefix = std::vector(params_.joints.size(), ""); } + else if(params_.state_prefix.size() == 1){ + RCLCPP_INFO( + logger, + "Only one state prefix specified, will use it for all state interfaces."); + params_.state_prefix = std::vector(params_.joints.size(), params_.state_prefix[0]); + } else if(params_.state_prefix.size() != params_.joints.size()) { RCLCPP_ERROR( @@ -796,26 +817,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - command_joint_names_ = params_.command_joints; - - if (command_joint_names_.empty()) - { - command_joint_names_ = params_.joints; - RCLCPP_INFO( - logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); - } - // construct command_full_joint_names_ / state_full_joint_names_ command_full_joint_names_.resize(command_joint_names_.size()); state_full_joint_names_.resize(params_.joints.size()); for (size_t i = 0; i < command_joint_names_.size(); i++) { - command_full_joint_names_[i] = (params_.command_prefix[i].empty() ? "" : params_.command_prefix[i] + "/") + + command_full_joint_names_[i] = std::string(params_.command_prefix[i].empty() ? "" : params_.command_prefix[i] + "/") + command_joint_names_[i]; } for (size_t i = 0; i < params_.joints.size(); i++) { - state_full_joint_names_[i] = (params_.state_prefix[i].empty() ? "" : params_.state_prefix[i] + "/") + + state_full_joint_names_[i] = std::string(params_.state_prefix[i].empty() ? "" : params_.state_prefix[i] + "/") + params_.joints[i]; }