diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..fd185e02d0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,7 +32,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -40,6 +40,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63e7f08682..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.0 + rev: v19.1.3 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.3 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 211716f301..ae3804919c 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -160,7 +160,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 a1a98bc59a..1bb84eb32c 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -87,6 +87,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 @@ -195,7 +196,7 @@ if(BUILD_TESTING) ) ament_add_gmock(test_hardware_spawner - test/test_hardware_spawner + test/test_hardware_spawner.cpp TIMEOUT 120 ) target_link_libraries(test_hardware_spawner diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6c0b4fde9b..068eefc1f9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -123,7 +123,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 9ce1aab8b6..0f44867814 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -37,7 +37,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; }; struct ControllerChainSpec diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a8668f7f1b..d7e924a863 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -543,7 +543,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 @@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers( continue; } auto controller = found_it->c; - // 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; @@ -2354,21 +2354,31 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + const rclcpp::Time current_time = get_clock()->now(); if ( - *loaded_controller.next_update_cycle_time == + *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 next_update_cycle_time to %fs for the controller : %s", - time.seconds(), loaded_controller.info.name.c_str()); - *loaded_controller.next_update_cycle_time = time; + 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()); } - - bool controller_go = + 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 '", @@ -2377,8 +2387,6 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - const auto controller_actual_period = - (time - *loaded_controller.next_update_cycle_time) + controller_period; auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function @@ -2402,7 +2410,7 @@ controller_interface::return_type ControllerManager::update( controller_ret = controller_interface::return_type::ERROR; } - *loaded_controller.next_update_cycle_time += controller_period; + *loaded_controller.last_update_cycle_time = current_time; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 13af864c2f..0ed68d9765 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,27 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); + + const bool lock_memory = cm->get_parameter_or("lock_memory", true); + std::string message; + if (lock_memory && !realtime_tools::lock_memory(message)) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + } + + const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); + if (cpu_affinity >= 0) + { + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); + } + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( @@ -64,7 +85,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority]() + [cm, thread_priority, use_sim_time, &rate]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -79,7 +100,7 @@ int main(int argc, char ** argv) { RCLCPP_INFO( cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", - kSchedPriority); + thread_priority); } // for calculating sleep time @@ -105,7 +126,14 @@ int main(int argc, char ** argv) // wait until we hit the end of the period next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); + if (use_sim_time) + { + rate.sleep(); + } + else + { + std::this_thread::sleep_until(next_iteration_time); + } } cm->shutdown_async_controllers_and_components(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8ff844adc2..0c4e51985f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -575,10 +575,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // 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_, - testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate)))); + 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 @@ -640,6 +638,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))); @@ -650,7 +651,6 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function - time_ = test_controller->get_node()->now(); // set to something nonzero const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; std::vector stop_controllers = {}; @@ -661,7 +661,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"; - time_ += rclcpp::Duration::from_seconds(0.01); + 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))); @@ -677,25 +678,29 @@ 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; // don't start with zero to check if the period is correct if controller is activated anytime 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_, + test_controller->update_period_.seconds(), testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate)))) - << "update_counter: " << update_counter; + 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 double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; @@ -708,15 +713,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_THAT( test_controller->internal_counter - initial_counter, testing::AnyOf( - testing::Eq(controller_update_rate * no_of_secs_passed), - testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); + 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)); class TestControllerManagerFallbackControllers : public ControllerManagerFixture, @@ -764,7 +769,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -772,7 +777,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -846,7 +851,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -854,7 +859,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -944,7 +949,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -952,7 +957,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1033,7 +1038,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1041,7 +1046,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1129,7 +1134,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1137,14 +1142,14 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 } @@ -1279,7 +1284,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1287,21 +1292,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1381,7 +1386,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1413,7 +1418,7 @@ TEST_F( // available controller_spec.info.fallback_controllers_names = { test_controller_4_name, test_controller_3_name, test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1509,7 +1514,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1517,21 +1522,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1617,7 +1622,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_3_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index bfb7481208..eb4790046c 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -77,7 +77,10 @@ controller_manager * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). +* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). +* The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). +* The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6fe4f25663..1dfd499c2c 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,7 +17,10 @@ #include #include +#include +#include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -69,13 +72,57 @@ class Handle { } - Handle(const Handle & other) = default; + Handle(const Handle & other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } - Handle(Handle && other) = default; + Handle(Handle && other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } - Handle & operator=(const Handle & other) = default; + Handle & operator=(const Handle & other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } + return *this; + } - Handle & operator=(Handle && other) = default; + Handle & operator=(Handle && other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } + return *this; + } virtual ~Handle() = default; @@ -95,8 +142,14 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } + [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]] double get_value() const { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return std::numeric_limits::quiet_NaN(); + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); @@ -104,12 +157,33 @@ class Handle // END } - void set_value(double value) + [[nodiscard]] bool get_value(double & value) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value directly if old functionality is removed + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + // END + } + + [[nodiscard]] bool set_value(double value) { + std::unique_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) set value_ directly if old functionality is removed THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; + return true; // END } @@ -122,6 +196,7 @@ class Handle // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; // END + mutable std::shared_mutex handle_mutex_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index aa306870a1..6013dea778 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,10 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" namespace hardware_interface { @@ -51,6 +54,27 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { + auto logger = rclcpp::get_logger(command_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u get_value calls", + get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u set_value calls", + get_name().c_str(), set_value_statistics_.timeout_counter, + (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.failed_counter, + (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -70,13 +94,70 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + template + [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10) + { + unsigned int nr_tries = 0; + ++set_value_statistics_.total_counter; + while (!command_interface_.set_value(value)) + { + ++set_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++set_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } - double get_value() const { return command_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!command_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: CommandInterface & command_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; + HandleRTStatistics set_value_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 96cc3e89df..3ebc8c7ca0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,11 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" - +#include "rclcpp/logging.hpp" namespace hardware_interface { class LoanedStateInterface @@ -56,6 +58,17 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { + auto logger = rclcpp::get_logger(state_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + logger, + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " + "get_value calls", + state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -75,11 +88,50 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!state_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: const StateInterface & state_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; }; } // namespace hardware_interface