[pid_controller] Update tests

pid_controller/CMakeLists.txt

@@ -92,6 +92,19 @@ if(BUILD_TESTING)
     hardware_interface
   )
 
+  add_rostest_with_parameters_gmock(
+    test_pid_controller_dual_interface
+    test/test_pid_controller_dual_interface.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml
+  )
+  target_include_directories(test_pid_controller_dual_interface PRIVATE include)
+  target_link_libraries(test_pid_controller_dual_interface pid_controller)
+  ament_target_dependencies(
+    test_pid_controller_dual_interface
+    controller_interface
+    hardware_interface
+  )
+
   ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp)
   target_include_directories(test_load_pid_controller PRIVATE include)
   ament_target_dependencies(

pid_controller/src/pid_controller.cpp

@@ -472,6 +472,7 @@ controller_interface::return_type PidController::update_and_write_commands(
   // check for any parameter updates
   update_parameters();
 
+  // Update feedback either from external measured state or from state interfaces
   if (params_.use_external_measured_states)
   {
     const auto measured_state = *(measured_state_.readFromRT());
@@ -498,17 +499,18 @@ controller_interface::return_type PidController::update_and_write_commands(
     state_interfaces_values_[i] = measured_state_values_[i];
   }
 
+  // Iterate through all the dofs to calculate the output command
   for (size_t i = 0; i < dof_; ++i)
   {
     double tmp_command = 0.0;
 
-    if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i]))
+    if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i]))
     {
       // calculate feed-forward
       if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON)
       {
         // two interfaces
-        if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
+        if (reference_interfaces_.size() == 2 * dof_)
         {
           if (std::isfinite(reference_interfaces_[dof_ + i]))
           {
@@ -535,8 +537,8 @@ controller_interface::return_type PidController::update_and_write_commands(
       if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
       {
         if (
-          !std::isnan(reference_interfaces_[dof_ + i]) &&
-          !std::isnan(measured_state_values_[dof_ + i]))
+          std::isfinite(reference_interfaces_[dof_ + i]) &&
+          std::isfinite(measured_state_values_[dof_ + i]))
         {
           // use calculation with 'error' and 'error_dot'
           tmp_command += pids_[i]->compute_command(
@@ -555,7 +557,13 @@ controller_interface::return_type PidController::update_and_write_commands(
       }
 
       // write calculated values
-      command_interfaces_[i].set_value(tmp_command);
+      auto success = command_interfaces_[i].set_value(tmp_command);
+      if (!success)
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(), "Failed to set command value for %s",
+          command_interfaces_[i].get_name().c_str());
+      }
     }
   }
 

pid_controller/test/pid_controller_params.yaml

@@ -8,8 +8,19 @@ test_pid_controller:
     reference_and_state_interfaces: ["position"]
 
     gains:
-      joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
+      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
 
+test_pid_controller_angle_wraparound_on:
+  ros__parameters:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    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}
 
 test_pid_controller_with_feedforward_gain:
   ros__parameters:
@@ -22,3 +33,17 @@ test_pid_controller_with_feedforward_gain:
 
     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}
+
+test_pid_controller_with_feedforward_gain_dual_interface:
+  ros__parameters:
+    dof_names:
+      - joint1
+      - joint2
+
+    command_interface: velocity
+
+    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}

pid_controller/test/test_pid_controller.cpp

@@ -47,7 +47,7 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success)
   {
     ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0);
     ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0);
-    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.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);
@@ -223,6 +223,11 @@ TEST_F(PidControllerTest, test_feedforward_mode_service)
   ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
 }
 
+/**
+ * @brief Check the update logic in non chained mode with feedforward OFF
+ *
+ */
+
 TEST_F(PidControllerTest, test_update_logic_feedforward_off)
 {
   SetUpController();
@@ -270,9 +275,22 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off)
   {
     EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
   }
+  // 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
+  auto expected_command_value = 30102.30102;
+
+  double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
 }
 
-TEST_F(PidControllerTest, test_update_logic_feedforward_on)
+/**
+ * @brief Check the update logic in non chained mode with feedforward ON and feedforward gain is 0
+ *
+ */
+
+TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain)
 {
   SetUpController();
   rclcpp::executors::MultiThreadedExecutor executor;
@@ -321,101 +339,73 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on)
   for (size_t i = 0; i < dof_command_values_.size(); ++i)
   {
     EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
-  }
-}
-
-TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off)
-{
-  SetUpController();
-  rclcpp::executors::MultiThreadedExecutor executor;
-  executor.add_node(controller_->get_node()->get_node_base_interface());
-  executor.add_node(service_caller_node_->get_node_base_interface());
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  controller_->set_chained_mode(true);
-  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(controller_->is_in_chained_mode());
-
-  std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
-  msg->dof_names = dof_names_;
-  msg->values.resize(dof_names_.size(), 0.0);
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    msg->values[i] = dof_command_values_[i];
-  }
-  msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
-  controller_->input_ref_.writeFromNonRT(msg);
 
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
-    EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]);
-    EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
-  }
-
-  ASSERT_EQ(
-    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+    // 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
+    // feedforward ON, feedforward_gain = 0
+    // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3
+    auto expected_command_value = 30102.301020;
 
-  ASSERT_TRUE(controller_->is_in_chained_mode());
-  EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
-  EXPECT_EQ(
-    controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
-  EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
-    EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]);
+    double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+    EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
   }
 }
 
-TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on)
+/**
+ * @brief Check the update logic when chain mode is on.
+ *  in chain mode, update_reference_from_subscribers is not called from update method, and the
+ * reference value is used for calculation
+ */
+
+TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
 {
   SetUpController();
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
   executor.add_node(service_caller_node_->get_node_base_interface());
 
+  // set chain mode to true
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   controller_->set_chained_mode(true);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_TRUE(controller_->is_in_chained_mode());
+  // feedforward mode is off as default, use this for convenience
   EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
 
-  std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
-  msg->dof_names = dof_names_;
-  msg->values.resize(dof_names_.size(), 0.0);
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    msg->values[i] = dof_command_values_[i];
-  }
-  msg->values_dot.resize(dof_names_.size(), std::numeric_limits<double>::quiet_NaN());
-  controller_->input_ref_.writeFromNonRT(msg);
+  // update reference interface which will be used for calculation
+  auto ref_interface_value = 5.0;
+  controller_->reference_interfaces_[0] = ref_interface_value;
 
-  controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
-  EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
+  // publish a command message which should be ignored as chain mode is on
+  publish_commands({10.0}, {0.0});
+  controller_->wait_for_commands(executor);
 
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
-    EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]);
-    EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
-  }
+  // check the reference interface is not updated as chain mode is on
+  EXPECT_EQ(controller_->reference_interfaces_[0], ref_interface_value);
 
+  // run update
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
   ASSERT_TRUE(controller_->is_in_chained_mode());
-  EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
+
   EXPECT_EQ(
     controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
   EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
-  for (size_t i = 0; i < dof_command_values_.size(); ++i)
-  {
-    EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i]));
-    EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]);
-  }
+
+  // check the command value
+  // 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,
+  // 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
+  auto expected_command_value = 1173.978;
+
+  EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
 }
 
 /**
@@ -429,23 +419,28 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off)
   executor.add_node(service_caller_node_->get_node_base_interface());
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  controller_->set_chained_mode(true);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(controller_->is_in_chained_mode());
+  ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound);
 
   // write reference interface so that the values are would be wrapped
+  controller_->reference_interfaces_[0] = 10.0;
 
   // run update
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check the result of the commands - the values are not wrapped
+  auto expected_command_value = 2679.078;
+  EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
 }
 
 /**
  * @brief check default calculation with angle_wraparound turned off
  */
 TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on)
 {
-  SetUpController();
+  SetUpController("test_pid_controller_angle_wraparound_on");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(controller_->get_node()->get_node_base_interface());
   executor.add_node(service_caller_node_->get_node_base_interface());
@@ -455,11 +450,20 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_TRUE(controller_->is_in_chained_mode());
 
-  // write reference interface so that the values are would be wrapped
+  // Check on wraparound is on
+  ASSERT_TRUE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound);
 
-  // run update
+  // Write reference interface with values that would wrap, state is 1.1
+  controller_->reference_interfaces_[0] = 10.0;
+
+  // Run update
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
-  // check the result of the commands - the values are wrapped
+  // Check the command value
+  auto expected_command_value = 787.713559;
+  EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
 }
 
 TEST_F(PidControllerTest, subscribe_and_get_messages_success)

pid_controller/test/test_pid_controller.hpp

@@ -54,13 +54,15 @@ class TestablePidController : public pid_controller::PidController
   FRIEND_TEST(PidControllerTest, reactivate_success);
   FRIEND_TEST(PidControllerTest, test_feedforward_mode_service);
   FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off);
-  FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on);
-  FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off);
-  FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on);
+  FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain);
+  FRIEND_TEST(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update);
+  FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_off);
+  FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_on);
   FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success);
   FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status);
   FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain);
   FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain);
+  FRIEND_TEST(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface);
 
 public:
   controller_interface::CallbackReturn on_configure(