Change the tests to work without deprecated PID settings (#1824)

(cherry picked from commit bf253f1)

# Conflicts:
#	pid_controller/test/test_pid_controller.cpp
#	pid_controller/test/test_pid_controller.hpp
#	pid_controller/test/test_pid_controller_dual_interface.cpp

Author: christophfroehlich

pid_controller/test/pid_controller_params.yaml

@@ -1,3 +1,10 @@
+/**:
+  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}
+
 test_pid_controller:
   ros__parameters:
     dof_names:
@@ -8,7 +15,19 @@ test_pid_controller:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
+      joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0}
+
+test_pid_controller_unlimited:
+  ros__parameters:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    reference_and_state_interfaces: ["position"]
+
+    gains:
+      joint1: {p: 1.0, i: 2.0, d: 3.0}
 
 test_pid_controller_angle_wraparound_on:
   ros__parameters:
@@ -20,7 +39,7 @@ test_pid_controller_angle_wraparound_on:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, angle_wraparound: true}
+      joint1: {p: 1.0, i: 2.0, d: 3.0, angle_wraparound: true}
 
 test_pid_controller_with_feedforward_gain:
   ros__parameters:
@@ -32,7 +51,7 @@ test_pid_controller_with_feedforward_gain:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}
+      joint1: {p: 0.5, i: 0.0, d: 0.0, feedforward_gain: 1.0}
 
 test_pid_controller_with_feedforward_gain_dual_interface:
   ros__parameters:
@@ -45,8 +64,8 @@ test_pid_controller_with_feedforward_gain_dual_interface:
     reference_and_state_interfaces: ["position", "velocity"]
 
     gains:
-      joint1: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}
-      joint2: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}
+      joint1: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0}
+      joint2: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0}
 
 test_save_i_term_off:
   ros__parameters:
@@ -58,7 +77,7 @@ test_save_i_term_off:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false}
+      joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: false}
 
 test_save_i_term_on:
   ros__parameters:
@@ -70,4 +89,4 @@ test_save_i_term_on:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true}
+      joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true}

pid_controller/test/test_pid_controller.cpp

@@ -46,8 +46,8 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success)
     ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0);
     ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 3.0);
     ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup);
-    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0);
-    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0);
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_max, 5.0);
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_min, -5.0);
     ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0);
   }
   ASSERT_EQ(controller_->params_.command_interface, command_interface_);
@@ -274,7 +274,7 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter)
 
 TEST_F(PidControllerTest, test_update_logic_feedforward_off)
 {
-  SetUpController();
+  SetUpController("test_pid_controller_unlimited");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
   executor.add_node(service_caller_node_->get_node_base_interface());
@@ -372,10 +372,10 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward
     // check the command value:
     // ref = 101.101, state = 1.1, ds = 0.01
     // error = ref - state = 100.001, error_dot = error/ds = 10000.1,
-    // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
+    // p_term = 100.001 * 1, i_term = 0.0 at first update call, d_term = error/ds = 10000.1 * 3
     // feedforward ON, feedforward_gain = 0
-    // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3
-    const double expected_command_value = 30102.301020;
+    // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3
+    const double expected_command_value = 30100.301000;
 
     double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
     EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
@@ -390,7 +390,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward
 
 TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
 {
-  SetUpController();
+  SetUpController("test_pid_controller_unlimited");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
   executor.add_node(service_caller_node_->get_node_base_interface());
@@ -429,20 +429,27 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
   // ref = 5.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0
   // error = ref - state =  5.0 - 1.1 = 3.9, error_dot = error/ds = 3.9/0.01 = 390.0,
   // p_term = error * p_gain = 3.9 * 1.0 = 3.9,
-  // i_term = error * ds * i_gain = 3.9 * 0.01 * 2.0 = 0.078,
+  // i_term = zero at first update
   // d_term = error_dot * d_gain = 390.0 * 3.0 = 1170.0
   // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978
+<<<<<<< HEAD
   const double expected_command_value = 1173.978;
 
   EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+=======
+  {
+    const double expected_command_value = 1173.9;
+    EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
+  }
+>>>>>>> bf253f1 (Change the tests to work without deprecated PID settings (#1824))
 }
 
 /**
  * @brief check default calculation with angle_wraparound turned off
  */
 TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off)
 {
-  SetUpController();
+  SetUpController("test_pid_controller_unlimited");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
   executor.add_node(service_caller_node_->get_node_base_interface());
@@ -451,7 +458,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound);
 
-  // write reference interface so that the values are would be wrapped
+  // write reference interface so that the values would be wrapped
   controller_->reference_interfaces_[0] = 10.0;
 
   // run update
@@ -460,8 +467,20 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off)
     controller_interface::return_type::OK);
 
   // check the result of the commands - the values are not wrapped
+<<<<<<< HEAD
   const double expected_command_value = 2679.078;
   EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
+=======
+  // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0
+  // error = ref - state =  10.0 - 1.1 = 8.9, error_dot = error/ds = 8.9/0.01 = 890.0,
+  // p_term = error * p_gain = 8.9 * 1.0 = 8.9,
+  // i_term = zero at first update
+  // d_term = error_dot * d_gain = 890.0 * 3.0 = 2670.0
+  // feedforward OFF -> cmd = p_term + i_term + d_term = 8.9 + 0.0 + 2670.0 = 2678.9
+  const double expected_command_value = 2678.9;
+  EXPECT_NEAR(
+    controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5);
+>>>>>>> bf253f1 (Change the tests to work without deprecated PID settings (#1824))
 }
 
 /**
@@ -490,9 +509,21 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on)
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+<<<<<<< HEAD
   // Check the command value
   const double expected_command_value = 787.713559;
   EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
+=======
+  // Check the command value with wrapped error
+  // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0
+  // error = ref - state =  wrap(10.0 - 1.1) = 8.9-2*pi = 2.616814, error_dot = error/ds
+  // = 2.6168/0.01 = 261.6814, p_term = error * p_gain = 2.6168 * 1.0 = 2.6168, i_term = zero at
+  // first update d_term = error_dot * d_gain = 261.6814 * 3.0 = 785.0444079 feedforward OFF -> cmd
+  // = p_term + i_term + d_term = 2.616814, + 0.0 + 785.0444079 = 787.6612219
+  const double expected_command_value = 787.6612219;
+  EXPECT_NEAR(
+    controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5);
+>>>>>>> bf253f1 (Change the tests to work without deprecated PID settings (#1824))
 }
 
 TEST_F(PidControllerTest, subscribe_and_get_messages_success)
@@ -720,9 +751,9 @@ TEST_F(PidControllerTest, test_save_i_term_off)
 
   // check the command value
   // error = ref - state = 100.001, error_dot = error/ds = 10000.1,
-  // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
-  // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
-  const double expected_command_value = 30102.30102;
+  // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3
+  // feedforward OFF -> cmd = p_term + i_term + d_term = 30100.301
+  const double expected_command_value = 30100.3010;
 
   double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
@@ -770,9 +801,9 @@ TEST_F(PidControllerTest, test_save_i_term_on)
 
   // check the command value
   // error = ref - state = 100.001, error_dot = error/ds = 10000.1,
-  // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
-  // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
-  const double expected_command_value = 30102.30102;
+  // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3
+  // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.301
+  const double expected_command_value = 30100.3010;
 
   double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, expected_command_value, 1e-5);

pid_controller/test/test_pid_controller.hpp

@@ -140,13 +140,18 @@ class PidControllerFixture : public ::testing::Test
     // initialize controller
     controller_ = std::make_unique<CtrlType>();
 
+<<<<<<< HEAD
     command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
     command_publisher_ = command_publisher_node_->create_publisher<ControllerCommandMsg>(
       "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS());
 
     service_caller_node_ = std::make_shared<rclcpp::Node>("service_caller");
     feedforward_service_client_ = service_caller_node_->create_client<ControllerModeSrvType>(
       "/test_pid_controller/set_feedforward_control");
+=======
+    // create a publisher node, publisher will be created in SetUpController
+    command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
+>>>>>>> bf253f1 (Change the tests to work without deprecated PID settings (#1824))
   }
 
   static void TearDownTestCase() { rclcpp::shutdown(); }
@@ -156,6 +161,9 @@ class PidControllerFixture : public ::testing::Test
 protected:
   void SetUpController(const std::string controller_name = "test_pid_controller")
   {
+    command_publisher_ = command_publisher_node_->create_publisher<ControllerCommandMsg>(
+      "/" + controller_name + "/reference", rclcpp::SystemDefaultsQoS());
+
     ASSERT_EQ(
       controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()),
       controller_interface::return_type::OK);

pid_controller/test/test_pid_controller_dual_interface.cpp

@@ -93,10 +93,17 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i
     controller_interface::return_type::OK);
 
   // check the commands
+<<<<<<< HEAD
   const double joint1_expected_cmd = 8.915;
   const double joint2_expected_cmd = 9.915;
   ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd);
   ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd);
+=======
+  const double joint1_expected_cmd = 8.90;
+  const double joint2_expected_cmd = 9.90;
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd);
+  ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd);
+>>>>>>> bf253f1 (Change the tests to work without deprecated PID settings (#1824))
 }
 
 int main(int argc, char ** argv)