Ackermann and control node adjusted for param passing

include/ap1/control/ackermann_controller.hpp

@@ -18,10 +18,11 @@ class AckermannController
   public:
     struct Config
     {
-        double L;             // wheelbase in meters
-        double a_max;         // max longitudinal accleration (ms^-2)
-        double delta_max;     // max steering angle
-        double throttle_gain; // scales accel to throttle [-1, 1]
+        // Added default values for safety if config file fails to load
+        double L = 0.33;             // wheelbase in meters
+        double a_max = 2.0;          // max longitudinal accleration (ms^-2)
+        double delta_max = 0.5;      // max steering angle (~28 deg)
+        double throttle_gain = 1.0;  // scales accel to throttle [-1, 1]
     };
 
     explicit AckermannController(const Config &cfg);

include/ap1/control/control_node.hpp

@@ -8,6 +8,7 @@
 
 #include <iostream>
 #include <string>
+#include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -33,7 +34,8 @@ class ControlNode : public rclcpp::Node
 
     // Controller
     std::unique_ptr<IController> controller_;
-    AckermannController ackermann_controller_;
+    // Unique_ptr used for delayd initialization. Allows construction after reading ros params
+    std::unique_ptr<AckermannController> ackermann_controller_;
 
     // Memory
     // half these types are very unnecessary, we should just have stampedfloat or
@@ -61,7 +63,7 @@ class ControlNode : public rclcpp::Node
     void control_loop_callback();
 
   public:
-    ControlNode(const std::string& cfg_path, float rate_hz = 60);
+    ControlNode(float rate_hz = 60);
 };
 } // namespace ap1::control
 

src/ackermann_controller.cpp

@@ -44,7 +44,7 @@ std::unordered_map<std::string, double> load_csv_config(const std::string& path)
             }
             catch (const std::exception& e)
             {
-                throw std::runtime_error("Invalid vlaue for key " + key + ": " + value_str);
+                throw std::runtime_error("Invalid value for key " + key + ": " + value_str);
             }
         }
     }
@@ -78,7 +78,9 @@ AckermannController::Command AckermannController::compute_command(const vec3f& a
     double delta = std::atan(cfg_.L * kappa);
     delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max);
 
-    double throttle = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, -1.0, 1.0);
+    // Safety: ensure a_max is not zero to avoid div-by-zero
+    double safe_a_max = (cfg_.a_max > EPSILON) ? cfg_.a_max : 1.0;
+    double throttle = std::clamp(a_long / safe_a_max * cfg_.throttle_gain, -1.0, 1.0);
 
     cmd.throttle = throttle;
     cmd.steering = delta;
@@ -88,19 +90,30 @@ AckermannController::Command AckermannController::compute_command(const vec3f& a
 
 AckermannController::Config AckermannController::load_config(const std::string& path)
 {
-    Config cfg;
-
-    auto map = load_csv_config(path);
-
-    if (map.count("wheelbase"))
-        cfg.L = map["wheelbase"];
-    if (map.count("a_max"))
-        cfg.a_max = map["a_max"];
-    if (map.count("delta_max"))
-        cfg.delta_max = map["delta_max"];
-    if (map.count("throttle_gain"))
-        cfg.throttle_gain = map["throttle_gain"];
+    Config cfg; 
+
+    // Try to load the CSV
+    try {
+        auto map = load_csv_config(path);
+
+        // If load succeeds, overwrite the defaults
+        if (map.count("wheelbase"))
+            cfg.L = map["wheelbase"];
+        if (map.count("a_max"))
+            cfg.a_max = map["a_max"];
+        if (map.count("delta_max"))
+            cfg.delta_max = map["delta_max"];
+        if (map.count("throttle_gain"))
+            cfg.throttle_gain = map["throttle_gain"];
+    }
+    catch (const std::exception& e) {
+        // If load fails, catch the error here. 
+        // Print a warning to stderr, but allow the program to continue using defaults.
+        std::cerr << "[AckermannController] Warning: " << e.what() 
+                  << ". Using default configuration." << std::endl;
+    }
 
+    // 4. Return the config (either loaded from file or defaults)
     return cfg;
 }
 } // namespace ap1::control

src/control_node.cpp

@@ -6,6 +6,7 @@
 #include <format>
 #include <iostream>
 #include <string>
+#include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -45,6 +46,12 @@ void ControlNode::on_turn_angle(const TurnAngleStamped turn_angle)
 
 void ControlNode::control_loop_callback()
 {
+    // Safety check if controller is initialized 
+    if (!ackermann_controller_) {
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Controller not initialized!");
+        return;
+    }
+
     // the car's current velocity. we only support moving forward atp
     const vec3f velocity(this->vehicle_speed_.speed, 0, 0); // +x is always forward on the car
 
@@ -71,7 +78,7 @@ void ControlNode::control_loop_callback()
     RCLCPP_INFO(this->get_logger(), "ACC: %.2f, %.2f, %.2f", acc.x, acc.y, acc.z);
 
     // compute acc and throttle using ackermann controller
-    AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity);
+    AckermannController::Command cmd = ackermann_controller_->compute_command(acc, velocity);
 
     // log
     RCLCPP_INFO(this->get_logger(), "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);
@@ -94,25 +101,47 @@ void ControlNode::control_loop_callback()
     motor_power_pub_->publish(pwr_msg);
 }
 
-ControlNode::ControlNode(const std::string& cfg_path, float rate_hz)
-    : Node("control_node"), rate_hz_(rate_hz), controller_(new PDController()),
-      ackermann_controller_(AckermannController(AckermannController::load_config(cfg_path)))
+ControlNode::ControlNode(float rate_hz): 
+    Node("control_node"), 
+    rate_hz_(rate_hz), 
+    controller_(new PDController())
 {
-    // Subs
+    // Declare and get parameter for config path
+    this->declare_parameter<std::string>("config_path", "config.csv");
+    std::string cfg_path = this->get_parameter("config_path").as_string();
+
+    // Return either file contents OR default values.
+    AckermannController::Config cfg = AckermannController::load_config(cfg_path);
+
+    // Initialize Controller
+    ackermann_controller_ = std::make_unique<AckermannController>(cfg);
+
+    // Topic Subscriptions
     speed_profile_sub_ = this->create_subscription<SpeedProfileStamped>(
-        "ap1/planning/speed_profile", 10,
-        std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1));
+        "ap1/planning/speed_profile", 
+        10,
+        std::bind(&ControlNode::on_speed_profile, this, std::placeholders::_1)
+    );
+
     target_path_sub_ = this->create_subscription<TargetPathStamped>(
-        "ap1/planning/target_path", 10,
-        std::bind(&ControlNode::on_path, this, std::placeholders::_1));
+        "ap1/planning/target_path", 
+        10,
+        std::bind(&ControlNode::on_path, this, std::placeholders::_1)
+    );
+
     vehicle_speed_sub_ = this->create_subscription<VehicleSpeedStamped>(
-        "ap1/actuation/speed_actual", 10,
-        std::bind(&ControlNode::on_speed, this, std::placeholders::_1));
+        "ap1/actuation/speed_actual", 
+        10,
+        std::bind(&ControlNode::on_speed, this, std::placeholders::_1)
+    );
+
     vehicle_turn_angle_sub_ = this->create_subscription<TurnAngleStamped>(
-        "ap1/actuation/turn_angle_actual", 10,
-        std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1));
+        "ap1/actuation/turn_angle_actual", 
+        10,
+        std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1)
+    );
 
-    // Pubs
+    // Topic Publications
     turning_angle_pub_ = this->create_publisher<TurnAngleStamped>("ap1/control/turn_angle", 10);
     motor_power_pub_ = this->create_publisher<MotorPowerStamped>("ap1/control/motor_power", 10);
 

src/main.cpp

@@ -10,19 +10,10 @@ int main(int argc, char** argv)
     // init ros & ros logger
     rclcpp::init(argc, argv);
 
-    // get control node config path from args
-    std::string config_path = "";
-    if (argc > 1)
-    {
-        config_path = argv[1];
-    }
-    else
-    {
-        RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Usage: control_node <config.csv>");
-        return 1;
-    }
-
+    // Create node
     auto node = std::make_shared<ap1::control::ControlNode>(config_path);
+
+    // Spin node
     rclcpp::spin(node);
     rclcpp::shutdown();
     return 0;