[JTC] Add Parameter to Toggle State Setting on Activation (backport #…

…1231) (#1320)
ros-controls · Oct 30, 2024 · b625c50 · b625c50
1 parent 2c2a47c
commit b625c50
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 1 deletion.
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
@@ -44,6 +44,8 @@ joint_trajectory_controller
      * -1 - The tolerance is "erased".  If there was a default, the joint will be
             allowed to move without restriction.
 
+* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 <https://github.com/ros-controls/ros2_controllers/pull/1231>`_).
+
 pid_controller
 ************************
 * 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -997,7 +997,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   // running already)
   trajectory_msgs::msg::JointTrajectoryPoint state;
   resize_joint_trajectory_point(state, dof_);
-  if (read_state_from_command_interfaces(state))
+  if (
+    params_.set_last_command_interface_value_as_state_on_activation &&
+    read_state_from_command_interfaces(state))
   {
     state_current_ = state;
     last_commanded_state_ = state;

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -72,6 +72,11 @@ joint_trajectory_controller:
       gt_eq: [0.1]
     }
   }
+  set_last_command_interface_value_as_state_on_activation: {
+    type: bool,
+    default_value: true,
+    description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.",
+  }
   action_monitor_rate: {
     type: double,
     default_value: 20.0,