#6 Stanley Algorithm for local test

algorithm/lib/controller/stanley.hpp

@@ -0,0 +1,76 @@
+#pragma once
+
+#include "utils/car_struct.h"
+
+#include <array>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <limits>
+#include <unistd.h>
+
+class Stanley {
+
+public:
+    Stanley(std::vector<std::vector<double>> route, double resolution, double speed, std::vector<double> cpoint,
+            double k, double ks);
+    void stanley_control();
+    std::vector<std::vector<double>> getTrajectory(); 
+    std::vector<int> getTargetnode();
+
+private:
+    // Struct
+    Car car;
+
+    // Vehicle State    
+    double x;
+    double y;
+    double yaw;
+    double v;
+    double acceleration;
+
+    double front_x;
+    double front_y;
+
+    // PID 
+    // double pid_integral;
+    // double previous_error;
+    double kp;
+    // double ki;
+    // double kd;
+
+    // Vehicle Config
+    double WB;
+
+    // System Config
+    double dt;
+
+    // Stanley Parameters
+    double k;
+    double ks;
+
+    // Path
+    std::vector<std::vector<double>> route;
+    int route_size;
+
+    // Target Status
+    double target_speed;
+    int target_node;
+    int previous_node;
+    double delta;
+
+    // etc
+    double node_mindistance;
+    double resolution;
+
+    // Storage
+    std::vector<std::vector<double>> trajectory;
+    std::vector<int> target_nodes;
+
+    bool isNearby(std::vector<double> cpoint, std::vector<double> target_point); 
+    double pid_speed_control();
+    void update_target_node();
+    double stanley_steer_calc();
+    double pi_2_pi(double angle);
+    void update_state();
+};

algorithm/lib/utils/car_struct.h

@@ -0,0 +1,20 @@
+#pragma once
+
+struct Car {
+    // PID Config
+    double kp = 1.0;
+    double ki = 1.0;
+    double kd = 1.0;
+
+    // Vehicle Config
+    double RF = 3.3;    // [cm] distance from rear to vehicle front end of vehicle
+    double RB = 0.8;    // [cm] distance from rear to vehicle back end of vehicle
+    double W = 2.4;     // [cm] width of vehicle
+    double WD = 0.7*W;  // [cm] distance between left-right wheels
+    double WB = 2.5;    // [cm] Wheel base
+    double TR = 0.44;   // [cm] Tyre radius
+    double TW = 0.7;    // [cm] Tyre width
+
+    double MAX_STEER = 0.65;    // [rad] Maximum steering angle
+    double MAX_SPEED = 10.0;    // [cm/s] Maximum speed
+};

algorithm/src/controller/pure_pursuit.cpp

@@ -41,8 +41,8 @@ PurePursuit::PurePursuit(std::vector<std::vector<double>> route, double speed, s
 void PurePursuit::purepursuit_control() {
     while(this->target_node != this->route_size-1) {
         this->acceleration = this->pid_speed_control();
-        this->delta = this->purepursuit_steer_calc();
         this->target_node = this->update_target_node();
+        this->delta = this->purepursuit_steer_calc();
         this->update_state();
     }
 }

algorithm/src/controller/stanley.cpp

@@ -0,0 +1,148 @@
+#include "controller/stanley.hpp"
+
+Stanley::Stanley(std::vector<std::vector<double>> route, double resolution, double speed, std::vector<double> cpoint,
+                double k, double ks) {
+    // Vehicle Status
+    this->x = cpoint[0];
+    this->y = cpoint[1];
+    this->yaw = cpoint[2];
+    this->v = 0.0;
+    this->acceleration = 0.0;
+
+    this->front_x = this->x + this->WB*cos(this->yaw);
+    this->front_y = this->y + this->WB*sin(this->yaw);
+
+    // PID Config
+    // this->pid_integral = 0.0;
+    // this->previous_error = 0.0;
+    this->kp = this->car.kp;
+    // this->ki = this->car.ki;
+    // this->kd = this->car.kd;
+
+    // Vehicle Config
+    this->WB = this->car.WB/resolution;
+
+    // System Config
+    this->dt = 0.1;
+
+    // Stanley Parameters
+    this->k = k;
+    this->ks = ks;
+
+    // Path
+    this->route = route;
+    this->route_size = route.size();
+
+    // Target
+    this->target_speed = speed;
+    this->target_node = 0;
+    this->previous_node = 0;
+    this->delta = 0.0;
+
+    // etc
+    this->node_mindistance = 5.0/resolution;
+    this->resolution = resolution;
+}
+
+void Stanley::stanley_control() {
+    while(!isNearby({this->x, this->y}, this->route.back())) {
+        this->acceleration = this->pid_speed_control();
+        this->update_target_node();
+        this->delta = this->stanley_steer_calc();
+        this->update_state();
+    }
+}
+
+bool Stanley::isNearby(std::vector<double> cpoint, std::vector<double> target_point) {
+    double distance = std::pow(cpoint[0]-target_point[0], 2) + std::pow(cpoint[1]-target_point[1], 2);
+
+    return distance < this->node_mindistance;
+}
+
+double Stanley::pid_speed_control() {
+    // double error = target_speed - v;
+    // pid_integral += error * dt;
+    // double derivative = (error - previous_error)/dt;
+    // double acceleration = kp * error + ki * pid_integral + kd * derivative;
+    // previous_error = error;
+
+    double acceleration = kp * (target_speed - v);
+
+    return acceleration;
+}
+
+void Stanley::update_target_node() {
+    int closest_node = -1;
+    int second_closest_node = -1;
+    double min_distance = std::numeric_limits<double>::max();
+    double second_min_distance = std::numeric_limits<double>::max();
+
+    for (size_t i = 0; i < this->route_size; ++i) {
+        double dx = this->front_x - this->route[i][0];
+        double dy = this->front_y - this->route[i][1];
+        double distance = std::pow(dx, 2) + std::pow(dy, 2);
+
+        if (distance < min_distance) {
+            second_min_distance = min_distance;
+            second_closest_node = closest_node;
+            min_distance = distance;
+            closest_node = i;
+        } 
+        else if (distance < second_min_distance) {
+            second_min_distance = distance;
+            second_closest_node = i;
+        }
+    }
+
+    this->target_node = std::max(closest_node, second_closest_node);
+}
+
+
+double Stanley::stanley_steer_calc() {
+    double dx = this->front_x - route[this->target_node][0];
+    double dy = this->front_y - route[this->target_node][1];
+
+    double front_axle_vec_rot_90_x = cos(yaw - M_PI/2.0);
+    double front_axle_vec_rot_90_y = sin(yaw - M_PI/2.0);
+
+    double e = dx * front_axle_vec_rot_90_x + dy * front_axle_vec_rot_90_y;
+
+    double theta_e = pi_2_pi(route[target_node][2] - yaw);
+
+    double delta = theta_e + atan2(k*e, v+ks);
+
+    return delta;
+}
+
+double Stanley::pi_2_pi(double angle) {
+    while (angle > M_PI) angle -= 2.0 * M_PI;
+    while (angle < -M_PI) angle += 2.0 * M_PI;
+    return angle;
+}
+
+void Stanley::update_state() {
+    x += v * cos(yaw) * dt;
+    y += v * sin(yaw) * dt;
+    yaw += v / WB * tan(delta) * dt;
+    v += acceleration * dt;
+
+    front_x = x + WB*cos(yaw);
+    front_y = y + WB*sin(yaw);
+
+    trajectory.push_back({x, y});
+    target_nodes.push_back(target_node);
+}
+
+std::vector<std::vector<double>> Stanley::getTrajectory() {
+    // for (const auto& point : trajectory) {
+    //     std::cout << "x: " << point[0] << ", y: " << point[1] << std::endl;
+    // }
+    return trajectory;
+}
+
+std::vector<int> Stanley::getTargetnode() {
+    // for (const int& node : target_nodes) {
+    //     std::cout << "Target Node : " << node << std::endl;;
+    // }
+    return target_nodes;
+}

algorithm/src/main.cpp

@@ -3,6 +3,7 @@
 #include "hybrid-a-star/a_star.hpp"
 
 #include "controller/pure_pursuit.hpp"
+#include "controller/stanley.hpp"
 
 #include "utils/matplotlibcpp.h"
 #include "utils/loader.hpp"
@@ -48,16 +49,25 @@ int main() {
     std::vector<std::vector<double>> route = planner.get_route();
 
     std::vector<double> cpoint = {130.0, 64.0, 0};
-    double speed = 20.0;
+    double speed = 5.0/resolution;
     double WB = 16/resolution;
-    double Kdd = 1.8/resolution;
-    double Ldc = 5/resolution;
+    double Kdd = 5/resolution;
+    double Ldc = 8/resolution;
 
-    auto purepursuit = PurePursuit(route, speed, cpoint, WB, Kdd, Ldc);
-    purepursuit.purepursuit_control();
+    // auto purepursuit = PurePursuit(route, speed, cpoint, WB, Kdd, Ldc);
+    // purepursuit.purepursuit_control();
 
-    std::vector<std::vector<double>> control_path = purepursuit.getTrajectory();
-    std::vector<int> target_nodes = purepursuit.getTargetnode();
+    // std::vector<std::vector<double>> control_path = purepursuit.getTrajectory();
+    // std::vector<int> target_nodes = purepursuit.getTargetnode();
+
+    double k = 0.2;
+    double ks = 1.0;
+
+    auto stanley = Stanley(route, resolution, speed, cpoint, k, ks);
+    stanley.stanley_control();
+
+    std::vector<std::vector<double>> control_path = stanley.getTrajectory();
+    std::vector<int> target_nodes = stanley.getTargetnode();
 
     // // Only for drawing
     std::cout << "Draw Map" << std::endl;

algorithm/src/planner/hybrid_astar_planner.cpp

@@ -63,22 +63,22 @@ void Planner::plan_route(std::vector<std::array<int, 3>> waypoints) {
         bool found = a_star.createPath(plan, num_it, tolerance);
         if (!found) {std::cout<<"NO WAY"<<std::endl;}
 
-        std::vector<Eigen::Vector2d> smooth_path;
-        for (const auto& node : plan) {
-            smooth_path.push_back(Eigen::Vector2d(node.x, node.y));
-        }
-
-        if (!smoother->smooth(smooth_path, map, smoother_params)) {
-            std::cout << "Smoothing Fail" << std::endl;
-        }
+        // std::vector<Eigen::Vector2d> smooth_path;
+        // for (const auto& node : plan) {
+        //     smooth_path.push_back(Eigen::Vector2d(node.x, node.y));
+        // }
 
-        for (size_t i = smooth_path.size()-1; 0<i; --i) {
-            waypoints_route.push_back({smooth_path[i].x(), smooth_path[i].y(), plan[i].theta * M_PI / 180 * theta_resolution});
-        }
+        // if (!smoother->smooth(smooth_path, map, smoother_params)) {
+        //     std::cout << "Smoothing Fail" << std::endl;
+        // }
 
-        // for (size_t i = plan.size()-1; 0 < i; --i) {
-        //     waypoints_route.push_back({plan[i].x, plan[i].y, plan[i].theta * M_PI / 180 * theta_resolution});
+        // for (size_t i = smooth_path.size()-1; 0<i; --i) {
+        //     waypoints_route.push_back({smooth_path[i].x(), smooth_path[i].y(), plan[i].theta * M_PI / 180 * theta_resolution});
         // }
+
+        for (size_t i = plan.size()-1; 0 < i; --i) {
+            waypoints_route.push_back({plan[i].x, plan[i].y, plan[i].theta * M_PI / 180 * theta_resolution});
+        }
     }
 }