From fe2d2e7f5473f29efc189520aed7aea0f08b6bd6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 7 Feb 2025 14:44:49 +0100 Subject: [PATCH 01/17] First set of changes for using the clock instead of the node clock interface --- .../include/hardware_interface/actuator.hpp | 3 +++ .../hardware_interface/actuator_interface.hpp | 9 ++++----- .../include/hardware_interface/sensor.hpp | 3 +++ .../include/hardware_interface/sensor_interface.hpp | 9 ++++----- .../include/hardware_interface/system.hpp | 3 +++ .../include/hardware_interface/system_interface.hpp | 9 ++++----- hardware_interface/src/actuator.cpp | 12 +++++++++--- hardware_interface/src/resource_manager.cpp | 2 +- hardware_interface/src/sensor.cpp | 10 ++++++++-- hardware_interface/src/system.cpp | 12 +++++++++--- 10 files changed, 48 insertions(+), 24 deletions(-) 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 87b5202734..fb0bd22e38 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -107,10 +107,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { - clock_interface_ = clock_interface; + actuator_clock_ = clock; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -514,7 +513,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. /** @@ -542,7 +541,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/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 c99138fc11..7a63476203 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -106,10 +106,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { - clock_interface_ = clock_interface; + sensor_clock_ = clock; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -318,7 +317,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. /** @@ -342,7 +341,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 4337f5fd19..5fc7b2a124 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -110,10 +110,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { - clock_interface_ = clock_interface; + system_clock_ = clock; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; if (info_.is_async) @@ -543,7 +542,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. /** @@ -581,7 +580,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 790898cfed..e1906be32f 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 a99ae3db3b..6a82c84bfa 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -188,7 +188,7 @@ class ResourceStorage try { const rclcpp_lifecycle::State new_state = - hardware.initialize(hardware_info, rm_logger_, clock_interface_); + hardware.initialize(hardware_info, rm_logger_, clock_interface_->get_clock()); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 0ef4da36d4..07950ce6e6 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 694e45f2f3..69b94c2a83 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)); From a4f8e59eb62ce35e63464054accf19b8003bad88 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 9 Feb 2025 22:46:36 +0100 Subject: [PATCH 02/17] Add new constructors and use rclcpp::Clock internally instead of NodeClockInterface --- .../hardware_interface/resource_manager.hpp | 25 +++++++++- hardware_interface/src/resource_manager.cpp | 47 ++++++++++++++++--- 2 files changed, 65 insertions(+), 7 deletions(-) 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/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6a82c84bfa..b238891fc7 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_->get_clock()); + hardware.initialize(hardware_info, rm_logger_, rm_clock_); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) @@ -968,7 +981,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_; @@ -976,8 +989,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_; @@ -1019,6 +1031,11 @@ ResourceManager::ResourceManager( { } +ResourceManager::ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger) +: resource_storage_(std::make_unique(clock, logger)) +{ +} + ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( @@ -1040,6 +1057,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_); From 9ee7e115f266a7a87eb0f91a417582a44f291b0f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 9 Feb 2025 23:29:42 +0100 Subject: [PATCH 03/17] Add trigger clock to have time critical code to use the steady clock instead of system/ros clock --- .../controller_manager/controller_manager.hpp | 14 +++++++++ controller_manager/src/controller_manager.cpp | 29 ++++++++++--------- 2 files changed, 30 insertions(+), 13 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 49eed135be..f588939d9d 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 bef2985c0b..26afd4b671 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -235,8 +235,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>( @@ -247,6 +245,8 @@ ControllerManager::ControllerManager( cm_node_options_(options) { initialize_parameters(); + resource_manager_ = + std::make_unique(trigger_clock_, this->get_logger()); init_controller_manager(); } @@ -267,8 +267,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(); } @@ -403,6 +402,9 @@ 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); } catch (const std::exception & e) { @@ -633,8 +635,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(); @@ -1819,7 +1821,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 @@ -2489,7 +2491,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 " @@ -2530,10 +2532,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; @@ -2553,8 +2555,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( @@ -2570,7 +2571,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()) @@ -2801,6 +2802,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) { From 58f49fbeef66439e67b72bbbc3e03c127223e6e8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 10:10:35 +0100 Subject: [PATCH 04/17] added log for the type of trigger clock --- controller_manager/src/controller_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 26afd4b671..99ab12bb47 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -405,6 +405,9 @@ void ControllerManager::initialize_parameters() 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) { From 916f0bed30dcbb596894af4cf070ea356d4fd6a6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 11:04:13 +0100 Subject: [PATCH 05/17] Deprecate the previous init methods of the hardware component interfaces --- .../hardware_interface/actuator_interface.hpp | 19 ++++++++++++++++++- .../hardware_interface/sensor_interface.hpp | 19 ++++++++++++++++++- .../hardware_interface/system_interface.hpp | 19 ++++++++++++++++++- 3 files changed, 54 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index fb0bd22e38..70cfcd5ea4 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -101,8 +101,25 @@ 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) + { + 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. */ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 7a63476203..2cf4817f1d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -100,8 +100,25 @@ 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) + { + 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. */ diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 5fc7b2a124..82361f8179 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -104,8 +104,25 @@ 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) + { + 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. */ From a01fe30bd9f19b3e00e9a558d699cc27afa4a952 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 11:36:39 +0100 Subject: [PATCH 06/17] Use trigger clock instead of ROS Clock --- controller_manager/src/ros2_control_node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 176c346b3f..53bf015997 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -124,24 +124,24 @@ 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()); + auto const cm_now = std::chrono::nanoseconds(cm->get_trigger_clock()->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() - period; 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; From 032715f7635e9acc370e9603fd9b2e4c7f459d45 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 17:25:16 +0100 Subject: [PATCH 07/17] Add resource_guard to avoid triggering cycles at the startup --- hardware_interface/src/resource_manager.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b238891fc7..a2dc797892 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1784,6 +1784,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) @@ -1870,6 +1876,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) From 6c6cd79079d56bdb534881d1bde7906dc6e36517 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 18:46:34 +0100 Subject: [PATCH 08/17] Add the overrun detection and sleep to use complete steady clock approach --- controller_manager/src/ros2_control_node.cpp | 27 ++++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 53bf015997..ef3d96d707 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -124,12 +124,12 @@ 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->get_trigger_clock()->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->get_trigger_clock()->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()) { @@ -144,13 +144,30 @@ int main(int argc, char ** argv) 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); } } From ecf095533456fd02ff7fd451c8517a3224c17fc9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 22:14:52 +0100 Subject: [PATCH 09/17] Use monotonic clock on realtime kernels --- controller_manager/src/controller_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 99ab12bb47..8f01c2c0b2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -402,9 +402,9 @@ 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); + trigger_clock_ = realtime_tools::has_realtime_kernel() + ? 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"); From 2182de865ba1bc9604e35cfab57d7756d88e43ca Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 22:14:57 +0100 Subject: [PATCH 10/17] update release notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3dec704f23..da574f6ab1 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -88,6 +88,7 @@ controller_manager * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * 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 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). +* When a realtime kernel is present, the controller manager will use a monotonic clock for triggering read-update-write cycles (`#2046 `_). hardware_interface ****************** From 79bd7a339233948faef11a3b752f75ed75a8e710 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 22:24:05 +0100 Subject: [PATCH 11/17] fix the condition of the clock --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8f01c2c0b2..7340c0bd00 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -403,8 +403,8 @@ void ControllerManager::initialize_parameters() params_ = std::make_shared(cm_param_listener_->get_params()); update_rate_ = static_cast(params_->update_rate); trigger_clock_ = realtime_tools::has_realtime_kernel() - ? this->get_clock() - : std::make_shared(RCL_STEADY_TIME); + ? std::make_shared(RCL_STEADY_TIME) + : this->get_clock(); RCLCPP_INFO( get_logger(), "Using %s clock for triggering controller manager cycles.", trigger_clock_->get_clock_type() == RCL_STEADY_TIME ? "Steady (Monotonic)" : "ROS"); From 6a7c368d2020a3d4ea7fd16a3577aa59edd1c300 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Feb 2025 23:02:20 +0100 Subject: [PATCH 12/17] update documentation of the method --- .../include/controller_manager/controller_manager.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f588939d9d..10a8329c2d 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -216,8 +216,8 @@ class ControllerManager : public rclcpp::Node * 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. + * @note When the controller manager is used with real-time kernel, the trigger clock should be + * monotonic and not affected by the system time changes. Otherwise, the clock should be ROS Clock * * \returns trigger clock of the controller manager. */ From 3c36a4e0aa9ad239567d4e8fbc99c2c1272dfe90 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 11 Feb 2025 21:33:43 +0100 Subject: [PATCH 13/17] Use monotonic for all non simulation cases --- controller_manager/src/controller_manager.cpp | 6 ++-- .../test/controller_manager_test_common.hpp | 2 +- .../test/test_controller_manager.cpp | 34 +++++++++---------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7340c0bd00..99ab12bb47 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -402,9 +402,9 @@ 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); - trigger_clock_ = realtime_tools::has_realtime_kernel() - ? std::make_shared(RCL_STEADY_TIME) - : this->get_clock(); + 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"); 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_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))); From 5b6fb02704d2d487fddbe995051473c7be5c100c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 11 Feb 2025 21:33:54 +0100 Subject: [PATCH 14/17] update documentation --- .../include/controller_manager/controller_manager.hpp | 4 ++-- doc/release_notes.rst | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 10a8329c2d..f588939d9d 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -216,8 +216,8 @@ class ControllerManager : public rclcpp::Node * The method is used to get the clock that is used for triggering the controllers and the * hardware components. * - * @note When the controller manager is used with real-time kernel, the trigger clock should be - * monotonic and not affected by the system time changes. Otherwise, the clock should be ROS Clock + * @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. */ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index da574f6ab1..d931425155 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -88,7 +88,7 @@ controller_manager * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * 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 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). -* When a realtime kernel is present, the controller manager will use a monotonic clock for triggering read-update-write cycles (`#2046 `_). +* 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 (`#2046 `_). hardware_interface ****************** From cecf3c310b27d15ca2da184453284343b02492f6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Feb 2025 02:16:26 +0100 Subject: [PATCH 15/17] add tests to check that the controllers receive ROS time but not the Steady clock time --- .../test_chainable_controller.cpp | 14 +++++++-- .../test/test_controller/test_controller.cpp | 6 +++- .../test_controller_with_interfaces.cpp | 6 +++- ...llers_chaining_with_controller_manager.cpp | 30 ++++++++++--------- 4 files changed, 38 insertions(+), 18 deletions(-) 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_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) From 9cf45841a394e1272421dc70d43daa80e2f3ed99 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 14 Feb 2025 09:33:45 +0100 Subject: [PATCH 16/17] add more information to the release notes --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 48d3e98c28..7c1d13b7bc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -90,7 +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 (`#2046 `_). +* 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 ****************** From 0eaa6020c8fd226a460e72eefc628fe8a6fd8153 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Feb 2025 17:53:26 +0100 Subject: [PATCH 17/17] Update doc/release_notes.rst MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7c1d13b7bc..70bff7a348 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -90,7 +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 `_). +* 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 ******************