ros2_control - rolling
steering_controllers_library.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 STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_
16 #define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_
17 
18 #include <cmath>
19 #include <memory>
20 #include <string>
21 #include <vector>
22 
23 #include "controller_interface/chainable_controller_interface.hpp"
24 #include "hardware_interface/handle.hpp"
25 #include "rclcpp_lifecycle/state.hpp"
26 #include "realtime_tools/realtime_buffer.hpp"
27 #include "realtime_tools/realtime_publisher.hpp"
28 #include "steering_controllers_library/steering_odometry.hpp"
29 #include "steering_controllers_library/visibility_control.h"
30 #include "steering_controllers_library_parameters.hpp"
31 
32 // TODO(anyone): Replace with controller specific messages
33 #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
34 #include "control_msgs/msg/steering_controller_status.hpp"
35 #include "geometry_msgs/msg/twist_stamped.hpp"
36 #include "nav_msgs/msg/odometry.hpp"
37 #include "tf2_msgs/msg/tf_message.hpp"
38 
39 namespace steering_controllers_library
40 {
42 {
43 public:
44  STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary();
45 
46  virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void
47  initialize_implementation_parameter_listener() = 0;
48 
49  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override;
50 
51  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration
52  command_interface_configuration() const override;
53 
54  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration
55  state_interface_configuration() const override;
56 
57  virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn
58  configure_odometry() = 0;
59 
60  virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry(
61  const rclcpp::Duration & period) = 0;
62 
63  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure(
64  const rclcpp_lifecycle::State & previous_state) override;
65 
66  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate(
67  const rclcpp_lifecycle::State & previous_state) override;
68 
69  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate(
70  const rclcpp_lifecycle::State & previous_state) override;
71 
72  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type
74  const rclcpp::Time & time, const rclcpp::Duration & period) override;
75 
76  STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type
77  update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override;
78 
79  using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped;
80  using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped;
81  using ControllerStateMsgOdom = nav_msgs::msg::Odometry;
82  using ControllerStateMsgTf = tf2_msgs::msg::TFMessage;
83  using AckermanControllerState = control_msgs::msg::SteeringControllerStatus;
84 
85 protected:
86  controller_interface::CallbackReturn set_interface_numbers(
87  size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs);
88 
89  std::shared_ptr<steering_controllers_library::ParamListener> param_listener_;
90  steering_controllers_library::Params params_;
91 
92  // Command subscribers and Controller State publisher
93  rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
94  rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_ackermann_ = nullptr;
96  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms
97 
100 
101  rclcpp::Publisher<ControllerStateMsgOdom>::SharedPtr odom_s_publisher_;
102  rclcpp::Publisher<ControllerStateMsgTf>::SharedPtr tf_odom_s_publisher_;
103 
104  std::unique_ptr<ControllerStatePublisherOdom> rt_odom_state_publisher_;
105  std::unique_ptr<ControllerStatePublisherTf> rt_tf_odom_state_publisher_;
106 
107  // override methods from ChainableControllerInterface
108  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
109 
110  bool on_set_chained_mode(bool chained_mode) override;
111 
114 
115  AckermanControllerState published_state_;
116 
118  rclcpp::Publisher<AckermanControllerState>::SharedPtr controller_s_publisher_;
119  std::unique_ptr<ControllerStatePublisher> controller_state_publisher_;
120 
121  // name constants for state interfaces
122  size_t nr_state_itfs_;
123  // name constants for command interfaces
124  size_t nr_cmd_itfs_;
125  // name constants for reference interfaces
126  size_t nr_ref_itfs_;
127 
128  // last velocity commands for open loop odometry
129  double last_linear_velocity_ = 0.0;
130  double last_angular_velocity_ = 0.0;
131 
132  std::vector<std::string> rear_wheels_state_names_;
133  std::vector<std::string> front_wheels_state_names_;
134 
135 private:
136  // callback for topic interface
137  STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback(
138  const std::shared_ptr<ControllerTwistReferenceMsg> msg);
139 };
140 
141 } // namespace steering_controllers_library
142 
143 #endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_
Definition: chainable_controller_interface.hpp:38
Definition: realtime_buffer.hpp:44
Definition: realtime_publisher.hpp:54
Definition: steering_controllers_library.hpp:42
STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: steering_controllers_library.cpp:232
std::vector< hardware_interface::CommandInterface > on_export_reference_interfaces() override
Definition: steering_controllers_library.cpp:312
bool on_set_chained_mode(bool chained_mode) override
Virtual method that each chainable controller should implement to switch chained mode.
Definition: steering_controllers_library.cpp:330
STEERING_CONTROLLERS__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: steering_controllers_library.cpp:369
STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: steering_controllers_library.cpp:55
STEERING_CONTROLLERS__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: steering_controllers_library.cpp:355
steering_odometry::SteeringOdometry odometry_
Odometry:
Definition: steering_controllers_library.hpp:113
STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: steering_controllers_library.cpp:270
The Odometry class handles odometry readings (2D pose and velocity with related timestamp)
Definition: steering_odometry.hpp:47
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58