Fix upstream Pid class deprecation warnings (#1959)

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1764,8 +1764,9 @@ void JointTrajectoryController::update_pids()
     const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
     control_toolbox::AntiWindupStrategy antiwindup_strat;
     antiwindup_strat.set_type(gains.antiwindup_strategy);
-    antiwindup_strat.i_max = gains.i_clamp;
-    antiwindup_strat.i_min = -gains.i_clamp;
+    // deprecated parameter i_clamp: i_clamp_max and i_clamp_min have precedence over i_clamp
+    antiwindup_strat.i_max = std::isfinite(gains.i_clamp_max) ? gains.i_clamp_max : gains.i_clamp;
+    antiwindup_strat.i_min = std::isfinite(gains.i_clamp_min) ? gains.i_clamp_min : -gains.i_clamp;
     antiwindup_strat.error_deadband = gains.error_deadband;
     antiwindup_strat.tracking_time_constant = gains.tracking_time_constant;
     if (pids_[i])

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -132,8 +132,18 @@ joint_trajectory_controller:
       }
       i_clamp: {
         type: double,
-        default_value: 0.0,
-        description: "Integral clamp. Symmetrical in both positive and negative direction."
+        default_value: .inf,
+        description: "[Deprecated, use i_clamp_max/i_clamp_min] Integral clamp. Symmetrical in both positive and negative direction."
+      }
+      i_clamp_max: {
+        type: double,
+        default_value: .inf,
+        description: "Upper integral clamp."
+      }
+      i_clamp_min: {
+        type: double,
+        default_value: -.inf,
+        description: "Lower integral clamp."
       }
       ff_velocity_scale: {
         type: double,
@@ -150,16 +160,6 @@ joint_trajectory_controller:
         default_value: -.inf,
         description: "Lower output clamp."
       }
-      i_clamp_max: {
-        type: double,
-        default_value: 0.0,
-        description: "[Deprecated, see antiwindup_strategy] Upper integral clamp."
-      }
-      i_clamp_min: {
-        type: double,
-        default_value: 0.0,
-        description: "[Deprecated, see antiwindup_strategy] Lower integral clamp."
-      }
       antiwindup_strategy: {
         type: string,
         default_value: "legacy",

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -191,6 +191,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   SetUpTrajectoryController(executor);
+  SetPidParameters(0.0, 1.0);
   traj_controller_->get_node()->set_parameter(
     rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
   traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10));

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -279,11 +279,18 @@ class TrajectoryControllerTest : public ::testing::Test
     traj_controller_ = std::make_shared<TestableJointTrajectoryController>();
 
     auto node_options = rclcpp::NodeOptions();
+    // read-only parameters have to be set before init
     std::vector<rclcpp::Parameter> parameter_overrides;
     parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_));
     parameter_overrides.push_back(
       rclcpp::Parameter("command_interfaces", command_interface_types_));
     parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_));
+    // avoid deprecation warning for legacy (default) antiwindup strategy
+    for (const auto & joint : joint_names_)
+    {
+      parameter_overrides.push_back(
+        rclcpp::Parameter("gains." + joint + ".antiwindup_strategy", "none"));
+    }
     parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
     node_options.parameter_overrides(parameter_overrides);
     traj_controller_->set_node_options(node_options);
@@ -303,7 +310,7 @@ class TrajectoryControllerTest : public ::testing::Test
       const rclcpp::Parameter k_p(prefix + ".p", p_value);
       const rclcpp::Parameter k_i(prefix + ".i", 0.0);
       const rclcpp::Parameter k_d(prefix + ".d", 0.0);
-      const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
+      const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 1000.0);
       const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value);
       node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale});
     }

pid_controller/src/pid_controller.yaml

@@ -85,12 +85,12 @@ pid_controller:
       }
       i_clamp_max: {
         type: double,
-        default_value: 0.0,
+        default_value: .inf,
         description: "[Deprecated, see antiwindup_strategy] Upper integral clamp."
       }
       i_clamp_min: {
         type: double,
-        default_value: 0.0,
+        default_value: -.inf,
         description: "[Deprecated, see antiwindup_strategy] Lower integral clamp."
       }
       antiwindup_strategy: {

pid_controller/test/pid_controller_params.yaml

@@ -2,8 +2,8 @@
   ros__parameters:
 # TODO(christohfroehlich): Remove this global parameters once the deprecated antiwindup parameters are removed.
     gains:
-      joint1: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf}
-      joint2: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf}
+      joint1: {antiwindup_strategy: "none",}
+      joint2: {antiwindup_strategy: "none",}
 
 test_pid_controller:
   ros__parameters:

pid_controller/test/pid_controller_preceding_params.yaml

@@ -9,3 +9,6 @@ test_pid_controller:
 
     reference_and_state_dof_names:
       - joint1state
+
+    gains:
+      joint1: {antiwindup_strategy: "none"}