diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 52bc970500..e3b35aec8f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -134,7 +134,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration + * \param[in] period The measured time since the last control loop iteration * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ CONTROLLER_INTERFACE_PUBLIC diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 312f5761a8..0586dd9709 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -85,6 +85,7 @@ if(BUILD_TESTING) ament_add_gmock(test_controller_manager test/test_controller_manager.cpp + TIMEOUT 180 ) target_link_libraries(test_controller_manager controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9d5541e1ad..baf33d1e28 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -122,7 +122,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 6f7483f3ec..7b52ff75f0 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -38,7 +38,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; - std::shared_ptr next_update_cycle_time; + std::shared_ptr last_update_cycle_time; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d158a48605..23c267ad32 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -596,7 +596,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared( + controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); // We have to fetch the parameters_file at the time of loading the controller, because this way we @@ -1545,8 +1545,8 @@ void ControllerManager::activate_controllers( } auto controller = found_it->c; auto controller_name = found_it->info.name; - // reset the next update cycle time for newly activated controllers - *found_it->next_update_cycle_time = + // reset the last update cycle time for newly activated controllers + *found_it->last_update_cycle_time = rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; @@ -2139,10 +2139,31 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - bool controller_go = + const rclcpp::Time current_time = get_clock()->now(); + if ( + *loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + // it is zero after activation + *loaded_controller.last_update_cycle_time = current_time - controller_period; + RCLCPP_DEBUG( + get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", + loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); + } + const auto controller_actual_period = + (current_time - *loaded_controller.last_update_cycle_time); + + /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the + /// jitter in the system sleep cycles. + // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have + // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we + // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue + // to keep up with the controller update rate (see issue #1769). + const bool controller_go = + run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); + (controller_actual_period.seconds() * controller_update_rate >= 0.99); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2151,15 +2172,28 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update(time, controller_period); - - if ( - *loaded_controller.next_update_cycle_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + auto controller_ret = controller_interface::return_type::OK; + // Catch exceptions thrown by the controller update function + try { - *loaded_controller.next_update_cycle_time = time; + controller_ret = loaded_controller.c->update(time, controller_period); } - *loaded_controller.next_update_cycle_time += controller_period; + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while updating controller '%s': %s", + typeid(e).name(), loaded_controller.info.name.c_str(), e.what()); + controller_ret = controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while updating controller '%s'", + loaded_controller.info.name.c_str()); + controller_ret = controller_interface::return_type::ERROR; + } + + *loaded_controller.last_update_cycle_time = current_time; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..06422ab953 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con } controller_interface::return_type TestController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + update_period_ = period; ++internal_counter; // set value to hardware to produce and test different behaviors there diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f73f592037..9107c19f3e 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -77,6 +77,7 @@ class TestController : public controller_interface::ControllerInterface // enables external setting of values to command interfaces - used for simulation of hardware // errors double set_first_command_interface_value_to; + rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.); }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index eb001ce70f..528bacc25c 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -337,11 +337,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); + const auto cm_update_rate = cm_->get_update_rate(); for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) { EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [cm_update_rate, 2*cm_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -402,6 +408,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ControllerManagerRunner cm_runner(this); cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); } + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -420,6 +429,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -434,16 +445,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; const auto initial_counter = test_controller->internal_counter; rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); - time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const auto no_of_secs_passed = update_counter / cm_update_rate; @@ -453,13 +476,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. - EXPECT_NEAR( + EXPECT_THAT( test_controller->internal_counter - initial_counter, - (controller_update_rate * no_of_secs_passed), 1); + testing::AnyOf( + testing::Ge((controller_update_rate - 1) * no_of_secs_passed), + testing::Lt((controller_update_rate * no_of_secs_passed)))); } } } INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, - testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90));