From b625c50089d49c8976a5c3e36107b2d9b12d38f9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 30 Oct 2024 13:43:14 +0000 Subject: [PATCH] [JTC] Add Parameter to Toggle State Setting on Activation (backport #1231) (#1320) --- doc/release_notes.rst | 2 ++ .../src/joint_trajectory_controller.cpp | 4 +++- .../src/joint_trajectory_controller_parameters.yaml | 5 +++++ 3 files changed, 10 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 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6dedb2d1d3..504ad9db53 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..ec54363188 100644 --- 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,