diff --git a/include/ap1/control/ackermann_controller.hpp b/include/ap1/control/ackermann_controller.hpp index 3c88484..349e580 100644 --- a/include/ap1/control/ackermann_controller.hpp +++ b/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); diff --git a/include/ap1/control/control_node.hpp b/include/ap1/control/control_node.hpp index a130b04..f2cacbf 100644 --- a/include/ap1/control/control_node.hpp +++ b/include/ap1/control/control_node.hpp @@ -8,6 +8,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -33,7 +34,8 @@ class ControlNode : public rclcpp::Node // Controller std::unique_ptr controller_; - AckermannController ackermann_controller_; + // Unique_ptr used for delayd initialization. Allows construction after reading ros params + std::unique_ptr 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 diff --git a/src/ackermann_controller.cpp b/src/ackermann_controller.cpp index 25a92c6..bad2152 100644 --- a/src/ackermann_controller.cpp +++ b/src/ackermann_controller.cpp @@ -44,7 +44,7 @@ std::unordered_map 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 diff --git a/src/control_node.cpp b/src/control_node.cpp index 29d9812..4fc1f61 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #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("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(cfg); + + // Topic Subscriptions speed_profile_sub_ = this->create_subscription( - "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( - "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( - "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( - "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("ap1/control/turn_angle", 10); motor_power_pub_ = this->create_publisher("ap1/control/motor_power", 10); diff --git a/src/main.cpp b/src/main.cpp index 596b78e..f64228c 100644 --- a/src/main.cpp +++ b/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 "); - return 1; - } - + // Create node auto node = std::make_shared(config_path); + + // Spin node rclcpp::spin(node); rclcpp::shutdown(); return 0;