feat: added topics into config file for the euler pid controller

vortexntnu · Talhanc · Sep 27, 2024 · Oct 11, 2024 · Oct 11, 2024 · Oct 13, 2024
commit 66837335d439d6f1d9e874629e889083dfa7056c
diff --git a/control/pid_controller_dp_euler/config/pid_params.yaml b/control/pid_controller_dp_euler/config/pid_params.yaml
@@ -1,5 +1,10 @@
 pid_controller_euler_node:
   ros__parameters:
+    control_topic: /thrust/wrench_input
+    dp_reference_topic: /dp/reference
+    nucleus_odom_topic: /orca/odom
+    software_kill_switch_topic: /softwareKillSwitch
+    software_operation_mode_topic: /softwareOperationMode
     Kp: [70.0, 70.0, 70.0, 15.0, 15.0, 15.0]
     Ki: [9.0, 9.0, 9.0, 1.0, 1.5, 1.5]
     Kd: [20.0, 20.0, 20.0, 0.1, 1.0, 1.0]
diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp
@@ -23,8 +23,7 @@ class PIDControllerNode : public rclcpp::Node {
 
     void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
 
-    void guidance_callback(
-        const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
+    void guidance_callback(const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
 
     void publish_tau();
 
@@ -44,8 +43,7 @@ class PIDControllerNode : public rclcpp::Node {
 
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
 
-    rclcpp::Subscription<vortex_msgs::msg::ReferenceFilter>::SharedPtr
-        guidance_sub_;
+    rclcpp::Subscription<vortex_msgs::msg::ReferenceFilter>::SharedPtr guidance_sub_;
 
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr kp_sub_;
 

diff --git a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp
@@ -1,26 +1,56 @@
 #include <pid_controller_dp_euler/pid_controller_ros.hpp>
 #include <pid_controller_dp_euler/pid_controller_utils.hpp>
+#include <string>
 
 PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") {
     time_step_ = std::chrono::milliseconds(10);
     rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
+
+    this->declare_parameter("nucleus_odom_topic", "/orca/odom");
+    this->declare_parameter("dp_reference_topic", "/dp/reference");
+    this->declare_parameter("control_topic", "/thrust/wrench_input");
+    this->declare_parameter("software_kill_switch_topic", "/softwareKillSwitch");
+    this->declare_parameter("software_operation_mode_topic", "/softwareOperationMode");
+
+    std::string nucleus_odom_topic =
+        this->get_parameter("nucleus_odom_topic").as_string();
+    std::string dp_reference_topic =
+        this->get_parameter("dp_reference_topic").as_string();
+    std::string control_topic =
+        this->get_parameter("control_topic").as_string();
+    std::string software_kill_switch_topic =
+        this->get_parameter("software_kill_switch_topic").as_string();
+    std::string software_operation_mode_topic =
+        this->get_parameter("software_operation_mode_topic").as_string();
+
+    std::string nucleus_odom_topic =
+        this->get_parameter("nucleus_odom_topic").as_string();
+    std::string dp_reference_topic =
+        this->get_parameter("dp_reference_topic").as_string();
+    std::string control_topic =
+        this->get_parameter("control_topic").as_string();
+    std::string software_kill_switch_topic =
+        this->get_parameter("software_kill_switch_topic").as_string();
+    std::string software_operation_mode_topic =
+        this->get_parameter("software_operation_mode_topic").as_string();
+
     auto qos_sensor_data = rclcpp::QoS(
         rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile);
     killswitch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
-        "/softwareKillSwitch", 10,
+        software_kill_switch_topic, 10,
         std::bind(&PIDControllerNode::killswitch_callback, this,
                   std::placeholders::_1));
     software_mode_sub_ = this->create_subscription<std_msgs::msg::String>(
-        "/softwareOperationMode", 10,
+        software_operation_mode_topic, 10,
         std::bind(&PIDControllerNode::software_mode_callback, this,
                   std::placeholders::_1));
     odometry_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
-        "/nucleus/odom", qos_sensor_data,
+        nucleus_odom_topic, qos_sensor_data,
         std::bind(&PIDControllerNode::odometry_callback, this,
                   std::placeholders::_1));
     guidance_sub_ =
         this->create_subscription<vortex_msgs::msg::ReferenceFilter>(
-            "/dp/reference", 10,
+            dp_reference_topic, 10,
             std::bind(&PIDControllerNode::guidance_callback, this,
                       std::placeholders::_1));
     kp_sub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
@@ -36,7 +66,7 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") {
         std::bind(&PIDControllerNode::kd_callback, this,
                   std::placeholders::_1));
     tau_pub_ = this->create_publisher<geometry_msgs::msg::Wrench>(
-        "/thrust/wrench_input", 10);
+        control_topic, 10);
     tau_pub_timer_ = this->create_wall_timer(
         time_step_, std::bind(&PIDControllerNode::publish_tau, this));
     set_pid_params();