feature: add support for prefixed joint names in JTC

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<std::string> command_joint_names_;
 
+  // Storing prefixed joint names for command
+  std::vector<std::string> command_full_joint_names_;
+
+  // Storing prefixed joint names for state
+  std::vector<std::string> state_full_joint_names_;
+
   // Parameters from ROS for joint_trajectory_controller
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;

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<std::string>(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<std::string>(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<std::string>(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<std::string>(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<size_t>(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<size_t>(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(),

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: [],