[pid_controller] Update tests (#1538)

(cherry picked from commit b24b310)

# Conflicts:
#	pid_controller/src/pid_controller.cpp
#	pid_controller/test/pid_controller_params.yaml
#	pid_controller/test/test_pid_controller.cpp
#	pid_controller/test/test_pid_controller.hpp

Author: Juliaj

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());
@@ -431,17 +432,27 @@ controller_interface::return_type PidController::update_and_write_commands(
     }
   }
 
+<<<<<<< HEAD
+=======
+  // Fill the information of the exported state interfaces
+  for (size_t i = 0; i < measured_state_values_.size(); ++i)
+  {
+    state_interfaces_values_[i] = measured_state_values_[i];
+  }
+
+  // Iterate through all the dofs to calculate the output command
+>>>>>>> b24b310 ([pid_controller] Update tests (#1538))
   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]))
           {
@@ -468,8 +479,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(
@@ -488,7 +499,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,10 +8,52 @@ 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:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    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}
+
+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}
+
+<<<<<<< HEAD
 
 test_pid_controller_with_feedforward_gain:
+=======
+test_save_i_term_off:
+>>>>>>> b24b310 ([pid_controller] Update tests (#1538))
   ros__parameters:
     dof_names:
       - joint1
@@ -21,4 +63,20 @@ test_pid_controller_with_feedforward_gain:
     reference_and_state_interfaces: ["position"]
 
     gains:
+<<<<<<< HEAD
       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: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false}
+
+test_save_i_term_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, save_i_term: true}
+>>>>>>> b24b310 ([pid_controller] Update tests (#1538))