Use command joints instead of joints for gains structure

Author: christophfroehlich

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -134,6 +134,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   // Storing command joint names for interfaces
   std::vector<std::string> command_joint_names_;
+  // TODO(anyone) remove this if there is another way to lock command_joints parameter
+  rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names;
 
   // Parameters from ROS for joint_trajectory_controller
   std::shared_ptr<ParamListener> param_listener_;

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -690,6 +690,36 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     command_joint_names_ = params_.joints;
     RCLCPP_INFO(
       logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
+
+    // set the parameter for the controller plugin
+    auto result =
+      get_node()->set_parameter(rclcpp::Parameter("command_joints", command_joint_names_));
+    if (result.successful == false)
+    {
+      RCLCPP_ERROR(logger, "Failed to set 'command_joints' parameter");
+      return CallbackReturn::FAILURE;
+    }
+    // TODO(christophfroehlich) how to lock the parameter (set read_only to false)?
+    // Setting it to read_only but override is not supported
+    // https://github.com/ros2/rclcpp/issues/1762 get_node()->undeclare_parameter("command_joints");
+    // rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
+    // parameter_descriptor.read_only = true;
+    // get_node()->declare_parameter("command_joints",
+    //  rclcpp::ParameterValue(command_joint_names_), parameter_descriptor);
+    lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback(
+      [this](std::vector<rclcpp::Parameter> & parameters)
+      {
+        for (auto & parameter : parameters)
+        {
+          if (parameter.get_name() == "command_joints")
+          {
+            RCLCPP_ERROR(
+              get_node()->get_logger(),
+              "The parameter 'command_joints' is read-only. You can't change it.");
+            parameter = rclcpp::Parameter("command_joints", command_joint_names_);
+          }
+        }
+      });
   }
   else if (command_joint_names_.size() != params_.joints.size())
   {

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -12,7 +12,7 @@ joint_trajectory_controller:
     type: string_array,
     default_value: [],
     description: "Names of the commanding joints used by the controller",
-    read_only: true,
+    read_only: false, # should be set to true after configuration of the controller
     validation: {
       unique<>: null,
     }

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -86,32 +86,18 @@ class TestableJointTrajectoryController
     return success;
   }
 
-  void set_joint_names(const std::vector<std::string> & joint_names)
-  {
-    params_.joints = joint_names;
-  }
-
   void set_command_joint_names(const std::vector<std::string> & command_joint_names)
   {
     command_joint_names_ = command_joint_names;
   }
 
-  void set_command_interfaces(const std::vector<std::string> & command_interfaces)
-  {
-    params_.command_interfaces = command_interfaces;
-  }
-
-  void set_state_interfaces(const std::vector<std::string> & state_interfaces)
-  {
-    params_.state_interfaces = state_interfaces;
-  }
-
   void trigger_declare_parameters() { param_listener_->declare_params(); }
 
   trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const
   {
     return last_commanded_state_;
   }
+
   bool has_position_state_interface() const { return has_position_state_interface_; }
 
   bool has_velocity_state_interface() const { return has_velocity_state_interface_; }
@@ -227,12 +213,15 @@ class TrajectoryControllerTest : public ::testing::Test
     }
   }
 
+  /**
+   * @brief set PIDs for every entry in joint_names_
+   * be aware to update if PIDs should be configured for different command_joints than joint_names
+   */
   void SetPidParameters(double p_default = 0.0, double ff_default = 1.0)
   {
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
 
-    // TODO(christophfroehlich): use command joints instead of joint_names
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
       const std::string prefix = "gains." + joint_names_[i];

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -75,7 +75,7 @@ void PidTrajectoryPlugin::updateGains()
       node_->get_logger(), "[PidTrajectoryPlugin] command_joint_names_ %lu : %s", i,
       command_joint_names_[i].c_str());
 
-    const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]);
+    const auto & gains = params_.gains.command_joints_map.at(command_joint_names_[i]);
     pids_[i] = std::make_shared<control_toolbox::Pid>(
       gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
     ff_velocity_scale_[i] = gains.ff_velocity_scale;

joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml

@@ -1,15 +1,15 @@
 joint_trajectory_controller_plugins:
-  joints: {
+  command_joints: {
     type: string_array,
     default_value: [],
-    description: "Names of joints used by the controller",
+    description: "Names of joints used by the controller plugin",
     read_only: true,
     validation: {
       size_gt<>: [0],
     }
   }
   gains:
-    __map_joints:
+    __map_command_joints:
       p: {
         type: double,
         default_value: 0.0,

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -31,7 +31,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
 
   // override read_only parameter
-  node_->declare_parameter("joints", joint_names_paramv);
+  node_->declare_parameter("command_joints", joint_names_paramv);
 
   ASSERT_TRUE(traj_contr->initialize(node_, joint_names));
 
@@ -63,7 +63,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
 
   // override read_only parameter
-  node_->declare_parameter("joints", joint_names_paramv);
+  node_->declare_parameter("command_joints", joint_names_paramv);
 
   ASSERT_TRUE(traj_contr->initialize(node_, joint_names));