Fix issues introduced by master merge

Author: Juliaj

pid_controller/src/pid_controller.cpp

@@ -503,7 +503,6 @@ controller_interface::return_type PidController::update_and_write_commands(
   for (size_t i = 0; i < dof_; ++i)
   {
     double tmp_command = 0.0;
-    double tmp_command = 0.0;
 
     if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i]))
     {
@@ -542,24 +541,19 @@ controller_interface::return_type PidController::update_and_write_commands(
           std::isfinite(measured_state_values_[dof_ + i]))
         {
           // use calculation with 'error' and 'error_dot'
-          tmp_command += pids_[i]->compute_command(
           tmp_command += pids_[i]->compute_command(
             error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period);
         }
         else
         {
           // Fallback to calculation with 'error' only
           tmp_command += pids_[i]->compute_command(error, period);
-          RCLCPP_WARN(
-            get_node()->get_logger(), "Two interfaces, fallback to calculation with 'error' only");
-          tmp_command += pids_[i]->compute_command(error, period);
         }
       }
       else
       {
         // use calculation with 'error' only
         tmp_command += pids_[i]->compute_command(error, period);
-        tmp_command += pids_[i]->compute_command(error, period);
       }
 
       // write calculated values

pid_controller/test/pid_controller_params.yaml

@@ -47,17 +47,5 @@ test_pid_controller_with_feedforward_gain_dual_interface:
     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: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
 
 
-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}

pid_controller/test/test_pid_controller.hpp

@@ -60,10 +60,10 @@ class TestablePidController : public pid_controller::PidController
   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(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface);
   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(
     const rclcpp_lifecycle::State & previous_state) override