Clean imports, comments, msg packaging

Author: alyashour

CHANGELOG.md

@@ -12,6 +12,7 @@
 - [ ] Add reversing
 - [ ] Add turn output
 - [ ] Fix turning bug
+- [ ] Throttle all main loop logs
 
 ### v0.1.2 - WIP
 - [ ] Add MPC

src/ackermann_controller.cpp

@@ -7,6 +7,7 @@
 #include <algorithm>
 #include <cmath>
 #include <fstream>
+#include <iostream>
 #include <sstream>
 #include <stdexcept>
 #include <string>
@@ -67,23 +68,16 @@ AckermannController::Command AckermannController::compute_command(const vec3f& a
     double speed = vel.length();
     double kappa = (speed > EPSILON) ? a_lat / (speed * speed) : 0.0;
     double delta = std::atan(cfg_.L * kappa);
-    delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max);
-
-    double throttle = 0.0;
-    double brake = 0.0;
+    cmd.steering = delta = std::clamp(delta, -cfg_.delta_max, cfg_.delta_max);
 
     if (a_long >= 0.0) {
-        throttle = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, 0.0, 1.0);
-        brake = 0.0;
+        cmd.throttle = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, 0.0, 1.0);
+        cmd.brake = 0.0;
     } else {
-        throttle = 0.0;
-        brake = std::clamp(-a_long / cfg_.a_max * cfg_.brake_gain, 0.0, 1.0);
+        cmd.throttle = 0.0;
+        cmd.brake = std::clamp(-a_long / cfg_.a_max * cfg_.brake_gain, 0.0, 1.0);
     }
 
-    cmd.throttle = throttle;
-    cmd.brake = brake;
-    cmd.steering = delta;
-
     return cmd;
 }
 
@@ -93,14 +87,11 @@ AckermannController::Config AckermannController::load_config(const std::string&
 
     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"];
+    cfg.L = map["wheelbase"];
+    cfg.a_max = map["a_max"];
+    cfg.delta_max = map["delta_max"];
+    cfg.throttle_gain = map["throttle_gain"];
+    cfg.brake_gain = map["brake_gain"];
 
     return cfg;
 }

src/control_node.cpp

@@ -50,20 +50,20 @@ void ControlNode::control_loop_callback()
         return;
     }
 
-    // the car's current velocity. we only support moving forward atp
+    // the car's current velocity - the car only ever moves forward G
     const vec3f velocity(this->vehicle_speed_->value, 0, 0); // +x is always forward on the car
 
     const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP
 
     // if path has no waypoints OR path is too old
     if (!target_path_ || target_path_->path.size() < 1 || PATH_IS_STALE) {
-        RCLCPP_WARN(this->get_logger(), "Target path is cooked fam. Null, not enough waypoints, or old, skipping."); // TODO: should be throttled
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Target path is invalid. Null, not enough waypoints, or old, skipping.");
         return;       
     }
 
     // if speed profile has no waypoints or speed profile is too old
     if (!speed_profile_ || speed_profile_->speeds.size() < 1 || SPEED_PROFILE_IS_STALE) {
-        RCLCPP_WARN(this->get_logger(), "Speed profile is cooked fam. Null, not enough waypoints, or old, skipping.");
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Speed profile is invalid. Null, not enough waypoints, or old, skipping.");
         return;
     }
 
@@ -77,7 +77,7 @@ void ControlNode::control_loop_callback()
     AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity);
 
     // ALY'S FAVOURITE DEBUG CMD 2
-    // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 700, "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);
+    // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 700, "CMD: {throttle: %.2f, steering: %.2f, brake: %.2f}", cmd.throttle, cmd.steering, cmd.brake);
 
     // pack the turn angle into a message
     FloatStamped turn_msg;