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..ef73b57e44 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) { @@ -771,6 +771,66 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( 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(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() != command_joint_names_.size()) + { + RCLCPP_ERROR( + logger, + "Size of command prefix (%zu) does not match number of joints (%zu).", + params_.command_prefix.size(), command_joint_names_.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() == 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( + logger, + "Size of state prefix (%zu) does not match number of joints (%zu).", + params_.state_prefix.size(), params_.joints.size()); + return CallbackReturn::FAILURE; + } + + // 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] = 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] = std::string(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 +1094,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 +1108,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: [],