diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index e45e61ea53..d0b2c44012 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -89,7 +89,174 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \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 hardware_interface::HardwareComponentParams & params); + CallbackReturn init(const hardware_interface::HardwareComponentParams & params) + { + clock_ = params.clock; + logger_ = params.logger; + info_ = params.hardware_info; + if (params.hardware_info.is_async) + { + realtime_tools::AsyncFunctionHandlerParams async_thread_params; + async_thread_params.thread_priority = info_.async_params.thread_priority; + async_thread_params.scheduling_policy = + realtime_tools::AsyncSchedulingPolicy(info_.async_params.scheduling_policy); + async_thread_params.cpu_affinity_cores = info_.async_params.cpu_affinity_cores; + async_thread_params.clock = params.clock; + async_thread_params.logger = get_logger(); + async_thread_params.exec_rate = params.hardware_info.rw_rate; + async_thread_params.print_warnings = info_.async_params.print_warnings; + RCLCPP_INFO( + get_logger(), "Starting async handler with scheduler priority: %d and policy : %s", + info_.async_params.thread_priority, + async_thread_params.scheduling_policy.to_string().c_str()); + async_handler_ = std::make_unique>(); + const bool is_sensor_type = (info_.type == "sensor"); + async_handler_->init( + [this, is_sensor_type](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if ( + !is_sensor_type && lifecycle_id_cache_.load(std::memory_order_acquire) == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + const auto write_start_time = std::chrono::steady_clock::now(); + const auto ret_write = write(time, period); + const auto write_end_time = std::chrono::steady_clock::now(); + write_return_info_.store(ret_write, std::memory_order_release); + write_execution_time_.store( + std::chrono::duration_cast( + write_end_time - write_start_time), + std::memory_order_release); + if (ret_write != return_type::OK) + { + return ret_write; + } + } + const auto read_start_time = std::chrono::steady_clock::now(); + const auto ret_read = read(time, period); + const auto read_end_time = std::chrono::steady_clock::now(); + read_return_info_.store(ret_read, std::memory_order_release); + read_execution_time_.store( + std::chrono::duration_cast(read_end_time - read_start_time), + std::memory_order_release); + return ret_read; + }, + async_thread_params); + async_handler_->start_thread(); + } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = hardware_interface::to_lower_case(params.hardware_info.name); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared( + node_name, params.node_namespace, get_hardware_component_node_options()); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + + double publish_rate = 0.0; + auto it = info_.hardware_parameters.find("status_publish_rate"); + if (it != info_.hardware_parameters.end()) + { + try + { + publish_rate = hardware_interface::stod(it->second); + } + catch (const std::invalid_argument &) + { + RCLCPP_WARN( + get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.", + publish_rate); + } + } + + if (publish_rate == 0.0) + { + RCLCPP_INFO( + get_logger(), + "`status_publish_rate` is set to 0.0, hardware status publisher will not be created."); + } + else + { + control_msgs::msg::HardwareStatus status_msg_template; + if (init_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) + { + RCLCPP_ERROR(get_logger(), "User-defined 'init_hardware_status_message' failed."); + return CallbackReturn::ERROR; + } + + if (!status_msg_template.hardware_device_states.empty()) + { + if (!hardware_component_node_) + { + RCLCPP_WARN( + get_logger(), + "Hardware status message was configured, but no node is available for the publisher. " + "Publisher will not be created."); + } + else + { + try + { + hardware_status_publisher_ = + hardware_component_node_->create_publisher( + "~/hardware_status", rclcpp::SystemDefaultsQoS()); + + hardware_status_timer_ = hardware_component_node_->create_wall_timer( + std::chrono::duration(1.0 / publish_rate), + [this]() + { + std::optional msg_to_publish_opt; + hardware_status_box_.get(msg_to_publish_opt); + + if (msg_to_publish_opt.has_value() && hardware_status_publisher_) + { + control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); + if (update_hardware_status_message(msg) != return_type::OK) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 1000, + "User's update_hardware_status_message() failed for '%s'.", + info_.name.c_str()); + return; + } + msg.header.stamp = this->get_clock()->now(); + hardware_status_publisher_->publish(msg); + } + }); + hardware_status_box_.set(std::make_optional(status_msg_template)); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception during publisher/timer setup for hardware status: %s", + e.what()); + return CallbackReturn::ERROR; + } + } + } + else + { + RCLCPP_WARN( + get_logger(), + "`status_publish_rate` was set to a non-zero value, but no hardware status message was " + "configured. Publisher will not be created. Are you sure " + "init_hardware_status_message() is set up properly?"); + } + } + + hardware_interface::HardwareComponentInterfaceParams interface_params; + interface_params.hardware_info = info_; + interface_params.executor = params.executor; + return on_init(interface_params); + }; /// User-overridable method to configure the structure of the HardwareStatus message. /**