ros2_control - rolling
Loading...
Searching...
No Matches
loaned_command_interface.hpp
1// Copyright 2020 Open Source Robotics Foundation, Inc.
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__LOANED_COMMAND_INTERFACE_HPP_
16#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
17
18#include <functional>
19#include <limits>
20#include <string>
21#include <thread>
22#include <utility>
23
24#include "hardware_interface/handle.hpp"
25#include "rclcpp/logging.hpp"
26
27namespace hardware_interface
28{
30{
31public:
32 using Deleter = std::function<void(void)>;
33
34 [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedCommandInterface(
35 CommandInterface & command_interface)
36 : LoanedCommandInterface(command_interface, nullptr)
37 {
38 }
39
40 [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface(
41 CommandInterface & command_interface, Deleter && deleter)
42 : command_interface_(command_interface), deleter_(std::forward<Deleter>(deleter))
43 {
44 }
45
46 LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter)
47 : command_interface_(*command_interface), deleter_(std::forward<Deleter>(deleter))
48 {
49 }
50
51 LoanedCommandInterface(const LoanedCommandInterface & other) = delete;
52
54
56 {
57 auto logger = rclcpp::get_logger(command_interface_.get_name());
58 RCLCPP_WARN_EXPRESSION(
59 rclcpp::get_logger(get_name()),
60 (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
61 "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
62 "%u get_value calls",
63 get_name().c_str(), get_value_statistics_.timeout_counter,
64 (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
65 get_value_statistics_.failed_counter,
66 (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
67 get_value_statistics_.total_counter);
68 RCLCPP_WARN_EXPRESSION(
69 rclcpp::get_logger(get_name()),
70 (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0),
71 "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
72 "%u set_value calls",
73 get_name().c_str(), set_value_statistics_.timeout_counter,
74 (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter,
75 set_value_statistics_.failed_counter,
76 (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter,
77 set_value_statistics_.total_counter);
78 if (deleter_)
79 {
80 deleter_();
81 }
82 }
83
84 const std::string & get_name() const { return command_interface_.get_name(); }
85
86 const std::string & get_interface_name() const { return command_interface_.get_interface_name(); }
87
88 [[deprecated(
89 "Replaced by get_name method, which is semantically more correct")]] const std::string
90 get_full_name() const
91 {
92 return command_interface_.get_name();
93 }
94
95 const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); }
96
112 template <typename T>
113 [[nodiscard]] bool set_value(const T & value, unsigned int max_tries = 10)
114 {
115 unsigned int nr_tries = 0;
116 ++set_value_statistics_.total_counter;
117 while (!command_interface_.set_value(value))
118 {
119 ++set_value_statistics_.failed_counter;
120 ++nr_tries;
121 if (nr_tries == max_tries)
122 {
123 ++set_value_statistics_.timeout_counter;
124 return false;
125 }
126 std::this_thread::yield();
127 }
128 return true;
129 }
130
131 [[deprecated(
132 "Use std::optional<T> get_optional() instead to retrieve the value. This method will be "
133 "removed by the ROS 2 Kilted Kaiju release.")]]
134 double get_value() const
135 {
136 double value = std::numeric_limits<double>::quiet_NaN();
137 if (get_value(value))
138 {
139 return value;
140 }
141 else
142 {
143 return std::numeric_limits<double>::quiet_NaN();
144 }
145 }
146
161 template <typename T = double>
162 [[nodiscard]] std::optional<T> get_optional(unsigned int max_tries = 10) const
163 {
164 unsigned int nr_tries = 0;
165 do
166 {
167 ++get_value_statistics_.total_counter;
168 const std::optional<T> data = command_interface_.get_optional<T>();
169 if (data.has_value())
170 {
171 return data;
172 }
173 ++get_value_statistics_.failed_counter;
174 ++nr_tries;
175 std::this_thread::yield();
176 } while (nr_tries < max_tries);
177
178 ++get_value_statistics_.timeout_counter;
179 return std::nullopt;
180 }
181
197 template <typename T>
198 [[deprecated(
199 "Use std::optional<T> get_optional() instead to retrieve the value. This method will be "
200 "removed by the ROS 2 Kilted Kaiju release.")]] [[nodiscard]] bool
201 get_value(T & value, unsigned int max_tries = 10) const
202 {
203 unsigned int nr_tries = 0;
204 ++get_value_statistics_.total_counter;
205 while (!command_interface_.get_value(value))
206 {
207 ++get_value_statistics_.failed_counter;
208 ++nr_tries;
209 if (nr_tries == max_tries)
210 {
211 ++get_value_statistics_.timeout_counter;
212 return false;
213 }
214 std::this_thread::yield();
215 }
216 return true;
217 }
218
219protected:
220 CommandInterface & command_interface_;
221 Deleter deleter_;
222
223private:
224 struct HandleRTStatistics
225 {
226 unsigned int total_counter = 0;
227 unsigned int failed_counter = 0;
228 unsigned int timeout_counter = 0;
229 };
230 mutable HandleRTStatistics get_value_statistics_;
231 HandleRTStatistics set_value_statistics_;
232};
233
234} // namespace hardware_interface
235#endif // HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
Definition handle.hpp:290
bool set_value(const T &value)
Set the value of the handle.
Definition handle.hpp:198
std::optional< T > get_optional() const
Get the value of the handle.
Definition handle.hpp:143
Definition loaned_command_interface.hpp:30
bool set_value(const T &value, unsigned int max_tries=10)
Set the value of the command interface.
Definition loaned_command_interface.hpp:113
bool get_value(T &value, unsigned int max_tries=10) const
Get the value of the command interface.
Definition loaned_command_interface.hpp:201
std::optional< T > get_optional(unsigned int max_tries=10) const
Get the value of the command interface.
Definition loaned_command_interface.hpp:162
Definition actuator.hpp:33