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

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_);
@@ -201,7 +201,7 @@ TEST_F(PidControllerTest, reactivate_success)
 
 TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
 {
-  SetUpController();
+  SetUpController("test_pid_controller_unlimited");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
 
@@ -238,10 +238,10 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
     // 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_optional().value() * 1e5) / 1e5;
@@ -257,7 +257,7 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
 
 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());
 
@@ -294,28 +294,29 @@ 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
-  const double expected_command_value = 1173.978;
-
-  EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
+  {
+    const double expected_command_value = 1173.9;
+    EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
+  }
 }
 
 /**
  * @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());
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   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
@@ -324,7 +325,13 @@ 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
-  const double expected_command_value = 2679.078;
+  // 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);
 }
@@ -354,8 +361,13 @@ 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);
 
-  // Check the command value
-  const double expected_command_value = 787.713559;
+  // 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);
 }
@@ -607,9 +619,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_optional().value() * 1e5) / 1e5;
@@ -657,9 +669,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_optional().value() * 1e5) / 1e5;

pid_controller/test/test_pid_controller.hpp

@@ -137,10 +137,8 @@ class PidControllerFixture : public ::testing::Test
     // initialize controller
     controller_ = std::make_unique<CtrlType>();
 
-    auto test = std::make_shared<rclcpp::Node>("command_publisher");
-    command_publisher_node_ = test;
-    command_publisher_ = command_publisher_node_->create_publisher<ControllerCommandMsg>(
-      "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS());
+    // create a publisher node, publisher will be created in SetUpController
+    command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
   }
 
   static void TearDownTestCase() { rclcpp::shutdown(); }
@@ -150,6 +148,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

@@ -88,8 +88,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i
     controller_interface::return_type::OK);
 
   // check the commands
-  const double joint1_expected_cmd = 8.915;
-  const double joint2_expected_cmd = 9.915;
+  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);
 }