[pid_controller] Update tests (backport #1538)

pid_controller/CMakeLists.txt

@@ -90,6 +90,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

@@ -411,6 +411,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());
@@ -435,13 +436,13 @@ controller_interface::return_type PidController::update_and_write_commands(
   {
     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]))
           {
@@ -468,8 +469,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]->computeCommand(

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}