ros2_control - iron
Loading...
Searching...
No Matches
pid_ros.hpp
1// Copyright (c) 2020, Open Source Robotics Foundation, Inc.
2// All rights reserved.
3//
4// Software License Agreement (BSD License 2.0)
5//
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions
8// are met:
9//
10// * Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12// * Redistributions in binary form must reproduce the above
13// copyright notice, this list of conditions and the following
14// disclaimer in the documentation and/or other materials provided
15// with the distribution.
16// * Neither the name of the Willow Garage nor the names of its
17// contributors may be used to endorse or promote products derived
18// from this software without specific prior written permission.
19//
20// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31// POSSIBILITY OF SUCH DAMAGE.
32
33#ifndef CONTROL_TOOLBOX__PID_ROS_HPP_
34#define CONTROL_TOOLBOX__PID_ROS_HPP_
35
36#include <memory>
37#include <string>
38
39#include "control_msgs/msg/pid_state.hpp"
40
41#include "rclcpp/clock.hpp"
42#include "rclcpp/duration.hpp"
43#include "rclcpp/node.hpp"
44
45#include "realtime_tools/realtime_buffer.hpp"
46#include "realtime_tools/realtime_publisher.hpp"
47
48#include "control_toolbox/pid.hpp"
49
50namespace control_toolbox
51{
52
53class PidROS
54{
55public:
70 template<class NodeT>
71 explicit PidROS(
72 std::shared_ptr<NodeT> node_ptr,
73 std::string prefix = std::string(""),
74 bool prefix_is_for_params = false
75 )
76 : PidROS(
77 node_ptr->get_node_base_interface(),
78 node_ptr->get_node_logging_interface(),
79 node_ptr->get_node_parameters_interface(),
80 node_ptr->get_node_topics_interface(),
81 prefix, prefix_is_for_params)
82 {
83 }
84
85 PidROS(
86 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
87 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
88 rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params,
89 rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
90 std::string prefix = std::string(""), bool prefix_is_for_params = false);
91
107 double p, double i, double d, double i_max, double i_min, bool antiwindup);
108
122 double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term);
123
129
135 void reset();
136
142 void reset(bool save_i_term);
143
154 double compute_command(double error, const rclcpp::Duration & dt);
155
167 double compute_command(double error, double error_dot, const rclcpp::Duration & dt);
168
174
189 void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
190
197 void set_gains(const Pid::Gains & gains);
198
203 void set_current_cmd(double cmd);
204
209 double get_current_cmd();
210
215 std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> get_pid_state_publisher();
216
223 void get_current_pid_errors(double & pe, double & ie, double & de);
224
228 void print_values();
229
234 inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
236 {
237 return parameter_callback_;
238 }
239
240protected:
241 std::string topic_prefix_;
242 std::string param_prefix_;
243
244private:
245 void set_parameter_event_callback();
246
247 void publish_pid_state(double cmd, double error, rclcpp::Duration dt);
248
249 void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value);
250
251 bool get_double_param(const std::string & param_name, double & value);
252
253 bool get_boolean_param(const std::string & param_name, bool & value);
254
262 void set_prefixes(const std::string &topic_prefix);
263
264 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
265
266 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
267 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
268 rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_;
269 rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_;
270
271 std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::PidState>> rt_state_pub_;
272 std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> state_pub_;
273
274 Pid pid_;
275};
276
277} // namespace control_toolbox
278
279#endif // CONTROL_TOOLBOX__PID_ROS_HPP_
Definition pid_ros.hpp:54
void print_values()
Print to console the current parameters.
Definition pid_ros.cpp:311
bool initialize_from_ros_parameters()
Initialize the PID controller based on already set parameters.
Definition pid_ros.cpp:167
void set_current_cmd(double cmd)
Set current command for this PID controller.
Definition pid_ros.cpp:298
void get_current_pid_errors(double &pe, double &ie, double &de)
Return PID error terms for the controller.
Definition pid_ros.cpp:302
double get_current_cmd()
Return current command for this PID controller.
Definition pid_ros.cpp:300
PidROS(std::shared_ptr< NodeT > node_ptr, std::string prefix=std::string(""), bool prefix_is_for_params=false)
Constructor of PidROS class.
Definition pid_ros.hpp:71
double compute_command(double error, const rclcpp::Duration &dt)
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is...
Definition pid_ros.cpp:240
Pid::Gains get_gains()
Get PID gains for the controller.
Definition pid_ros.cpp:254
void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Set PID gains for the controller.
Definition pid_ros.cpp:256
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr get_parameters_callback_handle()
Return PID parameters callback handle.
Definition pid_ros.hpp:235
std::shared_ptr< rclcpp::Publisher< control_msgs::msg::PidState > > get_pid_state_publisher()
Return PID state publisher.
Definition pid_ros.cpp:235
void reset()
Reset the state of this PID controller.
Definition pid_ros.cpp:224
void initialize_from_args(double p, double i, double d, double i_max, double i_min, bool antiwindup)
Initialize the PID controller and set the parameters.
Definition pid_ros.cpp:198
A basic pid class.
Definition pid.hpp:110
Definition dither.hpp:46
Store gains in a struct to allow easier realtime buffer usage.
Definition pid.hpp:116