ros2_control - rolling
async_components.hpp
1 // Copyright 2023 ros2_control development team
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
16 #define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
17 
18 #include <atomic>
19 #include <thread>
20 #include <type_traits>
21 #include <variant>
22 
23 #include "hardware_interface/actuator.hpp"
24 #include "hardware_interface/sensor.hpp"
25 #include "hardware_interface/system.hpp"
26 #include "lifecycle_msgs/msg/state.hpp"
27 #include "rclcpp/duration.hpp"
28 #include "rclcpp/node.hpp"
29 #include "rclcpp/time.hpp"
30 
31 namespace hardware_interface
32 {
33 
35 {
36 public:
37  explicit AsyncComponentThread(
38  unsigned int update_rate,
39  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
40  : cm_update_rate_(update_rate), clock_interface_(clock_interface)
41  {
42  }
43 
44  // Fills the internal variant with the desired component.
45  template <typename T>
46  void register_component(T * component)
47  {
48  hardware_component_ = component;
49  }
50 
51  AsyncComponentThread(const AsyncComponentThread & t) = delete;
53 
54  // Destructor, called when the component is erased from its map.
56  {
57  terminated_.store(true, std::memory_order_seq_cst);
58  if (write_and_read_.joinable())
59  {
60  write_and_read_.join();
61  }
62  }
64 
68  void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
69 
71 
77  {
78  using TimePoint = std::chrono::system_clock::time_point;
79 
80  std::visit(
81  [this](auto & component)
82  {
83  auto previous_time = clock_interface_->get_clock()->now();
84  while (!terminated_.load(std::memory_order_relaxed))
85  {
86  auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
87  TimePoint next_iteration_time =
88  TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));
89 
90  if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
91  {
92  auto current_time = clock_interface_->get_clock()->now();
93  auto measured_period = current_time - previous_time;
94  previous_time = current_time;
95 
96  if (!first_iteration)
97  {
98  component->write(clock_interface_->get_clock()->now(), measured_period);
99  }
100  component->read(clock_interface_->get_clock()->now(), measured_period);
101  first_iteration = false;
102  }
103  next_iteration_time += period;
104  std::this_thread::sleep_until(next_iteration_time);
105  }
106  },
107  hardware_component_);
108  }
109 
110 private:
111  std::atomic<bool> terminated_{false};
112  std::variant<Actuator *, System *, Sensor *> hardware_component_;
113  std::thread write_and_read_{};
114 
115  unsigned int cm_update_rate_;
116  bool first_iteration = true;
117  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
118 };
119 
120 }; // namespace hardware_interface
121 
122 #endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
Definition: async_components.hpp:35
void write_and_read()
Periodically execute the component's write and read methods.
Definition: async_components.hpp:76
void activate()
Creates the component's thread.
Definition: async_components.hpp:68
Definition: actuator.hpp:31