ros2_control - rolling
controller_interface_base.hpp
1 // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt)
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 CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
16 #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
17 
18 #include <memory>
19 #include <string>
20 #include <utility>
21 #include <vector>
22 
23 #include "controller_interface/visibility_control.h"
24 #include "realtime_tools/async_function_handler.hpp"
25 
26 #include "hardware_interface/handle.hpp"
27 #include "hardware_interface/loaned_command_interface.hpp"
28 #include "hardware_interface/loaned_state_interface.hpp"
29 
30 #include "rclcpp/version.h"
31 #include "rclcpp_lifecycle/lifecycle_node.hpp"
32 
33 namespace controller_interface
34 {
35 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
36 
37 enum class return_type : std::uint8_t
38 {
39  OK = 0,
40  ERROR = 1,
41 };
42 
44 
49 enum class interface_configuration_type : std::uint8_t
50 {
51  ALL = 0,
52  INDIVIDUAL = 1,
53  NONE = 2,
54 };
55 
58 {
59  interface_configuration_type type;
60  std::vector<std::string> names = {};
61 };
62 
64 {
65  void reset()
66  {
67  total_triggers = 0;
68  failed_triggers = 0;
69  }
70 
71  unsigned int total_triggers;
72  unsigned int failed_triggers;
73 };
74 
86 {
87  bool successful = true;
88  return_type result = return_type::OK;
89  std::optional<std::chrono::nanoseconds> execution_time = std::nullopt;
90  std::optional<rclcpp::Duration> period = std::nullopt;
91 };
92 
98 class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
99 {
100 public:
101  CONTROLLER_INTERFACE_PUBLIC
102  ControllerInterfaceBase() = default;
103 
104  CONTROLLER_INTERFACE_PUBLIC
105  virtual ~ControllerInterfaceBase() = default;
106 
108 
119  CONTROLLER_INTERFACE_PUBLIC
121 
123 
136  CONTROLLER_INTERFACE_PUBLIC
138 
140 
148  CONTROLLER_INTERFACE_PUBLIC
149  virtual void assign_interfaces(
150  std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
151  std::vector<hardware_interface::LoanedStateInterface> && state_interfaces);
152 
154 
157  CONTROLLER_INTERFACE_PUBLIC
158  virtual void release_interfaces();
159 
160  CONTROLLER_INTERFACE_PUBLIC
161  return_type init(
162  const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
163  const std::string & node_namespace, const rclcpp::NodeOptions & node_options);
164 
166  /*
167  * Override default implementation for configure of LifecycleNode to get parameters.
168  */
169  CONTROLLER_INTERFACE_PUBLIC
170  const rclcpp_lifecycle::State & configure();
171 
173  CONTROLLER_INTERFACE_PUBLIC
174  virtual CallbackReturn on_init() = 0;
175 
185  CONTROLLER_INTERFACE_PUBLIC
186  virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
187 
200  CONTROLLER_INTERFACE_PUBLIC
201  ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);
202 
203  CONTROLLER_INTERFACE_PUBLIC
204  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
205 
206  CONTROLLER_INTERFACE_PUBLIC
207  std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
208 
209  CONTROLLER_INTERFACE_PUBLIC
210  const rclcpp_lifecycle::State & get_lifecycle_state() const;
211 
212  CONTROLLER_INTERFACE_PUBLIC
213  unsigned int get_update_rate() const;
214 
215  CONTROLLER_INTERFACE_PUBLIC
216  bool is_async() const;
217 
218  CONTROLLER_INTERFACE_PUBLIC
219  const std::string & get_robot_description() const;
220 
232  CONTROLLER_INTERFACE_PUBLIC
233  virtual rclcpp::NodeOptions define_custom_node_options() const
234  {
235  rclcpp::NodeOptions node_options;
236 // \note The versions conditioning is added here to support the source-compatibility with Humble
237 #if RCLCPP_VERSION_MAJOR >= 21
238  node_options.enable_logger_service(true);
239 #else
240  node_options.allow_undeclared_parameters(true);
241  node_options.automatically_declare_parameters_from_overrides(true);
242 #endif
243  return node_options;
244  }
245 
247 
253  template <typename ParameterT>
254  auto auto_declare(const std::string & name, const ParameterT & default_value)
255  {
256  if (!node_->has_parameter(name))
257  {
258  return node_->declare_parameter<ParameterT>(name, default_value);
259  }
260  else
261  {
262  return node_->get_parameter(name).get_value<ParameterT>();
263  }
264  }
265 
266  // Methods for chainable controller types with default values so we can put all controllers into
267  // one list in Controller Manager
268 
270 
275  CONTROLLER_INTERFACE_PUBLIC
276  virtual bool is_chainable() const = 0;
277 
284  CONTROLLER_INTERFACE_PUBLIC
285  virtual std::vector<hardware_interface::CommandInterface::SharedPtr>
287 
294  CONTROLLER_INTERFACE_PUBLIC
295  virtual std::vector<hardware_interface::StateInterface::ConstSharedPtr>
297 
306  CONTROLLER_INTERFACE_PUBLIC
307  virtual bool set_chained_mode(bool chained_mode) = 0;
308 
310 
317  CONTROLLER_INTERFACE_PUBLIC
318  virtual bool is_in_chained_mode() const = 0;
319 
330  CONTROLLER_INTERFACE_PUBLIC
332 
333 protected:
334  std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
335  std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
336 
337 private:
338  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
339  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
340  unsigned int update_rate_ = 0;
341  bool is_async_ = false;
342  std::string urdf_ = "";
343  ControllerUpdateStats trigger_stats_;
344 };
345 
346 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
347 
348 } // namespace controller_interface
349 
350 #endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
Definition: controller_interface_base.hpp:99
auto auto_declare(const std::string &name, const ParameterT &default_value)
Declare and initialize a parameter with a type.
Definition: controller_interface_base.hpp:254
virtual CONTROLLER_INTERFACE_PUBLIC std::vector< hardware_interface::StateInterface::ConstSharedPtr > export_state_interfaces()=0
virtual CONTROLLER_INTERFACE_PUBLIC InterfaceConfiguration state_interface_configuration() const =0
Get configuration for controller's required state interfaces.
CONTROLLER_INTERFACE_PUBLIC void wait_for_trigger_update_to_finish()
Definition: controller_interface_base.cpp:229
virtual CONTROLLER_INTERFACE_PUBLIC CallbackReturn on_init()=0
Extending interface with initialization method which is individual for each controller.
virtual CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode)=0
virtual CONTROLLER_INTERFACE_PUBLIC std::vector< hardware_interface::CommandInterface::SharedPtr > export_reference_interfaces()=0
virtual CONTROLLER_INTERFACE_PUBLIC InterfaceConfiguration command_interface_configuration() const =0
Get configuration for controller's required command interfaces.
virtual CONTROLLER_INTERFACE_PUBLIC void release_interfaces()
Method that releases the Loaned interfaces from the controller.
Definition: controller_interface_base.cpp:153
virtual CONTROLLER_INTERFACE_PUBLIC bool is_in_chained_mode() const =0
Get information if a controller is currently in chained mode.
CONTROLLER_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure()
Custom configure method to read additional parameters for controller-nodes.
Definition: controller_interface_base.cpp:94
CONTROLLER_INTERFACE_PUBLIC ControllerUpdateStatus trigger_update(const rclcpp::Time &time, const rclcpp::Duration &period)
Definition: controller_interface_base.cpp:164
virtual CONTROLLER_INTERFACE_PUBLIC void assign_interfaces(std::vector< hardware_interface::LoanedCommandInterface > &&command_interfaces, std::vector< hardware_interface::LoanedStateInterface > &&state_interfaces)
Method that assigns the Loaned interfaces to the controller.
Definition: controller_interface_base.cpp:145
virtual CONTROLLER_INTERFACE_PUBLIC bool is_chainable() const =0
Get information if a controller is chainable.
virtual CONTROLLER_INTERFACE_PUBLIC return_type update(const rclcpp::Time &time, const rclcpp::Duration &period)=0
virtual CONTROLLER_INTERFACE_PUBLIC rclcpp::NodeOptions define_custom_node_options() const
Definition: controller_interface_base.hpp:233
Definition: controller_interface_base.hpp:64
Definition: controller_interface_base.hpp:86
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58