ros2_control - rolling
mecanum_drive_controller.hpp
1 // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
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 MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
16 #define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
17 
18 #include <chrono>
19 #include <cmath>
20 #include <memory>
21 #include <queue>
22 #include <string>
23 #include <utility>
24 #include <vector>
25 
26 #include "controller_interface/chainable_controller_interface.hpp"
27 #include "mecanum_drive_controller/odometry.hpp"
28 #include "mecanum_drive_controller/visibility_control.h"
29 #include "mecanum_drive_controller_parameters.hpp"
30 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
31 #include "rclcpp_lifecycle/state.hpp"
32 #include "realtime_tools/realtime_buffer.hpp"
33 #include "realtime_tools/realtime_publisher.hpp"
34 #include "std_srvs/srv/set_bool.hpp"
35 
36 #include "control_msgs/msg/mecanum_drive_controller_state.hpp"
37 #include "geometry_msgs/msg/twist_stamped.hpp"
38 #include "nav_msgs/msg/odometry.hpp"
39 #include "tf2_msgs/msg/tf_message.hpp"
40 namespace mecanum_drive_controller
41 {
42 // name constants for state interfaces
43 static constexpr size_t NR_STATE_ITFS = 4;
44 
45 // name constants for command interfaces
46 static constexpr size_t NR_CMD_ITFS = 4;
47 
48 // name constants for reference interfaces
49 static constexpr size_t NR_REF_ITFS = 3;
50 
52 {
53 public:
54  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
56 
57  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
58  controller_interface::CallbackReturn on_init() override;
59 
60  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
62 
63  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
65 
66  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
67  controller_interface::CallbackReturn on_configure(
68  const rclcpp_lifecycle::State & previous_state) override;
69 
70  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
71  controller_interface::CallbackReturn on_activate(
72  const rclcpp_lifecycle::State & previous_state) override;
73 
74  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
75  controller_interface::CallbackReturn on_deactivate(
76  const rclcpp_lifecycle::State & previous_state) override;
77 
78  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
79  controller_interface::return_type update_reference_from_subscribers(
80  const rclcpp::Time & time, const rclcpp::Duration & period) override;
81 
82  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
83  controller_interface::return_type update_and_write_commands(
84  const rclcpp::Time & time, const rclcpp::Duration & period) override;
85 
86  using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped;
87  using OdomStateMsg = nav_msgs::msg::Odometry;
88  using TfStateMsg = tf2_msgs::msg::TFMessage;
89  using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState;
90 
91 protected:
92  std::shared_ptr<mecanum_drive_controller::ParamListener> param_listener_;
93  mecanum_drive_controller::Params params_;
94 
102  enum WheelIndex : std::size_t
103  {
104  FRONT_LEFT = 0,
105  FRONT_RIGHT = 1,
106  REAR_RIGHT = 2,
107  REAR_LEFT = 3
108  };
109 
113  std::vector<std::string> command_joint_names_;
114 
120  std::vector<std::string> state_joint_names_;
121 
122  // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.
123  // used for preceding controller
124  std::vector<std::string> reference_names_;
125 
126  // Command subscribers and Controller State, odom state, tf state publishers
127  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
129  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
130 
132  rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_;
133  std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_;
134 
136  rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_;
137  std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_;
138 
140  rclcpp::Publisher<ControllerStateMsg>::SharedPtr controller_s_publisher_;
141  std::unique_ptr<ControllerStatePublisher> controller_state_publisher_;
142 
143  // override methods from ChainableControllerInterface
144  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
145 
146  bool on_set_chained_mode(bool chained_mode) override;
147 
148  Odometry odometry_;
149 
150 private:
151  // callback for topic interface
152  MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL
153  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
154 
155  double velocity_in_center_frame_linear_x_; // [m/s]
156  double velocity_in_center_frame_linear_y_; // [m/s]
157  double velocity_in_center_frame_angular_z_; // [rad/s]
158 };
159 
160 } // namespace mecanum_drive_controller
161 
162 #endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
Definition: chainable_controller_interface.hpp:38
Definition: mecanum_drive_controller.hpp:52
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: mecanum_drive_controller.cpp:57
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override
Execute calculations of the controller and update command interfaces.
Definition: mecanum_drive_controller.cpp:374
WheelIndex
Definition: mecanum_drive_controller.hpp:103
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time &time, const rclcpp::Duration &period) override
Update reference from input topics when not in chained mode.
Definition: mecanum_drive_controller.cpp:330
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: mecanum_drive_controller.cpp:257
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: mecanum_drive_controller.cpp:243
std::vector< std::string > command_joint_names_
Definition: mecanum_drive_controller.hpp:113
bool on_set_chained_mode(bool chained_mode) override
Virtual method that each chainable controller should implement to switch chained mode.
Definition: mecanum_drive_controller.cpp:294
std::vector< hardware_interface::CommandInterface > on_export_reference_interfaces() override
Definition: mecanum_drive_controller.cpp:274
std::vector< std::string > state_joint_names_
Definition: mecanum_drive_controller.hpp:120
The Odometry class handles odometry readings (2D pose and velocity with related timestamp)
Definition: odometry.hpp:29
Definition: realtime_buffer.hpp:44
Definition: realtime_publisher.hpp:54
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58