[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

@@ -477,6 +477,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());
@@ -503,17 +504,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]))
           {
@@ -540,8 +542,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(
@@ -560,7 +562,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,9 +8,9 @@ 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_save_i_term_off:
+test_pid_controller_angle_wraparound_on:
   ros__parameters:
     dof_names:
       - joint1
@@ -20,10 +20,35 @@ 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, i_clamp_max: 5.0, i_clamp_min: -5.0, angle_wraparound: true}
 
+test_pid_controller_with_feedforward_gain:
+  ros__parameters:
+    dof_names:
+      - joint1
 
-test_save_i_term_on:
+    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}
+
+test_save_i_term_off:
   ros__parameters:
     dof_names:
       - joint1
@@ -33,9 +58,9 @@ 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, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false}
 
-test_pid_controller_with_feedforward_gain:
+test_save_i_term_on:
   ros__parameters:
     dof_names:
       - joint1
@@ -45,4 +70,4 @@ 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: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true}