Program Listing for File async_components.hpp
↰ Return to documentation for file (include/hardware_interface/async_components.hpp
)
// Copyright 2023 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
#include <atomic>
#include <thread>
#include <type_traits>
#include <variant>
#include "hardware_interface/actuator.hpp"
#include "hardware_interface/sensor.hpp"
#include "hardware_interface/system.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/time.hpp"
namespace hardware_interface
{
class AsyncComponentThread
{
public:
explicit AsyncComponentThread(
unsigned int update_rate,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
: cm_update_rate_(update_rate), clock_interface_(clock_interface)
{
}
// Fills the internal variant with the desired component.
template <typename T>
void register_component(T * component)
{
hardware_component_ = component;
}
AsyncComponentThread(const AsyncComponentThread & t) = delete;
AsyncComponentThread(AsyncComponentThread && t) = delete;
// Destructor, called when the component is erased from its map.
~AsyncComponentThread()
{
terminated_.store(true, std::memory_order_seq_cst);
if (write_and_read_.joinable())
{
write_and_read_.join();
}
}
void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
void write_and_read()
{
using TimePoint = std::chrono::system_clock::time_point;
std::visit(
[this](auto & component)
{
auto previous_time = clock_interface_->get_clock()->now();
while (!terminated_.load(std::memory_order_relaxed))
{
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
TimePoint next_iteration_time =
TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));
if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
auto current_time = clock_interface_->get_clock()->now();
auto measured_period = current_time - previous_time;
previous_time = current_time;
if (!first_iteration)
{
component->write(clock_interface_->get_clock()->now(), measured_period);
}
component->read(clock_interface_->get_clock()->now(), measured_period);
first_iteration = false;
}
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
}
},
hardware_component_);
}
private:
std::atomic<bool> terminated_{false};
std::variant<Actuator *, System *, Sensor *> hardware_component_;
std::thread write_and_read_{};
unsigned int cm_update_rate_;
bool first_iteration = true;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
};
}; // namespace hardware_interface
#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_