ros2_control - rolling
gpio_command_controller.hpp
1 // Copyright 2022 ICUBE Laboratory, University of Strasbourg
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 GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_
16 #define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_
17 
18 #include <memory>
19 #include <string>
20 #include <unordered_map>
21 #include <vector>
22 
23 #include "control_msgs/msg/dynamic_interface_group_values.hpp"
24 #include "controller_interface/controller_interface.hpp"
25 #include "gpio_command_controller_parameters.hpp"
26 #include "gpio_controllers/visibility_control.h"
27 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
28 #include "rclcpp_lifecycle/state.hpp"
29 #include "realtime_tools/realtime_buffer.hpp"
30 #include "realtime_tools/realtime_publisher.hpp"
31 
32 namespace gpio_controllers
33 {
34 using CmdType = control_msgs::msg::DynamicInterfaceGroupValues;
35 using StateType = control_msgs::msg::DynamicInterfaceGroupValues;
36 using CallbackReturn = controller_interface::CallbackReturn;
37 using InterfacesNames = std::vector<std::string>;
38 using MapOfReferencesToCommandInterfaces = std::unordered_map<
39  std::string, std::reference_wrapper<hardware_interface::LoanedCommandInterface>>;
40 using MapOfReferencesToStateInterfaces =
41  std::unordered_map<std::string, std::reference_wrapper<hardware_interface::LoanedStateInterface>>;
42 using StateInterfaces =
43  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>;
44 
46 {
47 public:
48  GPIO_COMMAND_CONTROLLER_PUBLIC
50 
51  GPIO_COMMAND_CONTROLLER_PUBLIC
53 
54  GPIO_COMMAND_CONTROLLER_PUBLIC
56 
57  GPIO_COMMAND_CONTROLLER_PUBLIC
58  CallbackReturn on_init() override;
59 
60  GPIO_COMMAND_CONTROLLER_PUBLIC
61  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
62 
63  GPIO_COMMAND_CONTROLLER_PUBLIC
64  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
65 
66  GPIO_COMMAND_CONTROLLER_PUBLIC
67  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
68 
69  GPIO_COMMAND_CONTROLLER_PUBLIC
70  controller_interface::return_type update(
71  const rclcpp::Time & time, const rclcpp::Duration & period) override;
72 
73 private:
74  void store_command_interface_types();
75  void store_state_interface_types();
76  void initialize_gpio_state_msg();
77  void update_gpios_states();
78  controller_interface::return_type update_gpios_commands();
79  template <typename T>
80  std::unordered_map<std::string, std::reference_wrapper<T>> create_map_of_references_to_interfaces(
81  const InterfacesNames & interfaces_from_params, std::vector<T> & configured_interfaces);
82  template <typename T>
83  bool check_if_configured_interfaces_matches_received(
84  const InterfacesNames & interfaces_from_params, const T & configured_interfaces);
85  void apply_state_value(
86  StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const;
87  void apply_command(
88  const CmdType & gpio_commands, std::size_t gpio_index,
89  std::size_t command_interface_index) const;
90  bool should_broadcast_all_interfaces_of_configured_gpios() const;
91  void set_all_state_interfaces_of_configured_gpios();
92  InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const;
93  bool update_dynamic_map_parameters();
94  std::vector<hardware_interface::ComponentInfo> get_gpios_from_urdf() const;
95 
96 protected:
97  InterfacesNames command_interface_types_;
98  InterfacesNames state_interface_types_;
99  MapOfReferencesToCommandInterfaces command_interfaces_map_;
100  MapOfReferencesToStateInterfaces state_interfaces_map_;
101 
103  rclcpp::Subscription<CmdType>::SharedPtr gpios_command_subscriber_{};
104 
105  std::shared_ptr<rclcpp::Publisher<StateType>> gpio_state_publisher_{};
106  std::shared_ptr<realtime_tools::RealtimePublisher<StateType>> realtime_gpio_state_publisher_{};
107 
108  std::shared_ptr<gpio_command_controller_parameters::ParamListener> param_listener_{};
109  gpio_command_controller_parameters::Params params_;
110 };
111 
112 } // namespace gpio_controllers
113 
114 #endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_
Definition: controller_interface.hpp:28
Definition: gpio_command_controller.hpp:46
GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: gpio_command_controller.cpp:151
GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: gpio_command_controller.cpp:106
GPIO_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: gpio_command_controller.cpp:54
GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: gpio_command_controller.cpp:115
Definition: realtime_buffer.hpp:44
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58