From b24b3100bcedc4a339f7eaface0b20bcdef02328 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Fri, 14 Feb 2025 12:03:30 -0800 Subject: [PATCH] [pid_controller] Update tests (#1538) --- pid_controller/CMakeLists.txt | 13 + pid_controller/src/pid_controller.cpp | 18 +- .../test/pid_controller_params.yaml | 39 ++- pid_controller/test/test_pid_controller.cpp | 244 +++++++----------- pid_controller/test/test_pid_controller.hpp | 22 +- .../test_pid_controller_dual_interface.cpp | 110 ++++++++ 6 files changed, 277 insertions(+), 169 deletions(-) create mode 100644 pid_controller/test/test_pid_controller_dual_interface.cpp diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index d1374d54cb..d07546bada 100644 --- a/pid_controller/CMakeLists.txt +++ b/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( diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index dfb4c8e706..782d39dfdc 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/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()); + } } } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index c8cc14753f..15c4eaf049 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/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} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 75c39fbc44..1c0494a002 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -17,7 +17,6 @@ #include "test_pid_controller.hpp" -#include #include #include #include @@ -47,7 +46,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 +222,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(); @@ -241,15 +245,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) EXPECT_TRUE(std::isnan(interface)); } - std::shared_ptr msg = std::make_shared(); - 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::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); for (size_t i = 0; i < dof_command_values_.size(); ++i) { @@ -270,9 +266,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 + const double 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; @@ -290,15 +299,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) EXPECT_TRUE(std::isnan(interface)); } - std::shared_ptr msg = std::make_shared(); - 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::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); @@ -321,101 +322,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()); + // 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 + const double expected_command_value = 30102.301020; - 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 msg = std::make_shared(); - 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::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); - - 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 msg = std::make_shared(); - 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::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + // update reference interface which will be used for calculation + const double 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 + const double expected_command_value = 1173.978; + + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -429,15 +402,20 @@ 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 + const double expected_command_value = 2679.078; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } /** @@ -445,7 +423,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_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 +433,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 + const double 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) @@ -584,15 +571,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); // send a message to update reference interface - std::shared_ptr msg = std::make_shared(); - msg->dof_names = controller_->params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) - { - msg->values[i] = target_value; - } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference({target_value}); ASSERT_EQ( controller_->update_reference_from_subscribers( rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -650,15 +629,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); // send a message to update reference interface - std::shared_ptr msg = std::make_shared(); - msg->dof_names = controller_->params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) - { - msg->values[i] = target_value; - } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference({target_value}); ASSERT_EQ( controller_->update_reference_from_subscribers( rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -694,15 +665,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - std::shared_ptr msg = std::make_shared(); - 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::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -712,23 +675,14 @@ TEST_F(PidControllerTest, test_save_i_term_off) // 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; + const double 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); // deactivate the controller and set command=state ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - msg = std::make_shared(); - 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_state_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_state_values_); // reactivate the controller, the integral term should NOT be saved ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -761,15 +715,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - std::shared_ptr msg = std::make_shared(); - 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::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -779,23 +725,14 @@ TEST_F(PidControllerTest, test_save_i_term_on) // 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; + const double 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); // deactivate the controller and set command=state ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - msg = std::make_shared(); - 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_state_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_state_values_); // reactivate the controller, the integral term should be saved ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -803,9 +740,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - expected_command_value = 2.00002; // i_term from above actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; - EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } int main(int argc, char ** argv) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1ec762568c..1451ae919b 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -54,13 +55,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); FRIEND_TEST(PidControllerTest, test_save_i_term_on); FRIEND_TEST(PidControllerTest, test_save_i_term_off); @@ -102,6 +105,19 @@ class TestablePidController : public pid_controller::PidController { wait_for_command(executor, timeout); } + + void set_reference(const std::vector & target_value) + { + std::shared_ptr msg = std::make_shared(); + msg->dof_names = params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value[i]; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.writeFromNonRT(msg); + } }; // We are using template class here for easier reuse of Fixture in specializations of controllers diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp new file mode 100644 index 0000000000..e006986473 --- /dev/null +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -0,0 +1,110 @@ +// Copyright 2025 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerDualInterfaceTest : public PidControllerFixture +{ +public: + void SetUp() + { + PidControllerFixture::SetUp(); + + dof_names_ = {"joint1", "joint2"}; + + // set up interfaces + command_interface_ = "velocity"; + state_interfaces_ = {"position", "velocity"}; + dof_state_values_ = { + get_joint1_state_position(), get_joint2_state_position(), get_joint1_state_velocity(), + get_joint2_state_velocity()}; + } + + double get_joint1_state_position() const { return 10.0; } + double get_joint2_state_position() const { return 11.0; } + double get_joint1_state_velocity() const { return 5.0; } + double get_joint2_state_velocity() const { return 6.0; } + + double get_joint1_reference_position() const { return 15.0; } + double get_joint2_reference_position() const { return 16.0; } + double get_joint1_reference_velocity() const { return 6.0; } + double get_joint2_reference_velocity() const { return 7.0; } +}; + +/** + * @brief Test the feedforward gain with chained mode with two interfaces + * The second interface should be derivative of the first one + */ + +TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface) +{ + SetUpController("test_pid_controller_with_feedforward_gain_dual_interface"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 0.3); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 0.4); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set up the reference interface, + controller_->reference_interfaces_ = { + get_joint1_reference_position(), get_joint2_reference_position(), + get_joint1_reference_velocity(), get_joint2_reference_velocity()}; + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the commands + const double joint1_expected_cmd = 8.915; + const double joint2_expected_cmd = 9.915; + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}