From cad7894f64ea26fd5c1fe9aa29f067bb5b0304e2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Wed, 13 Sep 2023 11:12:03 +0200
Subject: [PATCH] [JTC] Rename parameter: normalize_error to angle_wraparound
 (#772)

---
 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 199f1b30dc..bb16f5b250 100644
--- a/joint_trajectory_controller/doc/parameters.rst
+++ b/joint_trajectory_controller/doc/parameters.rst
@@ -102,7 +102,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)
@@ -130,10 +130,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 229456c104..ad40fc2da8 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -726,12 +726,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 d299944976..5a1d985a7a 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -113,7 +113,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: {