From fa79a01e43ba47269120b77b28b678afd738c07d Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 12 Feb 2025 20:27:09 -0800 Subject: [PATCH 1/8] Clean up deprecated code and update tests --- pid_controller/CMakeLists.txt | 13 ++ pid_controller/src/pid_controller.cpp | 50 ++++-- .../test/pid_controller_params.yaml | 40 ++++- pid_controller/test/test_pid_controller.cpp | 155 +++++++++--------- pid_controller/test/test_pid_controller.hpp | 8 +- .../test_pid_controller_dual_interface.cpp | 116 +++++++++++++ 6 files changed, 286 insertions(+), 96 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 521227b49c..5cf6ab47b7 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -140,7 +140,7 @@ controller_interface::CallbackReturn PidController::configure_parameters() // prefix should be interpreted as parameters prefix pids_[i] = std::make_shared(get_node(), "gains." + params_.dof_names[i], true); - if (!pids_[i]->initPid()) + if (!pids_[i]->initialize_from_ros_parameters()) { return CallbackReturn::FAILURE; } @@ -470,6 +470,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()); @@ -496,22 +497,30 @@ 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 = std::numeric_limits::quiet_NaN(); + double tmp_command = 0.0; - // Using feedforward - 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) { - tmp_command = reference_interfaces_[dof_ + i] * - params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; - } - else - { - tmp_command = 0.0; + // two interfaces + if (reference_interfaces_.size() == 2 * dof_) + { + if (std::isfinite(reference_interfaces_[dof_ + i])) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + else // one interface + { + tmp_command = reference_interfaces_[i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } } double error = reference_interfaces_[i] - measured_state_values_[i]; @@ -526,27 +535,34 @@ 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( + 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]->computeCommand(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]->computeCommand(error, period); + tmp_command += pids_[i]->compute_command(error, period); } // 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 7555cfc156..0458c87012 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -8,4 +8,42 @@ 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} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 60d484e463..e64e6264cf 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -16,6 +16,7 @@ // #include "test_pid_controller.hpp" +#include "angles/angles.h" #include #include @@ -47,7 +48,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); @@ -202,6 +203,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(); @@ -249,9 +255,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 + auto 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; @@ -300,101 +319,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()); - - 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); + // 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 + auto expected_command_value = 30102.301020; - 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 + auto 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 + auto expected_command_value = 1173.978; + + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -408,15 +399,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 + auto expected_command_value = 2679.078; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } /** @@ -424,7 +420,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()); @@ -434,11 +430,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 + auto 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) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 4471f35a12..5e1fd2b3a0 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -54,11 +54,13 @@ 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(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface); public: controller_interface::CallbackReturn on_configure( 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..42fe711e2d --- /dev/null +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -0,0 +1,116 @@ +// 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 + auto joint1_expected_cmd = 8.915; + auto 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; +} From 40cb8e65119eb85a5285958dd5a5f310bf192768 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 12 Feb 2025 20:37:13 -0800 Subject: [PATCH 2/8] Fix pre-commit issue --- .../test_pid_controller_dual_interface.cpp | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 42fe711e2d..8feb6174e9 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -22,24 +22,21 @@ using pid_controller::feedforward_mode_type; -class PidControllerDualInterfaceTest: public PidControllerFixture +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() - }; + 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; } @@ -86,13 +83,10 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - // set up the reference interface, + // set up the reference interface, controller_->reference_interfaces_ = { - get_joint1_reference_position(), - get_joint2_reference_position(), - get_joint1_reference_velocity(), - get_joint2_reference_velocity() - }; + get_joint1_reference_position(), get_joint2_reference_position(), + get_joint1_reference_velocity(), get_joint2_reference_velocity()}; // run update ASSERT_EQ( From 9c0920c26243b631d12fae09ea56363bb6ac67c6 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 12 Feb 2025 21:10:07 -0800 Subject: [PATCH 3/8] Remove unneeded header --- pid_controller/test/test_pid_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index e64e6264cf..74da82af25 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -16,7 +16,6 @@ // #include "test_pid_controller.hpp" -#include "angles/angles.h" #include #include From 89b5f19be2a6407f9eabeccbdedfc89bbc04b2b4 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 13 Feb 2025 10:50:18 -0800 Subject: [PATCH 4/8] Fix issues introduced by master merge --- pid_controller/src/pid_controller.cpp | 6 ------ pid_controller/test/pid_controller_params.yaml | 12 ------------ pid_controller/test/test_pid_controller.hpp | 4 ++-- 3 files changed, 2 insertions(+), 20 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index a657a57001..8fb3480070 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/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,7 +541,6 @@ 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); } @@ -550,16 +548,12 @@ controller_interface::return_type PidController::update_and_write_commands( { // 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 diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index d2ca5bd5d7..ccc0b0141d 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/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} diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index f0abdcbe30..48d10be96c 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/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 From 543957dcbbb25dc0e250bc8d9f8e4add52458f07 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 13 Feb 2025 10:51:05 -0800 Subject: [PATCH 5/8] Fix pre-commit issues --- pid_controller/test/pid_controller_params.yaml | 2 -- pid_controller/test/test_pid_controller.hpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index ccc0b0141d..0458c87012 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -47,5 +47,3 @@ 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} - - diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 48d10be96c..f308f9ed0b 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -63,7 +63,7 @@ class TestablePidController : public pid_controller::PidController 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 From 65eed401ff447bb3494e94904b55284166b6f8d4 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 13 Feb 2025 13:13:39 -0800 Subject: [PATCH 6/8] Use explicit type const double instead of auto --- pid_controller/test/test_pid_controller.cpp | 12 ++++++------ .../test/test_pid_controller_dual_interface.cpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 73641502c3..2d0904a0ed 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -279,7 +279,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_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); @@ -346,7 +346,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward // 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 - auto expected_command_value = 30102.301020; + const double expected_command_value = 30102.301020; double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); @@ -375,7 +375,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); // update reference interface which will be used for calculation - auto ref_interface_value = 5.0; + const double ref_interface_value = 5.0; controller_->reference_interfaces_[0] = ref_interface_value; // publish a command message which should be ignored as chain mode is on @@ -403,7 +403,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // 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 - auto expected_command_value = 1173.978; + const double expected_command_value = 1173.978; EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } @@ -431,7 +431,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) controller_interface::return_type::OK); // check the result of the commands - the values are not wrapped - auto expected_command_value = 2679.078; + const double expected_command_value = 2679.078; EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } @@ -462,7 +462,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) controller_interface::return_type::OK); // Check the command value - auto expected_command_value = 787.713559; + const double expected_command_value = 787.713559; EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 8feb6174e9..e006986473 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -94,8 +94,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i controller_interface::return_type::OK); // check the commands - auto joint1_expected_cmd = 8.915; - auto joint2_expected_cmd = 9.915; + 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); } From 45fdcd5c879e5d06140e12e39a283f9067936d69 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 14 Feb 2025 08:59:54 +0000 Subject: [PATCH 7/8] Add convenience function for setting references --- pid_controller/test/test_pid_controller.cpp | 83 ++------------------- pid_controller/test/test_pid_controller.hpp | 14 ++++ 2 files changed, 22 insertions(+), 75 deletions(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9b54e76ac8..ce826190a4 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 @@ -246,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) { @@ -308,15 +299,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward 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); @@ -588,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)), @@ -654,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)), @@ -698,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)), @@ -723,16 +682,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) // 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); @@ -765,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)), @@ -790,16 +732,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) // 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); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1ddfc9f19c..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 @@ -104,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 From 53dce5c6a344ac2fc4717f84c4f5e1b4d30889bb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 14 Feb 2025 09:02:27 +0000 Subject: [PATCH 8/8] Use explicit type const double instead of auto --- pid_controller/test/test_pid_controller.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index ce826190a4..1c0494a002 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -675,7 +675,7 @@ 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); @@ -725,7 +725,7 @@ 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); @@ -740,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)