diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9df9fb56ca..8008cc90a0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -210,6 +210,19 @@ class ControllerManager : public rclcpp::Node */ unsigned int get_update_rate() const; + /// Get the trigger clock of the controller manager. + /** + * Get the trigger clock of the controller manager. + * The method is used to get the clock that is used for triggering the controllers and the + * hardware components. + * + * @note When the use_sim_time parameter is set to true, the clock will be the ROS clock. + * Otherwise, the clock will be the Steady Clock. + * + * \returns trigger clock of the controller manager. + */ + rclcpp::Clock::SharedPtr get_trigger_clock() const; + protected: void init_services(); @@ -537,6 +550,7 @@ class ControllerManager : public rclcpp::Node int used_by_realtime_controllers_index_ = -1; }; + rclcpp::Clock::SharedPtr trigger_clock_ = nullptr; std::unique_ptr preshutdown_cb_handle_{nullptr}; RTControllerListWrapper rt_controllers_wrapper_; std::unordered_map controller_chain_spec_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 64a2a1594b..9fdc810e67 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -274,8 +274,6 @@ ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - resource_manager_(std::make_unique( - this->get_node_clock_interface(), this->get_node_logging_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -286,6 +284,8 @@ ControllerManager::ControllerManager( cm_node_options_(options) { initialize_parameters(); + resource_manager_ = + std::make_unique(trigger_clock_, this->get_logger()); init_controller_manager(); } @@ -306,8 +306,7 @@ ControllerManager::ControllerManager( { initialize_parameters(); resource_manager_ = std::make_unique( - urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), - activate_all_hw_components, params_->update_rate); + urdf, trigger_clock_, this->get_logger(), activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -448,6 +447,12 @@ void ControllerManager::initialize_parameters() this->get_node_parameters_interface(), this->get_logger()); params_ = std::make_shared(cm_param_listener_->get_params()); update_rate_ = static_cast(params_->update_rate); + const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); + trigger_clock_ = + use_sim_time.as_bool() ? this->get_clock() : std::make_shared(RCL_STEADY_TIME); + RCLCPP_INFO( + get_logger(), "Using %s clock for triggering controller manager cycles.", + trigger_clock_->get_clock_type() == RCL_STEADY_TIME ? "Steady (Monotonic)" : "ROS"); } catch (const std::exception & e) { @@ -678,8 +683,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.last_update_cycle_time = std::make_shared( - 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.last_update_cycle_time = + std::make_shared(0, 0, this->get_trigger_clock()->get_clock_type()); controller_spec.execution_time_statistics = std::make_shared(); controller_spec.periodicity_statistics = std::make_shared(); @@ -1846,7 +1851,7 @@ void ControllerManager::activate_controllers( auto controller = found_it->c; // 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()); + rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type()); bool assignment_successful = true; // assign command interfaces to the controller @@ -2506,7 +2511,7 @@ controller_interface::return_type ControllerManager::update( // Check for valid time if (!get_clock()->started()) { - if (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + if (time == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) { throw std::runtime_error( "No clock received, and time argument is zero. Check your controller_manager node's " @@ -2547,10 +2552,10 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); bool first_update_cycle = false; - const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time; + const rclcpp::Time current_time = get_clock()->started() ? get_trigger_clock()->now() : time; if ( *loaded_controller.last_update_cycle_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) { // last_update_cycle_time is zero after activation first_update_cycle = true; @@ -2570,8 +2575,7 @@ controller_interface::return_type ControllerManager::update( // 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 == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) || (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; RCLCPP_DEBUG( @@ -2587,7 +2591,7 @@ controller_interface::return_type ControllerManager::update( try { const auto trigger_result = - loaded_controller.c->trigger_update(current_time, controller_actual_period); + loaded_controller.c->trigger_update(this->now(), controller_actual_period); trigger_status = trigger_result.successful; controller_ret = trigger_result.result; if (trigger_status && trigger_result.execution_time.has_value()) @@ -2813,6 +2817,8 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +rclcpp::Clock::SharedPtr ControllerManager::get_trigger_clock() const { return trigger_clock_; } + void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 176c346b3f..ef3d96d707 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -124,33 +124,50 @@ int main(int argc, char ** argv) // for calculating sleep time auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); - auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); - std::chrono::time_point - next_iteration_time{cm_now - period}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now() - period; + rclcpp::Time previous_time = cm->get_trigger_clock()->now(); + std::this_thread::sleep_for(period); + + std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; while (rclcpp::ok()) { // calculate measured period - auto const current_time = cm->now(); + auto const current_time = cm->get_trigger_clock()->now(); auto const measured_period = current_time - previous_time; previous_time = current_time; // execute update loop - cm->read(cm->now(), measured_period); - cm->update(cm->now(), measured_period); - cm->write(cm->now(), measured_period); + cm->read(cm->get_trigger_clock()->now(), measured_period); + cm->update(cm->get_trigger_clock()->now(), measured_period); + cm->write(cm->get_trigger_clock()->now(), measured_period); // wait until we hit the end of the period - next_iteration_time += period; if (use_sim_time) { cm->get_clock()->sleep_until(current_time + period); } else { + next_iteration_time += period; + const auto time_now = std::chrono::steady_clock::now(); + if (next_iteration_time < time_now) + { + const double time_diff = + static_cast( + std::chrono::duration_cast(time_now - next_iteration_time) + .count()) / + 1.e6; + const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); + const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); + RCLCPP_WARN( + cm->get_logger(), + "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " + "took %f ms (missed cycles : %d).", + cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); + next_iteration_time += (overrun_count * period); + } std::this_thread::sleep_until(next_iteration_time); } } diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index f25ebe9114..c9884947c3 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -76,7 +76,7 @@ class ControllerManagerFixture : public ::testing::Test { pass_robot_description_to_cm_and_rm(robot_description_); } - time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); + time_ = rclcpp::Time(0, 0, cm_->get_trigger_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e68af6345f..27562ccf27 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -69,8 +69,13 @@ TestChainableController::state_interface_configuration() const } controller_interface::return_type TestChainableController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (time.get_clock_type() != RCL_ROS_TIME) + { + throw std::runtime_error( + "ROS Time is required for the chainable controller to update references from subscribers."); + } for (size_t i = 0; i < reference_interfaces_.size(); ++i) { RCLCPP_INFO( @@ -95,8 +100,13 @@ controller_interface::return_type TestChainableController::update_reference_from } controller_interface::return_type TestChainableController::update_and_write_commands( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (time.get_clock_type() != RCL_ROS_TIME) + { + throw std::runtime_error( + "ROS Time is required for the chainable controller to update and write commands."); + } ++internal_counter; for (size_t i = 0; i < command_interfaces_.size(); ++i) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 306799a66e..4bac555858 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -59,8 +59,12 @@ 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) { + if (time.get_clock_type() != RCL_ROS_TIME) + { + throw std::runtime_error("ROS Time is required for the controller to operate."); + } if (is_async()) { std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 4e7065b094..62ba30bbbd 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -366,7 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); - time_ = cm_->get_clock()->now(); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -377,8 +377,8 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) << "The first trigger cycle should have zero period"; - const double exp_period = (cm_->get_clock()->now() - time_).seconds(); - time_ = cm_->get_clock()->now(); + const double exp_period = (cm_->get_trigger_clock()->now() - time_).seconds(); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -673,9 +673,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(); + time_ = cm_->get_trigger_clock()->now(); // set to something nonzero + cm_->get_trigger_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -696,8 +696,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(); + cm_->get_trigger_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -723,8 +723,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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(); + cm_->get_trigger_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); @@ -820,9 +820,9 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); } ASSERT_TRUE(test_controller->is_async()); - time_ = test_controller->get_node()->now(); // set to something nonzero - cm_->get_clock()->sleep_until(time_ + PERIOD); - time_ = cm_->get_clock()->now(); + time_ = cm_->get_trigger_clock()->now(); // set to something nonzero + cm_->get_trigger_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -843,8 +843,8 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an 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(); + cm_->get_trigger_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -870,8 +870,8 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an 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(); + cm_->get_trigger_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_trigger_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index efe903be6f..17dba091f4 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -28,8 +28,12 @@ TestControllerWithInterfaces::on_init() } controller_interface::return_type TestControllerWithInterfaces::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (time.get_clock_type() != RCL_ROS_TIME) + { + throw std::runtime_error("ROS Time is required for the controller to operate."); + } return controller_interface::return_type::OK; } 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 c143ab4862..b462033a93 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -766,14 +766,15 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) } position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const rclcpp::Time zero_time(0, 0, RCL_ROS_TIME); + position_tracking_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(position_tracking_controller->internal_counter, 8u); ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller - diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + diff_drive_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(diff_drive_controller->internal_counter, 10u); // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 @@ -782,11 +783,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); // run the update cycles of the robot localization and odom publisher controller - sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + sensor_fusion_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); - robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + robot_localization_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(robot_localization_controller->internal_counter, 4u); - odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + odom_publisher_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt @@ -800,9 +801,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); // update PID controllers that are writing to hardware - pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + pid_left_wheel_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); - pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + pid_right_wheel_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); // update hardware ('read' is sufficient for test hardware) @@ -1948,9 +1949,10 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add // update controllers std::vector reference = {32.0, 128.0}; - sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const rclcpp::Time zero_time(0, 0, RCL_ROS_TIME); + sensor_fusion_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); + robot_localization_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); + odom_publisher_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); // update 'Position Tracking' controller for (auto & value : diff_drive_controller->reference_interfaces_) @@ -1959,14 +1961,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add } position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + position_tracking_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(position_tracking_controller->internal_counter, 8u); ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller - diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + diff_drive_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(diff_drive_controller->internal_counter, 10u); // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 @@ -1975,9 +1977,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); // update PID controllers that are writing to hardware - pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + pid_left_wheel_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); - pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + pid_right_wheel_controller->update(zero_time, rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); // update hardware ('read' is sufficient for test hardware) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index efe81d0567..70bff7a348 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -90,6 +90,7 @@ controller_manager * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). * The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). +* The controller manager will use a monotonic clock for triggering read-update-write cycles, but when the ``use_sim_time`` parameter is set to true, it will use the ROS Clock for triggering. When monotonic clock is being used, all the hardware components will receive the monotonic time in their read and write method, instead the controllers will always receive the ROS time in their update method irrespective of the clock being used. (`#2046 `_). hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index aab829089a..e1e125a064 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -48,6 +48,9 @@ class Actuator final const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const rclcpp_lifecycle::State & configure(); const rclcpp_lifecycle::State & cleanup(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1f693d1d34..8c31c71ccf 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -102,16 +102,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] logger Logger for the hardware component. * \param[in] clock_interface pointer to the clock interface. - * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ + [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] CallbackReturn init( const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - clock_interface_ = clock_interface; + return this->init(hardware_info, logger, clock_interface->get_clock()); + } + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock pointer to the resource manager clock. + * \param[in] logger Logger for the hardware component. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + { + actuator_clock_ = clock; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -515,7 +531,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return clock of the ActuatorInterface. */ - rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; } /// Get the hardware info of the ActuatorInterface. /** @@ -559,7 +575,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unique_ptr> async_handler_; private: - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Clock::SharedPtr actuator_clock_; rclcpp::Logger actuator_logger_; // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 06b8f51952..3dbb4ab02a 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -52,6 +52,9 @@ class ResourceManager rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); + /// Default constructor for the Resource Manager. + explicit ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger); + /// Constructor for the Resource Manager. /** * The implementation loads the specified urdf and initializes the @@ -64,7 +67,8 @@ class ResourceManager * \param[in] update_rate Update rate of the controller manager to calculate calling frequency * of async components. * \param[in] clock_interface reference to the clock interface of the CM node for getting time - * used for triggering async components. + * used for triggering async components and different read/write component rates. + * \param[in] logger_interface reference to the logger interface of the CM node for logging. */ explicit ResourceManager( const std::string & urdf, @@ -72,6 +76,25 @@ class ResourceManager rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all = false, const unsigned int update_rate = 100); + /// Constructor for the Resource Manager. + /** + * The implementation loads the specified urdf and initializes the + * hardware components listed within as well as populate their respective + * state and command interfaces. + * + * \param[in] urdf string containing the URDF. + * \param[in] activate_all boolean argument indicating if all resources should be immediately + * activated. Currently used only in tests. + * \param[in] update_rate Update rate of the controller manager to calculate calling frequency + * of async components. + * \param[in] clock reference to the clock of the CM node for getting time used for triggering + * async components and different read/write component rates. + * \param[in] logger logger of the CM node for logging. + */ + explicit ResourceManager( + const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, + bool activate_all = false, const unsigned int update_rate = 100); + ResourceManager(const ResourceManager &) = delete; virtual ~ResourceManager(); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e47e34c0d7..833e150653 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -48,6 +48,9 @@ class Sensor final const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const rclcpp_lifecycle::State & configure(); const rclcpp_lifecycle::State & cleanup(); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 58a8b4790a..b93b595340 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -101,16 +101,32 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] logger Logger for the hardware component. * \param[in] clock_interface pointer to the clock interface. - * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ + [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] CallbackReturn init( const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - clock_interface_ = clock_interface; + return this->init(hardware_info, logger, clock_interface->get_clock()); + } + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock pointer to the resource manager clock. + * \param[in] logger Logger for the hardware component. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + { + sensor_clock_ = clock; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -319,7 +335,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return clock of the SensorInterface. */ - rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; } /// Get the hardware info of the SensorInterface. /** @@ -359,7 +375,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unique_ptr> read_async_handler_; private: - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Clock::SharedPtr sensor_clock_; rclcpp::Logger sensor_logger_; // interface names to Handle accessed through getters/setters std::unordered_map sensor_states_map_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 750cbb301a..591ac39435 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -48,6 +48,9 @@ class System final const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const rclcpp_lifecycle::State & configure(); const rclcpp_lifecycle::State & cleanup(); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 7577d0ebdc..96d0005a48 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -105,16 +105,32 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] logger Logger for the hardware component. * \param[in] clock_interface pointer to the clock interface. - * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ + [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] CallbackReturn init( const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - clock_interface_ = clock_interface; + return this->init(hardware_info, logger, clock_interface->get_clock()); + } + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock pointer to the resource manager clock. + * \param[in] logger Logger for the hardware component. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + { + system_clock_ = clock; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -544,7 +560,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return clock of the SystemInterface. */ - rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; } /// Get the hardware info of the SystemInterface. /** @@ -598,7 +614,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector unlisted_commands_; private: - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Clock::SharedPtr system_clock_; rclcpp::Logger system_logger_; // interface names to Handle accessed through getters/setters std::unordered_map system_states_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 65b337aa62..2a7af52468 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -48,15 +48,21 @@ Actuator::Actuator(Actuator && other) noexcept const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + return this->initialize(actuator_info, logger, clock_interface->get_clock()); +} + +const rclcpp_lifecycle::State & Actuator::initialize( + const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { std::unique_lock lock(actuators_mutex_); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(actuator_info, logger, clock_interface)) + switch (impl_->init(actuator_info, logger, clock)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); - last_write_cycle_time_ = clock_interface->get_clock()->now(); + last_read_cycle_time_ = clock->now(); + last_write_cycle_time_ = clock->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e43c650d5d..1bc71794c2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -103,20 +103,33 @@ class ResourceStorage : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), - clock_interface_(clock_interface), rm_logger_(rclcpp::get_logger("resource_manager")) { - if (!clock_interface_) + if (!clock_interface) { throw std::invalid_argument( "Clock interface is nullptr. ResourceManager needs a valid clock interface."); } + rm_clock_ = clock_interface->get_clock(); if (logger_interface) { rm_logger_ = logger_interface->get_logger().get_child("resource_manager"); } } + explicit ResourceStorage(rclcpp::Clock::SharedPtr clock_interface, rclcpp::Logger logger) + : actuator_loader_(pkg_name, actuator_interface_name), + sensor_loader_(pkg_name, sensor_interface_name), + system_loader_(pkg_name, system_interface_name), + rm_clock_(clock_interface), + rm_logger_(logger) + { + if (!rm_clock_) + { + throw std::invalid_argument("Clock is nullptr. ResourceManager needs a valid clock."); + } + } + template [[nodiscard]] bool load_hardware( const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, @@ -188,7 +201,7 @@ class ResourceStorage try { const rclcpp_lifecycle::State new_state = - hardware.initialize(hardware_info, rm_logger_, clock_interface_); + hardware.initialize(hardware_info, rm_logger_, rm_clock_); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) @@ -972,7 +985,7 @@ class ResourceStorage /** * \return clock of the resource storage */ - rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + rclcpp::Clock::SharedPtr get_clock() const { return rm_clock_; } // hardware plugins pluginlib::ClassLoader actuator_loader_; @@ -980,8 +993,7 @@ class ResourceStorage pluginlib::ClassLoader system_loader_; // Logger and Clock interfaces - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Clock::SharedPtr rm_clock_; rclcpp::Logger rm_logger_; std::vector actuators_; @@ -1023,6 +1035,11 @@ ResourceManager::ResourceManager( { } +ResourceManager::ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger) +: resource_storage_(std::make_unique(clock, logger)) +{ +} + ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( @@ -1044,6 +1061,24 @@ ResourceManager::ResourceManager( } } +ResourceManager::ResourceManager( + const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, + bool activate_all, const unsigned int update_rate) +: resource_storage_(std::make_unique(clock, logger)) +{ + load_and_initialize_components(urdf, update_rate); + + if (activate_all) + { + for (auto const & hw_info : resource_storage_->hardware_info_map_) + { + using lifecycle_msgs::msg::State; + rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + set_component_state(hw_info.first, state); + } + } +} + bool ResourceManager::shutdown_components() { std::unique_lock guard(resource_interfaces_lock_); @@ -1753,6 +1788,12 @@ HardwareReadWriteStatus ResourceManager::read( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); + // This is needed while we load and initialize the components + std::unique_lock resource_guard(resources_lock_, std::try_to_lock); + if (!resource_guard.owns_lock()) + { + return read_write_status; + } auto read_components = [&](auto & components) { for (auto & component : components) @@ -1839,6 +1880,12 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); + // This is needed while we load and initialize the components + std::unique_lock resource_guard(resources_lock_, std::try_to_lock); + if (!resource_guard.owns_lock()) + { + return read_write_status; + } auto write_components = [&](auto & components) { for (auto & component : components) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 87ea42790e..5d0a346415 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -46,14 +46,20 @@ Sensor::Sensor(Sensor && other) noexcept const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + return this->initialize(sensor_info, logger, clock_interface->get_clock()); +} + +const rclcpp_lifecycle::State & Sensor::initialize( + const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { std::unique_lock lock(sensors_mutex_); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(sensor_info, logger, clock_interface)) + switch (impl_->init(sensor_info, logger, clock)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_read_cycle_time_ = clock->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 8f626f7c8d..392b2454d4 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -46,15 +46,21 @@ System::System(System && other) noexcept const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + return this->initialize(system_info, logger, clock_interface->get_clock()); +} + +const rclcpp_lifecycle::State & System::initialize( + const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { std::unique_lock lock(system_mutex_); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(system_info, logger, clock_interface)) + switch (impl_->init(system_info, logger, clock)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); - last_write_cycle_time_ = clock_interface->get_clock()->now(); + last_read_cycle_time_ = clock->now(); + last_write_cycle_time_ = clock->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED));