Fix NAN issue, fix pointers

Author: alyashour

CHANGELOG.md

@@ -0,0 +1,15 @@
+# CHANGELOG
+
+## v0.1 | Initial dev
+
+### v0.1.0 - Unknown
+- [x] Control laterally (no rev, no brake)
+
+### v0.1.1 - WIP
+- [x] Fix control issue. Does not accelerate in 3D.
+- [ ] Add braking
+- [ ] Add reversing
+- [ ] Add turn output
+
+### v0.1.2 - WIP
+- [ ] Add MPC

config/control_node_cfg.csv

@@ -2,3 +2,4 @@ wheelbase,2.5
 a_max,3.0
 delta_max,0.5
 throttle_gain,1.0
+brake_gain,1.0

include/ap1/control/ackermann_controller.hpp

@@ -21,14 +21,16 @@ class AckermannController
         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]
+        double throttle_gain; // scales accel to throttle [0, 1]
+        double brake_gain;    // scales -accel to brake [0, 1]
     };
 
     explicit AckermannController(const Config &cfg);
 
     struct Command
     {
-        double throttle; // normalized [-1, 1]
+        double throttle; // normalized [0, 1]
+        double brake; // normalized [0, 1]
         double steering; // in rads
     };
 

include/ap1/control/control_node.hpp

@@ -34,10 +34,10 @@ class ControlNode : public rclcpp::Node
     AckermannController ackermann_controller_;
 
     // Memory
-    ap1_msgs::msg::SpeedProfileStamped speed_profile_;
-    ap1_msgs::msg::TargetPathStamped target_path_;
-    ap1_msgs::msg::FloatStamped vehicle_speed_;
-    ap1_msgs::msg::FloatStamped vehicle_turn_angle;
+    ap1_msgs::msg::SpeedProfileStamped::SharedPtr speed_profile_;
+    ap1_msgs::msg::TargetPathStamped::SharedPtr target_path_;
+    ap1_msgs::msg::FloatStamped::SharedPtr vehicle_speed_;
+    ap1_msgs::msg::FloatStamped::SharedPtr vehicle_turn_angle;
 
     // Subs
     rclcpp::Subscription<ap1_msgs::msg::TargetPathStamped>::SharedPtr target_path_sub_;
@@ -50,10 +50,10 @@ class ControlNode : public rclcpp::Node
     rclcpp::Publisher<ap1_msgs::msg::FloatStamped>::SharedPtr motor_power_pub_; // between -1 and 1? probably
 
     // Methods
-    void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile);
-    void on_path(const ap1_msgs::msg::TargetPathStamped target_path);
-    void on_speed(const ap1_msgs::msg::FloatStamped speed);
-    void on_turn_angle(const ap1_msgs::msg::FloatStamped turn_angle);
+    void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped::SharedPtr speed_profile);
+    void on_path(const ap1_msgs::msg::TargetPathStamped::SharedPtr target_path);
+    void on_speed(const ap1_msgs::msg::FloatStamped::SharedPtr speed);
+    void on_turn_angle(const ap1_msgs::msg::FloatStamped::SharedPtr turn_angle);
     void control_loop_callback();
 
   public:

include/ap1/control/icontroller.hpp

@@ -7,18 +7,18 @@
 #ifndef AP1_CONTROLLER_HPP
 #define AP1_CONTROLLER_HPP
 
-#include <vector>
-
 #include "vectors.hpp"
-#include "geometry_msgs/msg/point.hpp"
+#include "ap1_msgs/msg/target_path_stamped.hpp"
+
+using ap1_msgs::msg::TargetPathStamped;
 
 namespace ap1::control
 {
 class IController
 {
   public:
     ~IController() = default;
-    virtual vec3f compute_acceleration(const vec3f& vel, const vec2f& target_pos, const float target_speed) = 0;
+    virtual vec3f compute_acceleration(const vec3f& vel, const TargetPathStamped::ConstSharedPtr path, const float target_speed) = 0;
 };
 } // namespace ap1::control
 

include/ap1/control/pd_controller.hpp

@@ -9,15 +9,51 @@
 
 #include <cmath>
 #include <iostream>
-#include <stdexcept>
-#include <vector>
-
-#include "geometry_msgs/msg/point.hpp"
+#include <sys/types.h>
 
 #include "ap1/control/icontroller.hpp"
+#include "ap1_msgs/msg/target_path_stamped.hpp"
 #include "vectors.hpp"
 
 #define EPSILON 1e-6
+#define MAX_WAYPOINT_SEARCH 5 // how far into the planned path to search for a path far from the current position
+
+using ap1_msgs::msg::TargetPathStamped;
+
+/**
+ * @brief Figure's out the first, next up target position.
+ * @return vec2f Returns a vec containing the first waypoint that is further than EPSILON from the origin.
+ */
+inline vec2f get_target_position(const TargetPathStamped::ConstSharedPtr path) {
+    uint wpt_idx = 0;
+
+    while (wpt_idx < MAX_WAYPOINT_SEARCH) {
+        const vec2f next{(float) path->path[wpt_idx].x, (float) path->path[wpt_idx].y};
+        const vec2f origin{0, 0};
+
+        // if the waypoint and the origin are far enough apart
+        if (distance(next, origin) > EPSILON) {
+            // return it
+            return next;
+        }
+
+        // otherwise increment and try again
+        wpt_idx++;
+    }
+
+    // OPERATION FAILED!
+    // at this point in development I'm just trying to get it working
+    // this leaves me with 2 very bad options
+    // option 1: I return 0 here and let jesus take the wheel
+    // option 2: I can throw a runtime error and just take all of control down
+    // both options are kind of shit I can't lie
+    // really this should propagate upwards to some kind of system that takes care of it, decelerating the car or keeping it stopped depending
+    // on if we're starting up or moving or whatever
+    // but considering we don't have that I'm just going to take option 1.
+    return {0, 0};
+    // option 2 for fun:
+    // throw std::runtime_error("MAX_WAYPOINT_SEARCH exceeded! Couldn't find a waypoint far enough."); 
+}
 
 namespace ap1::control
 {
@@ -33,10 +69,19 @@ class PDController : public IController
 
     // Computes acceleration based on current velocity, target position, and target speed. Returns
     // acceleration vector.
-    virtual vec3f compute_acceleration(const vec3f& vel,
-                                       const vec2f& target_pos,
-                                       const float target_speed) override
-    {
+    virtual vec3f compute_acceleration(
+        const vec3f& vel,
+        const TargetPathStamped::ConstSharedPtr path,
+        const float target_speed
+    ) override {
+        if (!path) {
+            printf("Unc is null twin");
+            return {0, 0, 0};
+        }
+
+        const auto target_pos = get_target_position(path);
+
+        // Get the very next target position
         // Convert target position to vec3f and calculate distance
         const float distance = target_pos.length();
 

include/vectors.hpp

@@ -8,11 +8,17 @@
 #define AP1_VECTOR_HPP
 
 #include <format>
-#include <iostream>
 #include <math.h>
 #include <stdexcept>
 #include <stdlib.h>
 
+template <typename T>
+float distance(const T &v1, const T &v2) {
+    return std::sqrt(
+        std::pow(v1.x - v2.x, 2) + std::pow(v1.y - v2.y, 2)
+    );
+}
+
 // 2D vector class
 class vec2f
 {

src/ackermann_controller.cpp

@@ -54,33 +54,35 @@ std::unordered_map<std::string, double> load_csv_config(const std::string& path)
 
 AckermannController::AckermannController(const Config& cfg) : cfg_(cfg)
 {
-    printf("Created Ackermann Controller with config: a_max=%f, delta_max=%f, L=%f, "
-           "throttle_gain=%f\n",
-           cfg_.a_max, cfg_.delta_max, cfg_.L, cfg_.throttle_gain);
+    printf("Created Ackermann Controller with config: a_max=%f, delta_max=%f, L=%f, throttle_gain=%f, brake_gain=%f\n",
+           cfg_.a_max, cfg_.delta_max, cfg_.L, cfg_.throttle_gain, cfg_.brake_gain);
 }
 
-// we should move everything over to double but I already wrote all the message types in float
-// and I'm too lazy to switch so we'll do it later
 AckermannController::Command AckermannController::compute_command(const vec3f& acc, const vec3f& vel)
 {
     Command cmd{};
 
-    double a_long = std::clamp(
-        (double)acc.x,
-        -cfg_.a_max, 
-        cfg_.a_max
-    );
-
+    double a_long = std::clamp((double)acc.x, -cfg_.a_max, cfg_.a_max);
     double a_lat = acc.y;
 
     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 = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, -1.0, 1.0);
+    double throttle = 0.0;
+    double brake = 0.0;
+
+    if (a_long >= 0.0) {
+        throttle = std::clamp(a_long / cfg_.a_max * cfg_.throttle_gain, 0.0, 1.0);
+        brake = 0.0;
+    } else {
+        throttle = 0.0;
+        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;

src/control_node.cpp

@@ -3,8 +3,8 @@
  * Author(s): Aly Ashour
  */
 
-#include <format>
-#include <iostream>
+#include <cmath>
+#include <stdexcept>
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
@@ -21,69 +21,78 @@
 using namespace ap1_msgs::msg;
 using namespace ap1::control;
 
-void ControlNode::on_speed_profile(const SpeedProfileStamped speed_profile)
+void ControlNode::on_speed_profile(const SpeedProfileStamped::SharedPtr speed_profile)
 {
     speed_profile_ = speed_profile;
 }
 
-void ControlNode::on_path(const TargetPathStamped target_path)
+void ControlNode::on_path(const TargetPathStamped::SharedPtr target_path)
 {
     target_path_ = target_path;
 }
 
-void ControlNode::on_speed(const FloatStamped speed)
+void ControlNode::on_speed(const FloatStamped::SharedPtr speed)
 {
+    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Received speed from actuation: %.2f", speed->value);
     vehicle_speed_ = speed;
 }
 
-void ControlNode::on_turn_angle(const FloatStamped turn_angle)
+void ControlNode::on_turn_angle(const FloatStamped::SharedPtr turn_angle)
 {
     vehicle_turn_angle = turn_angle;
 }
 
+// TODO: refactor. This is just a mess of spaghetti code vro.
 void ControlNode::control_loop_callback()
 {
+    // check that we have the car's velocity yet
+    if (!this->vehicle_speed_) { // TODO: really we should safety stop all of ap1 on a NAN
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Velocity is null or nan fam. Skipping update.");
+        return;
+    }
+
+    // TEMP: DEBUG
+    if (std::isnan(this->vehicle_speed_->value)) {
+        throw std::runtime_error("Vehicle speed is nan. Crashing");
+    }
+    
     // the car's current velocity. we only support moving forward atp
-    const vec3f velocity(this->vehicle_speed_.value, 0, 0); // +x is always forward on the car
+    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_.path.size() < 1 || PATH_IS_STALE)
-        return; // don't update
-                // todo: ideally this should actually still
-                // send through the speed control
-                // but we'll implement that later
+    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
+        return;       
+    }
 
     // if speed profile has no waypoints or speed profile is too old
-    if (speed_profile_.speeds.size() < 1 || SPEED_PROFILE_IS_STALE)
+    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.");
         return;
+    }
 
     // ask the controller to calculate the acceleration needed
-    // for now we'll only consider the very next waypoint & speed value
-    auto next_waypoint = target_path_.path.at(0);
-    const vec3f acc = controller_->compute_acceleration(
-        velocity, vec2f(next_waypoint.x, next_waypoint.y), speed_profile_.speeds.at(0));
+    const vec3f acc = controller_->compute_acceleration(velocity, target_path_, speed_profile_->speeds.at(0));
 
-    // log
-    // RCLCPP_INFO(this->get_logger(), "ACC: %.2f, %.2f, %.2f", acc.x, acc.y, acc.z);
+    // ALY'S FAVOURITE DEBUG CMD 1
+    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 700, "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);
 
-    // DEBUG
-    // RCLCPP_INFO(this->get_logger(), "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);
+    // 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);
 
     // pack the turn angle into a message
     FloatStamped turn_msg;
     turn_msg.header.stamp = this->now();
-    turn_msg.header.frame_id = "base_link";
     turn_msg.value = cmd.steering; // rads
 
     // pack the power into a message
     FloatStamped pwr_msg;
     pwr_msg.header.stamp = this->now();
-    pwr_msg.header.frame_id = "base_link";
     pwr_msg.value = cmd.throttle; // [-1, 1]
 
     // send both messages out