ros2_control - rolling
r6bot_controller.hpp
1 // Copyright 2023 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 ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_
16 #define ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_
17 
18 #include <chrono>
19 #include <memory>
20 #include <mutex>
21 #include <string>
22 #include <unordered_map>
23 #include <utility>
24 #include <vector>
25 
26 #include "control_msgs/action/follow_joint_trajectory.hpp"
27 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
28 #include "controller_interface/controller_interface.hpp"
29 #include "hardware_interface/types/hardware_interface_type_values.hpp"
30 #include "rclcpp/duration.hpp"
31 #include "rclcpp/subscription.hpp"
32 #include "rclcpp/time.hpp"
33 #include "rclcpp/timer.hpp"
34 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
35 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
36 #include "realtime_tools/realtime_buffer.hpp"
37 #include "trajectory_msgs/msg/joint_trajectory.hpp"
38 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
39 
40 namespace ros2_control_demo_example_7
41 {
43 {
44 public:
45  CONTROLLER_INTERFACE_PUBLIC
47 
48  CONTROLLER_INTERFACE_PUBLIC
50 
51  CONTROLLER_INTERFACE_PUBLIC
53 
54  CONTROLLER_INTERFACE_PUBLIC
55  controller_interface::return_type update(
56  const rclcpp::Time & time, const rclcpp::Duration & period) override;
57 
58  CONTROLLER_INTERFACE_PUBLIC
59  controller_interface::CallbackReturn on_init() override;
60 
61  CONTROLLER_INTERFACE_PUBLIC
62  controller_interface::CallbackReturn on_configure(
63  const rclcpp_lifecycle::State & previous_state) override;
64 
65  CONTROLLER_INTERFACE_PUBLIC
66  controller_interface::CallbackReturn on_activate(
67  const rclcpp_lifecycle::State & previous_state) override;
68 
69  CONTROLLER_INTERFACE_PUBLIC
70  controller_interface::CallbackReturn on_deactivate(
71  const rclcpp_lifecycle::State & previous_state) override;
72 
73  CONTROLLER_INTERFACE_PUBLIC
74  controller_interface::CallbackReturn on_cleanup(
75  const rclcpp_lifecycle::State & previous_state) override;
76 
77  CONTROLLER_INTERFACE_PUBLIC
78  controller_interface::CallbackReturn on_error(
79  const rclcpp_lifecycle::State & previous_state) override;
80 
81  CONTROLLER_INTERFACE_PUBLIC
82  controller_interface::CallbackReturn on_shutdown(
83  const rclcpp_lifecycle::State & previous_state) override;
84 
85 protected:
86  std::vector<std::string> joint_names_;
87  std::vector<std::string> command_interface_types_;
88  std::vector<std::string> state_interface_types_;
89 
90  rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_;
92  traj_msg_external_point_ptr_;
93  bool new_msg_ = false;
94  rclcpp::Time start_time_;
95  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
96  trajectory_msgs::msg::JointTrajectoryPoint point_interp_;
97 
98  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
99  joint_position_command_interface_;
100  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
101  joint_velocity_command_interface_;
102  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
103  joint_position_state_interface_;
104  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
105  joint_velocity_state_interface_;
106 
107  std::unordered_map<
108  std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
109  command_interface_map_ = {
110  {"position", &joint_position_command_interface_},
111  {"velocity", &joint_velocity_command_interface_}};
112 
113  std::unordered_map<
114  std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> *>
115  state_interface_map_ = {
116  {"position", &joint_position_state_interface_},
117  {"velocity", &joint_velocity_state_interface_}};
118 };
119 
120 } // namespace ros2_control_demo_example_7
121 
122 #endif // ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_
Definition: controller_interface.hpp:28
Definition: realtime_buffer.hpp:44
Definition: r6bot_controller.hpp:43
CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: r6bot_controller.cpp:151
CONTROLLER_INTERFACE_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: r6bot_controller.cpp:66
CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: r6bot_controller.cpp:34
CONTROLLER_INTERFACE_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: r6bot_controller.cpp:49
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58