ros2_control - rolling
Loading...
Searching...
No Matches
handle.hpp
1// Copyright 2020 PAL Robotics S.L.
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__HANDLE_HPP_
16#define HARDWARE_INTERFACE__HANDLE_HPP_
17
18#include <algorithm>
19#include <limits>
20#include <memory>
21#include <mutex>
22#include <shared_mutex>
23#include <string>
24#include <utility>
25#include <variant>
26
27#include "hardware_interface/hardware_info.hpp"
28#include "hardware_interface/macros.hpp"
29
30namespace hardware_interface
31{
32
33using HANDLE_DATATYPE = std::variant<std::monostate, double>;
34
36class Handle
37{
38public:
39 [[deprecated("Use InterfaceDescription for initializing the Interface")]]
40
41 Handle(
42 const std::string & prefix_name, const std::string & interface_name,
43 double * value_ptr = nullptr)
44 : prefix_name_(prefix_name),
45 interface_name_(interface_name),
46 handle_name_(prefix_name_ + "/" + interface_name_),
47 value_ptr_(value_ptr)
48 {
49 }
50
51 explicit Handle(const InterfaceDescription & interface_description)
52 : prefix_name_(interface_description.get_prefix_name()),
53 interface_name_(interface_description.get_interface_name()),
54 handle_name_(interface_description.get_name())
55 {
56 // As soon as multiple datatypes are used in HANDLE_DATATYPE
57 // we need to initialize according the type passed in interface description
58 value_ = std::numeric_limits<double>::quiet_NaN();
59 value_ptr_ = std::get_if<double>(&value_);
60 }
61
62 [[deprecated("Use InterfaceDescription for initializing the Interface")]]
63
64 explicit Handle(const std::string & interface_name)
65 : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr)
66 {
67 }
68
69 [[deprecated("Use InterfaceDescription for initializing the Interface")]]
70
71 explicit Handle(const char * interface_name)
72 : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr)
73 {
74 }
75
76 Handle(const Handle & other) noexcept { copy(other); }
77
78 Handle & operator=(const Handle & other)
79 {
80 if (this != &other)
81 {
82 copy(other);
83 }
84 return *this;
85 }
86
87 Handle(Handle && other) noexcept { swap(*this, other); }
88
89 Handle & operator=(Handle && other)
90 {
91 swap(*this, other);
92 return *this;
93 }
94
95 virtual ~Handle() = default;
96
98 inline operator bool() const { return value_ptr_ != nullptr; }
99
100 const std::string & get_name() const { return handle_name_; }
101
102 const std::string & get_interface_name() const { return interface_name_; }
103
104 [[deprecated(
105 "Replaced by get_name method, which is semantically more correct")]] const std::string &
106 get_full_name() const
107 {
108 return get_name();
109 }
110
111 const std::string & get_prefix_name() const { return prefix_name_; }
112
113 [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]]
114 double get_value() const
115 {
116 std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
117 if (!lock.owns_lock())
118 {
119 return std::numeric_limits<double>::quiet_NaN();
120 }
121 // BEGIN (Handle export change): for backward compatibility
122 // TODO(Manuel) return value_ if old functionality is removed
123 THROW_ON_NULLPTR(value_ptr_);
124 return *value_ptr_;
125 // END
126 }
127
128 [[nodiscard]] bool get_value(double & value) const
129 {
130 std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
131 if (!lock.owns_lock())
132 {
133 return false;
134 }
135 // BEGIN (Handle export change): for backward compatibility
136 // TODO(Manuel) set value directly if old functionality is removed
137 THROW_ON_NULLPTR(value_ptr_);
138 value = *value_ptr_;
139 return true;
140 // END
141 }
142
143 [[nodiscard]] bool set_value(double value)
144 {
145 std::unique_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
146 if (!lock.owns_lock())
147 {
148 return false;
149 }
150 // BEGIN (Handle export change): for backward compatibility
151 // TODO(Manuel) set value_ directly if old functionality is removed
152 THROW_ON_NULLPTR(this->value_ptr_);
153 *this->value_ptr_ = value;
154 return true;
155 // END
156 }
157
158private:
159 void copy(const Handle & other) noexcept
160 {
161 std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
162 std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
163 prefix_name_ = other.prefix_name_;
164 interface_name_ = other.interface_name_;
165 handle_name_ = other.handle_name_;
166 value_ = other.value_;
167 if (std::holds_alternative<std::monostate>(value_))
168 {
169 value_ptr_ = other.value_ptr_;
170 }
171 else
172 {
173 value_ptr_ = std::get_if<double>(&value_);
174 }
175 }
176
177 void swap(Handle & first, Handle & second) noexcept
178 {
179 std::unique_lock<std::shared_mutex> lock(first.handle_mutex_);
180 std::unique_lock<std::shared_mutex> lock_this(second.handle_mutex_);
181 std::swap(first.prefix_name_, second.prefix_name_);
182 std::swap(first.interface_name_, second.interface_name_);
183 std::swap(first.handle_name_, second.handle_name_);
184 std::swap(first.value_, second.value_);
185 std::swap(first.value_ptr_, second.value_ptr_);
186 }
187
188protected:
189 std::string prefix_name_;
190 std::string interface_name_;
191 std::string handle_name_;
192 HANDLE_DATATYPE value_ = std::monostate{};
193 // BEGIN (Handle export change): for backward compatibility
194 // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
195 double * value_ptr_;
196 // END
197 mutable std::shared_mutex handle_mutex_;
198};
199
200class StateInterface : public Handle
201{
202public:
203 explicit StateInterface(const InterfaceDescription & interface_description)
204 : Handle(interface_description)
205 {
206 }
207
208 StateInterface(const StateInterface & other) = default;
209
210 StateInterface(StateInterface && other) = default;
211
212 using Handle::Handle;
213
214 using SharedPtr = std::shared_ptr<StateInterface>;
215 using ConstSharedPtr = std::shared_ptr<const StateInterface>;
216};
217
219{
220public:
221 explicit CommandInterface(const InterfaceDescription & interface_description)
222 : Handle(interface_description)
223 {
224 }
226
231 CommandInterface(const CommandInterface & other) = delete;
232
233 CommandInterface(CommandInterface && other) = default;
234
235 using Handle::Handle;
236
237 using SharedPtr = std::shared_ptr<CommandInterface>;
238};
239
240} // namespace hardware_interface
241
242#endif // HARDWARE_INTERFACE__HANDLE_HPP_
Definition handle.hpp:219
CommandInterface(const CommandInterface &other)=delete
CommandInterface copy constructor is actively deleted.
A handle used to get and set a value on a given interface.
Definition handle.hpp:37
Definition handle.hpp:201
Definition actuator.hpp:33
Definition hardware_info.hpp:138