From cd21a5d8068a84925bf32671e4ab66a4d7f02d65 Mon Sep 17 00:00:00 2001
From: Kenta Kato <kenta.kato.4321@gmail.com>
Date: Sat, 26 Oct 2024 23:57:38 +0900
Subject: [PATCH] [JTC] Add Parameter to Toggle State Setting on Activation
 (#1231)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* [JTC] Add param to setting last command interface value as state on activation

* [JTC] add a note about set_last_command_interface_value_as_state_on_activation to release_notes. Updated the parameters.yaml description to match the same wording.

---------

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
(cherry picked from commit f96d2fc0fbf94537f769cffcf844858f7a085671)

# Conflicts:
#	joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
---
 doc/release_notes.rst                                      | 2 ++
 .../src/joint_trajectory_controller.cpp                    | 4 +++-
 .../src/joint_trajectory_controller_parameters.yaml        | 7 +++++++
 3 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 4e720ada81..93fd132cd1 100644
--- 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
index d14a5d24c2..907752e9a9 100644
--- 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
index 6fea36752c..eb307621d4 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -64,6 +64,7 @@ joint_trajectory_controller:
     default_value: false,
     description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
   }
+<<<<<<< HEAD
   state_publish_rate: {
     type: double,
     default_value: 50.0,
@@ -71,6 +72,12 @@ joint_trajectory_controller:
     validation: {
       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.",
+>>>>>>> f96d2fc ([JTC] Add Parameter to Toggle State Setting on Activation (#1231))
   }
   action_monitor_rate: {
     type: double,