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 <[email protected]>
UTNuclearRoboticsPublic · May 28, 2024 · 07c4a62 · 07c4a62
1 parent 0cf5d0a
commit 07c4a62
Show file tree

Hide file tree

Showing 6 changed files with 99 additions and 89 deletions.
diff --git 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
+
+#endif
diff --git 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
@@ -73,7 +73,7 @@ int main(int argc, char * argv[])
   rclcpp::init(argc, argv);
 
   // Create the PID node.
-  auto my_pid = std::make_shared<pid_ns::PID>(5, 0.1, 0.1);
+  auto my_pid = std::make_shared<pid_ns::PID>();
 
   // Respond to inputs until shut down
   // TODO: make this rate configurable

diff --git a/src/pid/pid.cpp b/src/pid/pid.cpp
@@ -41,49 +41,54 @@
 #include <pid/pid.h>
 
 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<std_msgs::msg::Float64>(topic_from_plant_, state_callback);
-  setpoint_sub_ = create_subscription<std_msgs::msg::Float64>(setpoint_topic_, setpoint_callback);
+  state_sub_ = this->create_subscription<std_msgs::msg::Float64>(topic_from_plant_, 10, std::bind(&PID::state_callback, this, _1));
+  setpoint_sub_ = this->create_subscription<std_msgs::msg::Float64>(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<std_msgs::msg::Float64>(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<std_msgs::msg::Float64>(topic_from_controller_, 10);
+
+  // Declare parameters
+  this->declare_parameter<double>("Kp", 1.0);
+  this->declare_parameter<double>("Ki", 0.0);
+  this->declare_parameter<double>("Kd", 0.0);
+  this->declare_parameter<double>("lower_limit", -1000.0);
+  this->declare_parameter<double>("upper_limit", 1000.0);
+  this->declare_parameter<double>("windup_limit", -1000.0);
+  this->declare_parameter<double>("cutoff_frequency", -1.0);
+  this->declare_parameter<bool>("angle_error", false);
+  this->declare_parameter<bool>("pid_enabled", true);
 
   printParameters();
 
   if (not validateParameters())
     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;
-}
+}
diff --git 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<std_msgs::msg::Float64>();
+    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<std_msgs::msg::Float64>("state", custom_qos_profile);
+    // rclcpp::QoS custom_qos_profile(rclcpp::KeepLast(7), rmw_qos_profile_sensor_data);
+    state_pub_ = this->create_publisher<std_msgs::msg::Float64>("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<std_msgs::msg::Float64>("control_effort", control_effort_callback);
+    control_effort_sub_ = this->create_subscription<std_msgs::msg::Float64>("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<std_msgs::msg::Float64> 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<std_msgs::msg::Float64>::SharedPtr state_pub_;
   rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr control_effort_sub_;
   double control_effort_ = 0;

diff --git 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<std_msgs::msg::Float64>();
-
-    // 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<std_msgs::msg::Float64>("setpoint", custom_qos_profile);
+    // rclcpp::QoS custom_qos_profile(rclcpp::KeepLast(7), rmw_qos_profile_sensor_data);
+    pub_ = this->create_publisher<std_msgs::msg::Float64>("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<std_msgs::msg::Float64> msg_;
+  std_msgs::msg::Float64 msg_;
   rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_;
 };