diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 4dbe5ab966..7bf5cae628 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -100,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test cm_->robot_description_callback(msg); } } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -118,9 +119,7 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -158,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; const bool pass_urdf_as_parameter_; + rclcpp::Time time_; }; class TestControllerManagerSrvs @@ -178,15 +178,9 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { - cm_->read( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); - cm_->write( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); executor_->add_node(cm_); @@ -213,9 +207,7 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); } } else diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 4e3b8b470d..e317726585 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -96,9 +96,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -110,9 +108,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; @@ -127,9 +123,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -138,9 +132,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); // Start the real test controller, will take effect at the end of the update function @@ -155,9 +147,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -167,9 +157,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -185,9 +173,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) << "Controller is stopped at the end of update, so it should have done one more update"; { @@ -222,9 +208,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -236,9 +220,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -255,9 +237,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -271,9 +251,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -308,9 +286,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -323,9 +299,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -342,9 +316,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -360,9 +332,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -378,10 +348,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "switch_controller should be blocking until next update cycle"; EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))) + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -425,9 +392,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -445,9 +410,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -461,8 +424,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto controller_update_rate = test_controller->get_update_rate(); const auto initial_counter = test_controller->internal_counter; - rclcpp::Time time = - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); + rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 0830b0638a..20af889c81 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -138,11 +138,7 @@ class TestControllerManagerWithTestableCM cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; @@ -230,11 +226,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -257,11 +249,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -273,11 +261,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -294,8 +278,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->read( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -306,11 +289,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -354,11 +333,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -375,8 +350,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_higher = test_controller_system->internal_counter; auto new_counter = test_broadcaster_sensor->internal_counter + 1; - EXPECT_NO_THROW(cm_->read( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -387,11 +361,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) @@ -448,11 +418,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -475,11 +441,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -491,11 +453,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -512,8 +470,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -524,11 +481,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -572,11 +525,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -595,8 +544,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -607,11 +555,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has write error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index ca82bb376d..a34d70ada7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -378,9 +378,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 16c84486b6..4137386f72 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -39,15 +39,9 @@ class TestLoadController : public ControllerManagerFixtureread( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); - cm_->update( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); - cm_->write( - rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()), - PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); update_executor_ =