ros2_control - rolling
diff_drive_controller.hpp
1 // Copyright 2020 PAL Robotics S.L.
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 /*
16  * Author: Bence Magyar, Enrique Fernández, Manuel Meraz
17  */
18 
19 #ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
20 #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
21 
22 #include <chrono>
23 #include <memory>
24 #include <queue>
25 #include <string>
26 #include <vector>
27 
28 #include "controller_interface/controller_interface.hpp"
29 #include "diff_drive_controller/odometry.hpp"
30 #include "diff_drive_controller/speed_limiter.hpp"
31 #include "diff_drive_controller/visibility_control.h"
32 #include "geometry_msgs/msg/twist_stamped.hpp"
33 #include "nav_msgs/msg/odometry.hpp"
34 #include "odometry.hpp"
35 #include "rclcpp_lifecycle/state.hpp"
36 #include "realtime_tools/realtime_box.hpp"
37 #include "realtime_tools/realtime_publisher.hpp"
38 #include "tf2_msgs/msg/tf_message.hpp"
39 
40 // auto-generated by generate_parameter_library
41 #include "diff_drive_controller_parameters.hpp"
42 
43 namespace diff_drive_controller
44 {
46 {
47  using TwistStamped = geometry_msgs::msg::TwistStamped;
48 
49 public:
50  DIFF_DRIVE_CONTROLLER_PUBLIC
52 
53  DIFF_DRIVE_CONTROLLER_PUBLIC
55 
56  DIFF_DRIVE_CONTROLLER_PUBLIC
58 
59  DIFF_DRIVE_CONTROLLER_PUBLIC
60  controller_interface::return_type update(
61  const rclcpp::Time & time, const rclcpp::Duration & period) override;
62 
63  DIFF_DRIVE_CONTROLLER_PUBLIC
64  controller_interface::CallbackReturn on_init() override;
65 
66  DIFF_DRIVE_CONTROLLER_PUBLIC
67  controller_interface::CallbackReturn on_configure(
68  const rclcpp_lifecycle::State & previous_state) override;
69 
70  DIFF_DRIVE_CONTROLLER_PUBLIC
71  controller_interface::CallbackReturn on_activate(
72  const rclcpp_lifecycle::State & previous_state) override;
73 
74  DIFF_DRIVE_CONTROLLER_PUBLIC
75  controller_interface::CallbackReturn on_deactivate(
76  const rclcpp_lifecycle::State & previous_state) override;
77 
78  DIFF_DRIVE_CONTROLLER_PUBLIC
79  controller_interface::CallbackReturn on_cleanup(
80  const rclcpp_lifecycle::State & previous_state) override;
81 
82  DIFF_DRIVE_CONTROLLER_PUBLIC
83  controller_interface::CallbackReturn on_error(
84  const rclcpp_lifecycle::State & previous_state) override;
85 
86  DIFF_DRIVE_CONTROLLER_PUBLIC
87  controller_interface::CallbackReturn on_shutdown(
88  const rclcpp_lifecycle::State & previous_state) override;
89 
90 protected:
91  struct WheelHandle
92  {
93  std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
94  std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
95  };
96 
97  const char * feedback_type() const;
98  controller_interface::CallbackReturn configure_side(
99  const std::string & side, const std::vector<std::string> & wheel_names,
100  std::vector<WheelHandle> & registered_handles);
101 
102  std::vector<WheelHandle> registered_left_wheel_handles_;
103  std::vector<WheelHandle> registered_right_wheel_handles_;
104 
105  // Parameters from ROS for diff_drive_controller
106  std::shared_ptr<ParamListener> param_listener_;
107  Params params_;
108  /* Number of wheels on each side of the robot. This is important to take the wheels slip into
109  * account when multiple wheels on each side are present. If there are more wheels then control
110  * signals for each side, you should enter number for control signals. For example, Husky has two
111  * wheels on each side, but they use one control signal, in this case '1' is the correct value of
112  * the parameter. */
113  int wheels_per_side_;
114 
115  Odometry odometry_;
116 
117  // Timeout to consider cmd_vel commands old
118  std::chrono::milliseconds cmd_vel_timeout_{500};
119 
120  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
121  std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
122  realtime_odometry_publisher_ = nullptr;
123 
124  std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
125  nullptr;
126  std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
127  realtime_odometry_transform_publisher_ = nullptr;
128 
129  bool subscriber_is_active_ = false;
130  rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
131 
132  realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
133  std::shared_ptr<TwistStamped> last_command_msg_;
134 
135  std::queue<TwistStamped> previous_commands_; // last two commands
136 
137  // speed limiters
138  SpeedLimiter limiter_linear_;
139  SpeedLimiter limiter_angular_;
140 
141  bool publish_limited_velocity_ = false;
142  std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
143  std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
144  realtime_limited_velocity_publisher_ = nullptr;
145 
146  rclcpp::Time previous_update_timestamp_{0};
147 
148  // publish rate limiter
149  double publish_rate_ = 50.0;
150  rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
151  rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
152 
153  bool is_halted = false;
154 
155  bool reset();
156  void halt();
157 };
158 } // namespace diff_drive_controller
159 #endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
Definition: controller_interface.hpp:28
Definition: diff_drive_controller.hpp:46
DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: diff_drive_controller.cpp:107
DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: diff_drive_controller.cpp:93
DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: diff_drive_controller.cpp:62
DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: diff_drive_controller.cpp:79
Definition: odometry.hpp:36
Definition: realtime_box.hpp:60
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58
Definition: diff_drive_controller.hpp:92