15 #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
16 #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
22 #include "controller_interface/visibility_control.h"
24 #include "hardware_interface/handle.hpp"
25 #include "hardware_interface/loaned_command_interface.hpp"
26 #include "hardware_interface/loaned_state_interface.hpp"
28 #include "rclcpp/version.h"
29 #include "rclcpp_lifecycle/lifecycle_node.hpp"
31 namespace controller_interface
33 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
35 enum class return_type : std::uint8_t
47 enum class interface_configuration_type : std::uint8_t
57 interface_configuration_type type;
58 std::vector<std::string> names = {};
69 CONTROLLER_INTERFACE_PUBLIC
72 CONTROLLER_INTERFACE_PUBLIC
87 CONTROLLER_INTERFACE_PUBLIC
104 CONTROLLER_INTERFACE_PUBLIC
116 CONTROLLER_INTERFACE_PUBLIC
118 std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
119 std::vector<hardware_interface::LoanedStateInterface> && state_interfaces);
125 CONTROLLER_INTERFACE_PUBLIC
128 CONTROLLER_INTERFACE_PUBLIC
130 const std::string & controller_name,
const std::string & urdf,
unsigned int cm_update_rate,
131 const std::string & node_namespace,
const rclcpp::NodeOptions & node_options);
137 CONTROLLER_INTERFACE_PUBLIC
138 const rclcpp_lifecycle::State &
configure();
141 CONTROLLER_INTERFACE_PUBLIC
153 CONTROLLER_INTERFACE_PUBLIC
154 virtual return_type
update(
const rclcpp::Time & time,
const rclcpp::Duration & period) = 0;
156 CONTROLLER_INTERFACE_PUBLIC
157 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
159 CONTROLLER_INTERFACE_PUBLIC
160 std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node()
const;
162 CONTROLLER_INTERFACE_PUBLIC
163 const rclcpp_lifecycle::State & get_lifecycle_state()
const;
165 CONTROLLER_INTERFACE_PUBLIC
166 unsigned int get_update_rate()
const;
168 CONTROLLER_INTERFACE_PUBLIC
169 bool is_async()
const;
171 CONTROLLER_INTERFACE_PUBLIC
172 const std::string & get_robot_description()
const;
185 CONTROLLER_INTERFACE_PUBLIC
189 #if RCLCPP_VERSION_MAJOR >= 21
190 return rclcpp::NodeOptions().enable_logger_service(
true);
192 return rclcpp::NodeOptions()
193 .allow_undeclared_parameters(
true)
194 .automatically_declare_parameters_from_overrides(
true);
205 template <
typename ParameterT>
206 auto auto_declare(
const std::string & name,
const ParameterT & default_value)
208 if (!node_->has_parameter(name))
210 return node_->declare_parameter<ParameterT>(name, default_value);
214 return node_->get_parameter(name).get_value<ParameterT>();
227 CONTROLLER_INTERFACE_PUBLIC
236 CONTROLLER_INTERFACE_PUBLIC
245 CONTROLLER_INTERFACE_PUBLIC
256 CONTROLLER_INTERFACE_PUBLIC
267 CONTROLLER_INTERFACE_PUBLIC
271 std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
272 std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
275 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
276 unsigned int update_rate_ = 0;
277 bool is_async_ =
false;
278 std::string urdf_ =
"";
281 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
Definition: controller_interface_base.hpp:67
auto auto_declare(const std::string &name, const ParameterT &default_value)
Declare and initialize a parameter with a type.
Definition: controller_interface_base.hpp:206
virtual CONTROLLER_INTERFACE_PUBLIC std::vector< hardware_interface::CommandInterface > export_reference_interfaces()=0
virtual CONTROLLER_INTERFACE_PUBLIC InterfaceConfiguration state_interface_configuration() const =0
Get configuration for controller's required state interfaces.
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::StateInterface > export_state_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:103
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:76
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:95
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:186
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:56