18 #ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_
19 #define PID_CONTROLLER__PID_CONTROLLER_HPP_
25 #include "control_msgs/msg/multi_dof_command.hpp"
26 #include "control_msgs/msg/multi_dof_state_stamped.hpp"
27 #include "control_toolbox/pid_ros.hpp"
28 #include "controller_interface/chainable_controller_interface.hpp"
29 #include "pid_controller/visibility_control.h"
30 #include "pid_controller_parameters.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"
36 namespace pid_controller
39 enum class feedforward_mode_type : std::uint8_t
48 PID_CONTROLLER__VISIBILITY_PUBLIC
51 PID_CONTROLLER__VISIBILITY_PUBLIC
52 controller_interface::CallbackReturn
on_init()
override;
54 PID_CONTROLLER__VISIBILITY_PUBLIC
57 PID_CONTROLLER__VISIBILITY_PUBLIC
60 PID_CONTROLLER__VISIBILITY_PUBLIC
61 controller_interface::CallbackReturn on_cleanup(
62 const rclcpp_lifecycle::State & previous_state)
override;
64 PID_CONTROLLER__VISIBILITY_PUBLIC
65 controller_interface::CallbackReturn on_configure(
66 const rclcpp_lifecycle::State & previous_state)
override;
68 PID_CONTROLLER__VISIBILITY_PUBLIC
69 controller_interface::CallbackReturn on_activate(
70 const rclcpp_lifecycle::State & previous_state)
override;
72 PID_CONTROLLER__VISIBILITY_PUBLIC
73 controller_interface::CallbackReturn on_deactivate(
74 const rclcpp_lifecycle::State & previous_state)
override;
76 PID_CONTROLLER__VISIBILITY_PUBLIC
78 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
80 PID_CONTROLLER__VISIBILITY_PUBLIC
82 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
84 using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand;
85 using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand;
86 using ControllerModeSrvType = std_srvs::srv::SetBool;
87 using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped;
90 std::shared_ptr<pid_controller::ParamListener> param_listener_;
91 pid_controller::Params params_;
93 std::vector<std::string> reference_and_state_dof_names_;
95 std::vector<double> measured_state_values_;
97 using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
98 std::vector<PidPtr> pids_;
100 std::vector<double> feedforward_gain_;
103 rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ =
nullptr;
106 rclcpp::Subscription<ControllerMeasuredStateMsg>::SharedPtr measured_state_subscriber_ =
nullptr;
109 rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
114 rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
115 std::unique_ptr<ControllerStatePublisher> state_publisher_;
125 void update_parameters();
126 controller_interface::CallbackReturn configure_parameters();
130 PID_CONTROLLER__VISIBILITY_LOCAL
131 void reference_callback(
const std::shared_ptr<ControllerReferenceMsg> msg);
Definition: chainable_controller_interface.hpp:38
Definition: pid_controller.hpp:46
PID_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: pid_controller.cpp:473
bool on_set_chained_mode(bool chained_mode) override
Virtual method that each chainable controller should implement to switch chained mode.
Definition: pid_controller.cpp:424
std::vector< hardware_interface::StateInterface > on_export_state_interfaces() override
Definition: pid_controller.cpp:403
PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: pid_controller.cpp:356
std::vector< hardware_interface::CommandInterface > on_export_reference_interfaces() override
Definition: pid_controller.cpp:381
PID_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: pid_controller.cpp:452
PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: pid_controller.cpp:75
PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: pid_controller.cpp:342
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58