19 #ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_
20 #define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_
30 #include "ackermann_msgs/msg/ackermann_drive.hpp"
31 #include "controller_interface/controller_interface.hpp"
32 #include "geometry_msgs/msg/twist.hpp"
33 #include "geometry_msgs/msg/twist_stamped.hpp"
34 #include "nav_msgs/msg/odometry.hpp"
35 #include "rclcpp_lifecycle/state.hpp"
36 #include "realtime_tools/realtime_box.hpp"
37 #include "realtime_tools/realtime_publisher.hpp"
38 #include "std_srvs/srv/empty.hpp"
39 #include "tf2_msgs/msg/tf_message.hpp"
40 #include "tricycle_controller/odometry.hpp"
41 #include "tricycle_controller/steering_limiter.hpp"
42 #include "tricycle_controller/traction_limiter.hpp"
43 #include "tricycle_controller/visibility_control.h"
46 #include "tricycle_controller_parameters.hpp"
48 namespace tricycle_controller
50 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
54 using Twist = geometry_msgs::msg::Twist;
55 using TwistStamped = geometry_msgs::msg::TwistStamped;
56 using AckermannDrive = ackermann_msgs::msg::AckermannDrive;
59 TRICYCLE_CONTROLLER_PUBLIC
62 TRICYCLE_CONTROLLER_PUBLIC
65 TRICYCLE_CONTROLLER_PUBLIC
68 TRICYCLE_CONTROLLER_PUBLIC
69 controller_interface::return_type
update(
70 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
72 TRICYCLE_CONTROLLER_PUBLIC
73 CallbackReturn
on_init()
override;
75 TRICYCLE_CONTROLLER_PUBLIC
76 CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state)
override;
78 TRICYCLE_CONTROLLER_PUBLIC
79 CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state)
override;
81 TRICYCLE_CONTROLLER_PUBLIC
82 CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state)
override;
84 TRICYCLE_CONTROLLER_PUBLIC
85 CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state)
override;
87 TRICYCLE_CONTROLLER_PUBLIC
88 CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state)
override;
90 TRICYCLE_CONTROLLER_PUBLIC
91 CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state)
override;
96 std::reference_wrapper<const hardware_interface::LoanedStateInterface> velocity_state;
97 std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity_command;
101 std::reference_wrapper<const hardware_interface::LoanedStateInterface> position_state;
102 std::reference_wrapper<hardware_interface::LoanedCommandInterface> position_command;
105 CallbackReturn get_traction(
106 const std::string & traction_joint_name, std::vector<TractionHandle> & joint);
107 CallbackReturn get_steering(
108 const std::string & steering_joint_name, std::vector<SteeringHandle> & joint);
109 double convert_trans_rot_vel_to_steering_angle(
double v,
double omega,
double wheelbase);
110 std::tuple<double, double> twist_to_ackermann(
double linear_command,
double angular_command);
113 std::shared_ptr<ParamListener> param_listener_;
117 std::vector<TractionHandle> traction_joint_;
118 std::vector<SteeringHandle> steering_joint_;
120 std::shared_ptr<rclcpp::Publisher<AckermannDrive>> ackermann_command_publisher_ =
nullptr;
121 std::shared_ptr<realtime_tools::RealtimePublisher<AckermannDrive>>
122 realtime_ackermann_command_publisher_ =
nullptr;
126 std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ =
nullptr;
127 std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
128 realtime_odometry_publisher_ =
nullptr;
130 std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
132 std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
133 realtime_odometry_transform_publisher_ =
nullptr;
136 std::chrono::milliseconds cmd_vel_timeout_{500};
138 bool subscriber_is_active_ =
false;
139 rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ =
nullptr;
142 std::shared_ptr<TwistStamped> last_command_msg_;
144 rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;
146 std::queue<AckermannDrive> previous_commands_;
149 TractionLimiter limiter_traction_;
150 SteeringLimiter limiter_steering_;
152 bool is_halted =
false;
155 const std::shared_ptr<rmw_request_id_t> request_header,
156 const std::shared_ptr<std_srvs::srv::Empty::Request> req,
157 std::shared_ptr<std_srvs::srv::Empty::Response> res);
Definition: controller_interface.hpp:28
Definition: odometry.hpp:35
Definition: tricycle_controller.hpp:53
TRICYCLE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: tricycle_controller.cpp:77
TRICYCLE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: tricycle_controller.cpp:68
TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: tricycle_controller.cpp:51
TRICYCLE_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: tricycle_controller.cpp:86
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58
Definition: tricycle_controller.hpp:100
Definition: tricycle_controller.hpp:95