ros2_control - rolling
actuator_interface.hpp
1 // Copyright 2020 - 2021 ros2_control Development Team
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__ACTUATOR_INTERFACE_HPP_
16 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
17 
18 #include <limits>
19 #include <memory>
20 #include <string>
21 #include <unordered_map>
22 #include <utility>
23 #include <vector>
24 
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"
38 
39 namespace hardware_interface
40 {
42 
76 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
77 
78 class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
79 {
80 public:
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"))
85  {
86  }
87 
89 
93  ActuatorInterface(const ActuatorInterface & other) = delete;
94 
95  ActuatorInterface(ActuatorInterface && other) = default;
96 
97  virtual ~ActuatorInterface() = default;
98 
101 
109  const HardwareInfo & hardware_info, rclcpp::Logger logger,
110  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
111  {
112  clock_interface_ = clock_interface;
113  actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
114  info_ = hardware_info;
115  if (info_.is_async)
116  {
117  RCLCPP_INFO_STREAM(
118  get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
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)
122  {
123  if (next_trigger_ == TriggerType::READ)
124  {
125  const auto ret = read(time, period);
126  next_trigger_ = TriggerType::WRITE;
127  return ret;
128  }
129  else
130  {
131  const auto ret = write(time, period);
132  next_trigger_ = TriggerType::READ;
133  return ret;
134  }
135  },
136  info_.thread_priority);
137  async_handler_->start_thread();
138  }
139  return on_init(hardware_info);
140  };
141 
143 
148  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
149  {
150  info_ = hardware_info;
151  parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
152  parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
153  return CallbackReturn::SUCCESS;
154  };
155 
157 
168  [[deprecated(
169  "Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
170  "Exporting is "
171  "handled "
172  "by the Framework.")]] virtual std::vector<StateInterface>
174  {
175  // return empty vector by default. For backward compatibility we try calling
176  // export_state_interfaces() and only when empty vector is returned call
177  // on_export_state_interfaces()
178  return {};
179  }
180 
187  virtual std::vector<hardware_interface::InterfaceDescription>
189  {
190  // return empty vector by default.
191  return {};
192  }
193 
201  virtual std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces()
202  {
203  // import the unlisted interfaces
204  std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
206 
207  std::vector<StateInterface::ConstSharedPtr> state_interfaces;
208  state_interfaces.reserve(
209  unlisted_interface_descriptions.size() + joint_state_interfaces_.size());
210 
211  // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to
212  // maps.
213  for (const auto & description : unlisted_interface_descriptions)
214  {
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));
221  }
222 
223  for (const auto & [name, descr] : joint_state_interfaces_)
224  {
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));
229  }
230  return state_interfaces;
231  }
232 
234 
245  [[deprecated(
246  "Replaced by vector<CommandInterface::SharedPtr> on_export_command_interfaces() method. "
247  "Exporting is "
248  "handled "
249  "by the Framework.")]] virtual std::vector<CommandInterface>
251  {
252  // return empty vector by default. For backward compatibility we try calling
253  // export_command_interfaces() and only when empty vector is returned call
254  // on_export_command_interfaces()
255  return {};
256  }
257 
264  virtual std::vector<hardware_interface::InterfaceDescription>
266  {
267  // return empty vector by default.
268  return {};
269  }
270 
278  virtual std::vector<CommandInterface::SharedPtr> on_export_command_interfaces()
279  {
280  // import the unlisted interfaces
281  std::vector<hardware_interface::InterfaceDescription> unlisted_interface_descriptions =
283 
284  std::vector<CommandInterface::SharedPtr> command_interfaces;
285  command_interfaces.reserve(
286  unlisted_interface_descriptions.size() + joint_command_interfaces_.size());
287 
288  // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to
289  // maps.
290  for (const auto & description : unlisted_interface_descriptions)
291  {
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);
298  }
299 
300  for (const auto & [name, descr] : joint_command_interfaces_)
301  {
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);
306  }
307 
308  return command_interfaces;
309  }
311 
323  virtual return_type prepare_command_mode_switch(
324  const std::vector<std::string> & /*start_interfaces*/,
325  const std::vector<std::string> & /*stop_interfaces*/)
326  {
327  return return_type::OK;
328  }
329 
330  // Perform switching to the new command interface.
342  virtual return_type perform_command_mode_switch(
343  const std::vector<std::string> & /*start_interfaces*/,
344  const std::vector<std::string> & /*stop_interfaces*/)
345  {
346  return return_type::OK;
347  }
348 
350 
360  return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period)
361  {
362  return_type result = return_type::ERROR;
363  if (info_.is_async)
364  {
365  bool trigger_status = true;
366  if (next_trigger_ == TriggerType::WRITE)
367  {
368  RCLCPP_WARN(
369  get_logger(),
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!",
372  info_.name.c_str());
373  return return_type::OK;
374  }
375  std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
376  if (!trigger_status)
377  {
378  RCLCPP_WARN(
379  get_logger(),
380  "Trigger read called while write async trigger is still in progress for hardware "
381  "interface : '%s'. Failed to trigger read cycle!",
382  info_.name.c_str());
383  return return_type::OK;
384  }
385  }
386  else
387  {
388  result = read(time, period);
389  }
390  return result;
391  }
392 
394 
403  virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
404 
406 
415  return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period)
416  {
417  return_type result = return_type::ERROR;
418  if (info_.is_async)
419  {
420  bool trigger_status = true;
421  if (next_trigger_ == TriggerType::READ)
422  {
423  RCLCPP_WARN(
424  get_logger(),
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!",
427  info_.name.c_str());
428  return return_type::OK;
429  }
430  std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
431  if (!trigger_status)
432  {
433  RCLCPP_WARN(
434  get_logger(),
435  "Trigger write called while read async trigger is still in progress for hardware "
436  "interface : '%s'. Failed to trigger write cycle!",
437  info_.name.c_str());
438  return return_type::OK;
439  }
440  }
441  else
442  {
443  result = write(time, period);
444  }
445  return result;
446  }
447 
449 
457  virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
458 
460 
463  virtual std::string get_name() const { return info_.name; }
464 
466 
469  virtual std::string get_group_name() const { return info_.group; }
470 
472 
475  const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; }
476 
478 
481  void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)
482  {
483  lifecycle_state_ = new_state;
484  }
485 
486  void set_state(const std::string & interface_name, const double & value)
487  {
488  actuator_states_.at(interface_name)->set_value(value);
489  }
490 
491  double get_state(const std::string & interface_name) const
492  {
493  return actuator_states_.at(interface_name)->get_value();
494  }
495 
496  void set_command(const std::string & interface_name, const double & value)
497  {
498  actuator_commands_.at(interface_name)->set_value(value);
499  }
500 
501  double get_command(const std::string & interface_name) const
502  {
503  return actuator_commands_.at(interface_name)->get_value();
504  }
505 
507 
510  rclcpp::Logger get_logger() const { return actuator_logger_; }
511 
513 
516  rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); }
517 
519 
522  const HardwareInfo & get_hardware_info() const { return info_; }
523 
524 protected:
525  HardwareInfo info_;
526  // interface names to InterfaceDescription
527  std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
528  std::unordered_map<std::string, InterfaceDescription> joint_command_interfaces_;
529 
530  std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
531  std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;
532 
533  // Exported Command- and StateInterfaces in order they are listed in the hardware description.
534  std::vector<StateInterface::SharedPtr> joint_states_;
535  std::vector<CommandInterface::SharedPtr> joint_commands_;
536 
537  std::vector<StateInterface::SharedPtr> unlisted_states_;
538  std::vector<CommandInterface::SharedPtr> unlisted_commands_;
539 
540  rclcpp_lifecycle::State lifecycle_state_;
541  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
542 
543 private:
544  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
545  rclcpp::Logger actuator_logger_;
546  // interface names to Handle accessed through getters/setters
547  std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
548  std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
549  enum class TriggerType
550  {
551  READ,
552  WRITE
553  };
554  std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
555 };
556 
557 } // namespace hardware_interface
558 #endif // HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
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