From 07c4a6276297c8365f14596eb338ad72a2877658 Mon Sep 17 00:00:00 2001 From: Brian Holt Date: Tue, 28 May 2024 16:28:18 +0100 Subject: [PATCH] Update to Foxy Fitzroy (#2) * Initial commit * Setpoint node is done. * Adjusting file structure. * Getting into the meat of code migration. * The meat of pid.cpp compiles but it does not spin correctly. * Fixed the spinning issue. Controller node is functional. * It works! At a basic level. * Adding a launch file and minor cleanup. * Trying to fix readme format. * Setting topic names up for configuration, debug on PID parameters. * Setting version number for initial release. * Correcting the tag. * Updating version * Re-write to use newer ROS2 launch file structure * Update to Foxy QoS profile * Replace lambda functions with member functions * Add newlines at end of files * make control_effort callback private * Use ROS2 parameter mechanism for PID variables * Log callbacks under debug mode * Do not ROS QoS profile - for some reason data isn't making it through to the subscriber * Use 2 space indentation * fixed typo in comment * exposed angle_error and pid_enabled parameters --------- Co-authored-by: AndyZe --- include/pid/pid.h | 9 +++-- launch/servo_sim.py | 46 +++++++++++-------------- src/pid/controller.cpp | 2 +- src/pid/pid.cpp | 78 +++++++++++++++++++++++++----------------- src/pid/servo_sim.cpp | 30 ++++++++-------- src/pid/setpoint.cpp | 23 ++++++------- 6 files changed, 99 insertions(+), 89 deletions(-) diff --git a/include/pid/pid.h b/include/pid/pid.h index 1ae11a8..e7f193b 100644 --- a/include/pid/pid.h +++ b/include/pid/pid.h @@ -14,13 +14,15 @@ namespace pid_ns class PID : public rclcpp::Node { public: - explicit PID(double Kp, double Ki, double Kd); + explicit PID(); void doCalcs(); // Primary output variable double control_effort_ = 0; private: + void state_callback(const std_msgs::msg::Float64::SharedPtr msg); + void setpoint_callback(const std_msgs::msg::Float64::SharedPtr msg); void printParameters(); bool validateParameters(); @@ -39,7 +41,7 @@ class PID : public rclcpp::Node // PID gains double Kp_ = 0, Ki_ = 0, Kd_ = 0; - // Parameters for error calc. with disconinuous input + // Parameters for error calc. with discontinuous input bool angle_error_ = false; double angle_wrap_ = 2.0 * 3.14159; @@ -94,4 +96,5 @@ class PID : public rclcpp::Node }; } // end pid namespace -#endif \ No newline at end of file + +#endif diff --git a/launch/servo_sim.py b/launch/servo_sim.py index 206e9e5..11d8b9d 100644 --- a/launch/servo_sim.py +++ b/launch/servo_sim.py @@ -36,30 +36,24 @@ ## ############################################################################### -from launch.exit_handler import default_exit_handler, restart_exit_handler -from ros2run.api import get_executable_path +from launch import LaunchDescription +from launch_ros.actions import Node - -def launch(launch_descriptor, argv): - ld = launch_descriptor - package = 'pid' - ld.add_process( - cmd=[get_executable_path(package_name=package, executable_name='controller')], - name='controller', - exit_handler=restart_exit_handler, - ) - package = 'pid' - ld.add_process( - cmd=[get_executable_path(package_name=package, executable_name='setpoint')], - name='setpoint', - exit_handler=restart_exit_handler, - ) - package = 'pid' - ld.add_process( - cmd=[get_executable_path(package_name=package, executable_name='servo_sim')], - name='servo_sim', - # The joy node is required, die if it dies - exit_handler=restart_exit_handler, - ) - - return ld +def generate_launch_description(): + return LaunchDescription([ + Node( + package='pid', + executable='controller', + name='controller' + ), + Node( + package='pid', + executable='setpoint', + name='setpoint' + ), + Node( + package='pid', + executable='servo_sim', + name='servo_sim' + ) + ]) diff --git a/src/pid/controller.cpp b/src/pid/controller.cpp index 05511df..2b67620 100644 --- a/src/pid/controller.cpp +++ b/src/pid/controller.cpp @@ -73,7 +73,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); // Create the PID node. - auto my_pid = std::make_shared(5, 0.1, 0.1); + auto my_pid = std::make_shared(); // Respond to inputs until shut down // TODO: make this rate configurable diff --git a/src/pid/pid.cpp b/src/pid/pid.cpp index dc4b377..c77fd9b 100644 --- a/src/pid/pid.cpp +++ b/src/pid/pid.cpp @@ -41,42 +41,29 @@ #include using namespace pid_ns; +using std::placeholders::_1; -PID::PID(double Kp, double Ki, double Kd): +PID::PID(): Node("controller"), delta_t_(0, 0) { - // Callbacks for incoming state and setpoint messages - auto state_callback = - [this](const std_msgs::msg::Float64::SharedPtr msg) -> void - { - plant_state_ = msg-> data; - RCLCPP_INFO(this->get_logger(), "State: [%f]", plant_state_) - - new_state_or_setpt_ = true; - }; - - auto setpoint_callback = - [this](const std_msgs::msg::Float64::SharedPtr msg) -> void - { - setpoint_ = msg->data; - RCLCPP_INFO(this->get_logger(), "Setpoint: [%f]", setpoint_) - - new_state_or_setpt_ = true; - }; - state_sub_ = create_subscription(topic_from_plant_, state_callback); - setpoint_sub_ = create_subscription(setpoint_topic_, setpoint_callback); + state_sub_ = this->create_subscription(topic_from_plant_, 10, std::bind(&PID::state_callback, this, _1)); + setpoint_sub_ = this->create_subscription(setpoint_topic_, 10, std::bind(&PID::setpoint_callback, this, _1)); // Create a publisher with a custom Quality of Service profile. - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 7; - control_effort_pub_ = this->create_publisher(topic_from_controller_, custom_qos_profile); - - // TODO: get these parameters from server, - // along with the other configurable params (cutoff_frequency_, etc.) - Kp_ = Kp; - Ki_ = Ki; - Kd_ = Kd; + // rclcpp::QoS custom_qos_profile(rclcpp::KeepLast(7), rmw_qos_profile_sensor_data); + control_effort_pub_ = this->create_publisher(topic_from_controller_, 10); + + // Declare parameters + this->declare_parameter("Kp", 1.0); + this->declare_parameter("Ki", 0.0); + this->declare_parameter("Kd", 0.0); + this->declare_parameter("lower_limit", -1000.0); + this->declare_parameter("upper_limit", 1000.0); + this->declare_parameter("windup_limit", -1000.0); + this->declare_parameter("cutoff_frequency", -1.0); + this->declare_parameter("angle_error", false); + this->declare_parameter("pid_enabled", true); printParameters(); @@ -84,6 +71,24 @@ PID::PID(double Kp, double Ki, double Kd): std::cout << "Error: invalid parameter\n"; } + +// Callbacks for incoming state message +void PID::state_callback(const std_msgs::msg::Float64::SharedPtr msg) +{ + plant_state_ = msg->data; + RCLCPP_DEBUG(this->get_logger(), "State: [%f]", plant_state_); + + new_state_or_setpt_ = true; +} + +void PID::setpoint_callback(const std_msgs::msg::Float64::SharedPtr msg) +{ + setpoint_ = msg->data; + RCLCPP_DEBUG(this->get_logger(), "Setpoint: [%f]", setpoint_); + + new_state_or_setpt_ = true; +} + void PID::printParameters() { std::cout << std::endl << "PID PARAMETERS" << std::endl << "-----------------------------------------" << std::endl; @@ -120,6 +125,17 @@ void PID::doCalcs() // Do fresh calcs if knowledge of the system has changed. if (new_state_or_setpt_) { + // Get parameters from the server + this->get_parameter("Kp", Kp_); + this->get_parameter("Ki", Ki_); + this->get_parameter("Kd", Kd_); + this->get_parameter("lower_limit", lower_limit_); + this->get_parameter("upper_limit", upper_limit_); + this->get_parameter("windup_limit", windup_limit_); + this->get_parameter("cutoff_frequency", cutoff_frequency_); + this->get_parameter("angle_error", angle_error_); + this->get_parameter("pid_enabled", pid_enabled_); + if (!((Kp_ <= 0. && Ki_ <= 0. && Kd_ <= 0.) || (Kp_ >= 0. && Ki_ >= 0. && Kd_ >= 0.))) // All 3 gains should have the same sign { @@ -236,4 +252,4 @@ void PID::doCalcs() error_integral_ = 0.0; new_state_or_setpt_ = false; -} \ No newline at end of file +} diff --git a/src/pid/servo_sim.cpp b/src/pid/servo_sim.cpp index d54f122..eb2d70a 100644 --- a/src/pid/servo_sim.cpp +++ b/src/pid/servo_sim.cpp @@ -45,6 +45,7 @@ #include "rcutils/cmdline_parser.h" #include "std_msgs/msg/float64.hpp" +using std::placeholders::_1; void print_usage() { @@ -62,21 +63,13 @@ class ServoSim : public rclcpp::Node explicit ServoSim() : Node("servo_sim"), delta_t_(0, 0) { - state_msg_ = std::make_shared(); + state_msg_ = std_msgs::msg::Float64(); // Create a publisher with a custom Quality of Service profile. - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 7; - state_pub_ = this->create_publisher("state", custom_qos_profile); + // rclcpp::QoS custom_qos_profile(rclcpp::KeepLast(7), rmw_qos_profile_sensor_data); + state_pub_ = this->create_publisher("state", 10); - // Callback for incoming control_effort messages - auto control_effort_callback = - [this](const std_msgs::msg::Float64::SharedPtr msg) -> void - { - control_effort_ = msg-> data; - }; - - control_effort_sub_ = create_subscription("control_effort", control_effort_callback); + control_effort_sub_ = this->create_subscription("control_effort", 10, std::bind(&ServoSim::control_effort_callback, this, _1)); prev_time_ = this->now(); } @@ -90,13 +83,20 @@ class ServoSim : public rclcpp::Node acceleration_ = ((Kv_ * (control_effort_ - (Kbackemf_ * speed_)) + decel_force_) / mass_); // a = F/m speed_ = speed_ + (acceleration_ * delta_t_.nanoseconds()/1e9); displacement_ = displacement_ + speed_ * delta_t_.nanoseconds()/1e9; - state_msg_->data = displacement_; - + state_msg_.data = displacement_; + state_pub_->publish(state_msg_); } private: - std::shared_ptr state_msg_; + // Callback for incoming control_effort messages + void control_effort_callback(const std_msgs::msg::Float64::SharedPtr msg) + { + control_effort_ = msg->data; + RCLCPP_DEBUG(this->get_logger(), "control effort: [%f]", control_effort_); + } + + std_msgs::msg::Float64 state_msg_; rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Subscription::SharedPtr control_effort_sub_; double control_effort_ = 0; diff --git a/src/pid/setpoint.cpp b/src/pid/setpoint.cpp index 6eefbc0..4529719 100644 --- a/src/pid/setpoint.cpp +++ b/src/pid/setpoint.cpp @@ -63,20 +63,11 @@ class Setpoint : public rclcpp::Node explicit Setpoint() : Node("setpoint") { - msg_ = std::make_shared(); - - // Create a function for when messages are to be sent. - auto publish_message = - [this](double& setpoint) -> void - { - msg_->data = setpoint; - pub_->publish(msg_); - }; + msg_ = std_msgs::msg::Float64(); // Create a publisher with a custom Quality of Service profile. - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 7; - pub_ = this->create_publisher("setpoint", custom_qos_profile); + // rclcpp::QoS custom_qos_profile(rclcpp::KeepLast(7), rmw_qos_profile_sensor_data); + pub_ = this->create_publisher("setpoint", 10); // Negate the setpoint every 5 seconds rclcpp::Rate loop_rate(0.2); @@ -91,8 +82,14 @@ class Setpoint : public rclcpp::Node } } + void publish_message(double& setpoint) + { + msg_.data = setpoint; + pub_->publish(msg_); + } + private: - std::shared_ptr msg_; + std_msgs::msg::Float64 msg_; rclcpp::Publisher::SharedPtr pub_; };