15 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
16 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
21 #include <unordered_map>
25 #include "hardware_interface/component_parser.hpp"
26 #include "hardware_interface/handle.hpp"
27 #include "hardware_interface/hardware_info.hpp"
28 #include "hardware_interface/types/hardware_interface_return_values.hpp"
29 #include "hardware_interface/types/lifecycle_state_names.hpp"
30 #include "lifecycle_msgs/msg/state.hpp"
31 #include "rclcpp/duration.hpp"
32 #include "rclcpp/logger.hpp"
33 #include "rclcpp/node_interfaces/node_clock_interface.hpp"
34 #include "rclcpp/time.hpp"
35 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
36 #include "rclcpp_lifecycle/state.hpp"
37 #include "realtime_tools/async_function_handler.hpp"
76 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
82 : lifecycle_state_(rclcpp_lifecycle::State(
83 lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)),
84 actuator_logger_(rclcpp::get_logger(
"actuator_interface"))
109 const HardwareInfo & hardware_info, rclcpp::Logger logger,
110 rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
112 clock_interface_ = clock_interface;
113 actuator_logger_ = logger.get_child(
"hardware_component.actuator." + hardware_info.
name);
114 info_ = hardware_info;
119 async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
120 async_handler_->init(
121 [
this](
const rclcpp::Time & time,
const rclcpp::Duration & period)
123 if (next_trigger_ == TriggerType::READ)
125 const auto ret =
read(time, period);
126 next_trigger_ = TriggerType::WRITE;
131 const auto ret =
write(time, period);
132 next_trigger_ = TriggerType::READ;
137 async_handler_->start_thread();
150 info_ = hardware_info;
153 return CallbackReturn::SUCCESS;
169 "Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
172 "by the Framework.")]]
virtual std::vector<StateInterface>
187 virtual std::vector<hardware_interface::InterfaceDescription>
204 std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
207 std::vector<StateInterface::ConstSharedPtr> state_interfaces;
208 state_interfaces.reserve(
209 unlisted_interface_descriptions.size() + joint_state_interfaces_.size());
213 for (
const auto & description : unlisted_interface_descriptions)
215 auto name = description.get_name();
216 unlisted_state_interfaces_.insert(std::make_pair(name, description));
217 auto state_interface = std::make_shared<StateInterface>(description);
218 actuator_states_.insert(std::make_pair(name, state_interface));
219 unlisted_states_.push_back(state_interface);
220 state_interfaces.push_back(std::const_pointer_cast<const StateInterface>(state_interface));
223 for (
const auto & [name, descr] : joint_state_interfaces_)
225 auto state_interface = std::make_shared<StateInterface>(descr);
226 actuator_states_.insert(std::make_pair(name, state_interface));
227 joint_states_.push_back(state_interface);
228 state_interfaces.push_back(std::const_pointer_cast<const StateInterface>(state_interface));
230 return state_interfaces;
246 "Replaced by vector<CommandInterface::SharedPtr> on_export_command_interfaces() method. "
249 "by the Framework.")]]
virtual std::vector<CommandInterface>
264 virtual std::vector<hardware_interface::InterfaceDescription>
281 std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
284 std::vector<CommandInterface::SharedPtr> command_interfaces;
285 command_interfaces.reserve(
286 unlisted_interface_descriptions.size() + joint_command_interfaces_.size());
290 for (
const auto & description : unlisted_interface_descriptions)
292 auto name = description.get_name();
293 unlisted_command_interfaces_.insert(std::make_pair(name, description));
294 auto command_interface = std::make_shared<CommandInterface>(description);
295 actuator_commands_.insert(std::make_pair(name, command_interface));
296 unlisted_commands_.push_back(command_interface);
297 command_interfaces.push_back(command_interface);
300 for (
const auto & [name, descr] : joint_command_interfaces_)
302 auto command_interface = std::make_shared<CommandInterface>(descr);
303 actuator_commands_.insert(std::make_pair(name, command_interface));
304 joint_commands_.push_back(command_interface);
305 command_interfaces.push_back(command_interface);
308 return command_interfaces;
324 const std::vector<std::string> & ,
325 const std::vector<std::string> & )
327 return return_type::OK;
343 const std::vector<std::string> & ,
344 const std::vector<std::string> & )
346 return return_type::OK;
360 return_type
trigger_read(
const rclcpp::Time & time,
const rclcpp::Duration & period)
362 return_type result = return_type::ERROR;
365 bool trigger_status =
true;
366 if (next_trigger_ == TriggerType::WRITE)
370 "Trigger read called while write async handler call is still pending for hardware "
371 "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
373 return return_type::OK;
375 std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
380 "Trigger read called while write async trigger is still in progress for hardware "
381 "interface : '%s'. Failed to trigger read cycle!",
383 return return_type::OK;
388 result =
read(time, period);
403 virtual return_type
read(
const rclcpp::Time & time,
const rclcpp::Duration & period) = 0;
415 return_type
trigger_write(
const rclcpp::Time & time,
const rclcpp::Duration & period)
417 return_type result = return_type::ERROR;
420 bool trigger_status =
true;
421 if (next_trigger_ == TriggerType::READ)
425 "Trigger write called while read async handler call is still pending for hardware "
426 "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
428 return return_type::OK;
430 std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
435 "Trigger write called while read async trigger is still in progress for hardware "
436 "interface : '%s'. Failed to trigger write cycle!",
438 return return_type::OK;
443 result =
write(time, period);
457 virtual return_type
write(
const rclcpp::Time & time,
const rclcpp::Duration & period) = 0;
483 lifecycle_state_ = new_state;
486 void set_state(
const std::string & interface_name,
const double & value)
488 actuator_states_.at(interface_name)->set_value(value);
491 double get_state(
const std::string & interface_name)
const
493 return actuator_states_.at(interface_name)->get_value();
496 void set_command(
const std::string & interface_name,
const double & value)
498 actuator_commands_.at(interface_name)->set_value(value);
501 double get_command(
const std::string & interface_name)
const
503 return actuator_commands_.at(interface_name)->get_value();
510 rclcpp::Logger
get_logger()
const {
return actuator_logger_; }
516 rclcpp::Clock::SharedPtr
get_clock()
const {
return clock_interface_->get_clock(); }
527 std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
528 std::unordered_map<std::string, InterfaceDescription> joint_command_interfaces_;
530 std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
531 std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;
534 std::vector<StateInterface::SharedPtr> joint_states_;
535 std::vector<CommandInterface::SharedPtr> joint_commands_;
537 std::vector<StateInterface::SharedPtr> unlisted_states_;
538 std::vector<CommandInterface::SharedPtr> unlisted_commands_;
540 rclcpp_lifecycle::State lifecycle_state_;
541 std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
544 rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
545 rclcpp::Logger actuator_logger_;
547 std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
548 std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
549 enum class TriggerType
554 std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
Definition: actuator_interface.hpp:79
virtual return_type read(const rclcpp::Time &time, const rclcpp::Duration &period)=0
Read the current state values from the actuator.
virtual CallbackReturn on_init(const HardwareInfo &hardware_info)
Initialization of the hardware interface from data parsed from the robot's URDF.
Definition: actuator_interface.hpp:148
virtual std::vector< StateInterface::ConstSharedPtr > on_export_state_interfaces()
Definition: actuator_interface.hpp:201
CallbackReturn init(const HardwareInfo &hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
Definition: actuator_interface.hpp:108
virtual std::string get_group_name() const
Get name of the actuator hardware group to which it belongs to.
Definition: actuator_interface.hpp:469
virtual std::vector< CommandInterface > export_command_interfaces()
Exports all command interfaces for this hardware interface.
Definition: actuator_interface.hpp:250
virtual std::string get_name() const
Get name of the actuator hardware.
Definition: actuator_interface.hpp:463
return_type trigger_write(const rclcpp::Time &time, const rclcpp::Duration &period)
Triggers the write method synchronously or asynchronously depending on the HardwareInfo.
Definition: actuator_interface.hpp:415
virtual return_type write(const rclcpp::Time &time, const rclcpp::Duration &period)=0
Write the current command values to the actuator.
rclcpp::Logger get_logger() const
Get the logger of the ActuatorInterface.
Definition: actuator_interface.hpp:510
void set_lifecycle_state(const rclcpp_lifecycle::State &new_state)
Set life-cycle state of the actuator hardware.
Definition: actuator_interface.hpp:481
virtual std::vector< CommandInterface::SharedPtr > on_export_command_interfaces()
Definition: actuator_interface.hpp:278
virtual std::vector< hardware_interface::InterfaceDescription > export_unlisted_command_interface_descriptions()
Definition: actuator_interface.hpp:265
ActuatorInterface(const ActuatorInterface &other)=delete
ActuatorInterface copy constructor is actively deleted.
virtual std::vector< hardware_interface::InterfaceDescription > export_unlisted_state_interface_descriptions()
Definition: actuator_interface.hpp:188
virtual return_type prepare_command_mode_switch(const std::vector< std::string > &, const std::vector< std::string > &)
Prepare for a new command interface switch.
Definition: actuator_interface.hpp:323
const rclcpp_lifecycle::State & get_lifecycle_state() const
Get life-cycle state of the actuator hardware.
Definition: actuator_interface.hpp:475
return_type trigger_read(const rclcpp::Time &time, const rclcpp::Duration &period)
Triggers the read method synchronously or asynchronously depending on the HardwareInfo.
Definition: actuator_interface.hpp:360
virtual return_type perform_command_mode_switch(const std::vector< std::string > &, const std::vector< std::string > &)
Definition: actuator_interface.hpp:342
rclcpp::Clock::SharedPtr get_clock() const
Get the clock of the ActuatorInterface.
Definition: actuator_interface.hpp:516
const HardwareInfo & get_hardware_info() const
Get the hardware info of the ActuatorInterface.
Definition: actuator_interface.hpp:522
virtual std::vector< StateInterface > export_state_interfaces()
Exports all state interfaces for this hardware interface.
Definition: actuator_interface.hpp:173
Definition: actuator.hpp:34
HARDWARE_INTERFACE_PUBLIC std::vector< InterfaceDescription > parse_command_interface_descriptions(const std::vector< ComponentInfo > &component_info)
Definition: component_parser.cpp:1031
HARDWARE_INTERFACE_PUBLIC std::vector< InterfaceDescription > parse_state_interface_descriptions(const std::vector< ComponentInfo > &component_info)
Definition: component_parser.cpp:998
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CallbackReturn
Virtual Class to implement when integrating a 1 DoF actuator into ros2_control.
Definition: actuator_interface.hpp:76
This structure stores information about hardware defined in a robot's URDF.
Definition: hardware_info.hpp:170
int thread_priority
Async thread priority.
Definition: hardware_info.hpp:182
std::string group
Hardware group to which the hardware belongs.
Definition: hardware_info.hpp:176
bool is_async
Component is async.
Definition: hardware_info.hpp:180
std::vector< ComponentInfo > joints
Definition: hardware_info.hpp:191
std::string name
Name of the hardware.
Definition: hardware_info.hpp:172