19 #ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
20 #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
29 #include "controller_interface/controller_interface.hpp"
30 #include "diff_drive_controller/odometry.hpp"
31 #include "diff_drive_controller/speed_limiter.hpp"
32 #include "diff_drive_controller/visibility_control.h"
33 #include "geometry_msgs/msg/twist_stamped.hpp"
34 #include "nav_msgs/msg/odometry.hpp"
35 #include "odometry.hpp"
36 #include "rclcpp_lifecycle/state.hpp"
37 #include "realtime_tools/realtime_box.h"
38 #include "realtime_tools/realtime_publisher.h"
39 #include "tf2_msgs/msg/tf_message.hpp"
42 #include "diff_drive_controller_parameters.hpp"
44 namespace diff_drive_controller
48 using Twist = geometry_msgs::msg::TwistStamped;
51 DIFF_DRIVE_CONTROLLER_PUBLIC
54 DIFF_DRIVE_CONTROLLER_PUBLIC
57 DIFF_DRIVE_CONTROLLER_PUBLIC
60 DIFF_DRIVE_CONTROLLER_PUBLIC
61 controller_interface::return_type
update(
62 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
64 DIFF_DRIVE_CONTROLLER_PUBLIC
65 controller_interface::CallbackReturn
on_init()
override;
67 DIFF_DRIVE_CONTROLLER_PUBLIC
68 controller_interface::CallbackReturn on_configure(
69 const rclcpp_lifecycle::State & previous_state)
override;
71 DIFF_DRIVE_CONTROLLER_PUBLIC
72 controller_interface::CallbackReturn on_activate(
73 const rclcpp_lifecycle::State & previous_state)
override;
75 DIFF_DRIVE_CONTROLLER_PUBLIC
76 controller_interface::CallbackReturn on_deactivate(
77 const rclcpp_lifecycle::State & previous_state)
override;
79 DIFF_DRIVE_CONTROLLER_PUBLIC
80 controller_interface::CallbackReturn on_cleanup(
81 const rclcpp_lifecycle::State & previous_state)
override;
83 DIFF_DRIVE_CONTROLLER_PUBLIC
84 controller_interface::CallbackReturn on_error(
85 const rclcpp_lifecycle::State & previous_state)
override;
87 DIFF_DRIVE_CONTROLLER_PUBLIC
88 controller_interface::CallbackReturn on_shutdown(
89 const rclcpp_lifecycle::State & previous_state)
override;
94 std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
95 std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
98 const char * feedback_type()
const;
99 controller_interface::CallbackReturn configure_side(
100 const std::string & side,
const std::vector<std::string> & wheel_names,
101 std::vector<WheelHandle> & registered_handles);
103 std::vector<WheelHandle> registered_left_wheel_handles_;
104 std::vector<WheelHandle> registered_right_wheel_handles_;
107 std::shared_ptr<ParamListener> param_listener_;
114 int wheels_per_side_;
119 std::chrono::milliseconds cmd_vel_timeout_{500};
121 std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ =
nullptr;
122 std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
123 realtime_odometry_publisher_ =
nullptr;
125 std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
127 std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
128 realtime_odometry_transform_publisher_ =
nullptr;
130 bool subscriber_is_active_ =
false;
131 rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ =
nullptr;
135 std::queue<Twist> previous_commands_;
138 SpeedLimiter limiter_linear_;
139 SpeedLimiter limiter_angular_;
141 bool publish_limited_velocity_ =
false;
142 std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ =
nullptr;
143 std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ =
146 rclcpp::Time previous_update_timestamp_{0};
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};
153 bool is_halted =
false;
Definition: controller_interface.hpp:28
Definition: diff_drive_controller.hpp:47
DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: diff_drive_controller.cpp:100
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:86
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:55
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:72
Definition: odometry.hpp:38
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:56
Definition: diff_drive_controller.hpp:93