From faa08e26be110dfb5b07b14737cad5c71a84ebb0 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Fri, 15 Sep 2023 11:40:58 +0100
Subject: [PATCH] [JTC] Rename parameter: normalize_error to angle_wraparound
 (#772) (#776)

---
 joint_trajectory_controller/doc/parameters.rst      | 10 ++++++----
 .../src/joint_trajectory_controller.cpp             | 13 +++++++++++--
 .../src/joint_trajectory_controller_parameters.yaml |  8 +++++++-
 3 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst
index 8be135ff4b..f62320870c 100644
--- a/joint_trajectory_controller/doc/parameters.rst
+++ b/joint_trajectory_controller/doc/parameters.rst
@@ -100,7 +100,7 @@ gains (structure)
 
      u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v)
 
-  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
+  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below),
   the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
 
 gains.<joint_name>.p (double)
@@ -128,10 +128,12 @@ gains.<joint_name>.ff_velocity_scale (double)
 
   Default: 0.0
 
-gains.<joint_name>.normalize_error (bool)
+gains.<joint_name>.angle_wraparound (bool)
+  For joints that wrap around (without end stop, ie. are continuous),
+  where the shortest rotation to the target position is the desired motion.
   If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`.
   Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
-  position :math:`s` from the state interface. Use this for revolute joints without end stop,
-  where the shortest rotation to the target position is the desired motion.
+  position :math:`s` from the state interface.
+
 
   Default: false
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index d85d23b37b..cff2918b75 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -722,12 +722,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     }
   }
 
-  // Configure joint position error normalization from ROS parameters
+  // Configure joint position error normalization from ROS parameters (angle_wraparound)
   normalize_joint_error_.resize(dof_);
   for (size_t i = 0; i < dof_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
-    normalize_joint_error_[i] = gains.normalize_error;
+    if (gains.normalize_error)
+    {
+      // TODO(anyone): Remove deprecation warning in the end of 2023
+      RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!");
+      normalize_joint_error_[i] = gains.normalize_error;
+    }
+    else
+    {
+      normalize_joint_error_[i] = gains.angle_wraparound;
+    }
   }
 
   if (params_.state_interfaces.empty())
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index a754929ef7..bb0b45bb80 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -109,7 +109,13 @@ joint_trajectory_controller:
       normalize_error: {
         type: bool,
         default_value: false,
-        description: "Use position error normalization to -pi to pi."
+        description: "(Deprecated) Use position error normalization to -pi to pi."
+      }
+      angle_wraparound: {
+        type: bool,
+        default_value: false,
+        description: "For joints that wrap around (ie. are continuous).
+          Normalizes position-error to -pi to pi."
       }
   constraints:
     stopped_velocity_tolerance: {