From d3dd21b08d66c152a00331cae3550804926050c3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Denis=20=C5=A0togl?= <denis@stogl.de>
Date: Mon, 25 Jul 2022 16:45:11 +0200
Subject: [PATCH] Use command_joint_states correctly in parameters depending on
 joint states.

---
 .../src/joint_trajectory_controller.cpp                  | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 18fa9f9e9d..0c545f74ac 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -605,12 +605,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     // Init PID gains from ROS parameter server
     for (size_t i = 0; i < pids_.size(); ++i)
     {
-      const std::string prefix = "gains." + joint_names_[i];
+      const std::string prefix = "gains." + command_joint_names_[i];
       const auto k_p = auto_declare<double>(prefix + ".p", 0.0);
       const auto k_i = auto_declare<double>(prefix + ".i", 0.0);
       const auto k_d = auto_declare<double>(prefix + ".d", 0.0);
       const auto i_clamp = auto_declare<double>(prefix + ".i_clamp", 0.0);
-      ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + joint_names_[i], 0.0);
+      ff_velocity_scale_[i] =
+        auto_declare<double>("ff_velocity_scale/" + command_joint_names_[i], 0.0);
       // Initialize PID
       pids_[i] = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
     }
@@ -714,7 +715,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     get_interface_list(command_interface_types_).c_str(),
     get_interface_list(state_interface_types_).c_str());
 
-  default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_);
+  default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_);
 
   // Read parameters customizing controller for special cases
   open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value<bool>();
@@ -750,7 +751,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   state_publisher_ = std::make_unique<StatePublisher>(publisher_);
 
   state_publisher_->lock();
-  state_publisher_->msg_.joint_names = joint_names_;
+  state_publisher_->msg_.joint_names = command_joint_names_;
   state_publisher_->msg_.desired.positions.resize(dof_);
   state_publisher_->msg_.desired.velocities.resize(dof_);
   state_publisher_->msg_.desired.accelerations.resize(dof_);