starting_point.cc

//
// Created by Gastone Pietro Rosati Papini on 10/08/22.
//

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <algorithm>

extern "C" {
#include "screen_print_c.h"
}
#include "screen_print.h"
#include "server_lib.h"
#include "logvars.h"

#include "primitives.h"
#include "rrt_star.h"
#include "Clothoids.hh"

#define DEFAULT_SERVER_IP    "127.0.0.1"
#define SERVER_PORT           30000             // Server port
#define DT                    0.05
#define M_SIZE                6                 // ignore m0, use m1-m5
#define ACC_MAX               5.0
#define ACC_MIN               -3.0
#define P_GAIN                0.002
#define I_GAIN                0.1
#define LNG_CTRL_LOG          "lng_ctrl_log"
#define OBS_LOG               "obs_log"
#define CLOTHOID_CURVES_LOG   "clothoid_curve_log"
#define STEERING_LOG          "steering_log"

// Handler for CTRL-C
#include <signal.h>
static uint32_t server_run = 1;

double j_eval(double t, const double m[] );
void free_flow(double v0,double a0,double xf,double *vr, double *m, double *T);
bool is_arr_zeros (const double m[], int size);
void action_selection(const input_data_str *in, double *m_star, std::string &action_msg);
void arr_copy(double *m_target, const double m[], int size);
void low_level_control(const input_data_str *in, const double *m_star, double &a_req, double &requested_pedal);
void
lateral_control(const input_data_str *in, const G2lib::ClothoidList &reference_path, double &requested_steering_angle);
void path_planning(uint32_t message_id, input_data_str *in, G2lib::ClothoidList &path_clothoids);

void intHandler(int signal) {
    server_run = 0;
}

int main(int argc, const char * argv[]) {
    logger.enable(true);

    // Messages variables
    scenario_msg_t scenario_msg;
    manoeuvre_msg_t manoeuvre_msg;
    size_t scenario_msg_size = sizeof(scenario_msg.data_buffer);
    size_t manoeuvre_msg_size = sizeof(manoeuvre_msg.data_buffer);
    uint32_t message_id = 0;

#ifndef _MSC_VER
    // More portable way of supporting signals on UNIX
    struct sigaction act;
    act.sa_handler = intHandler;
    sigaction(SIGINT, &act, NULL);
#else
    signal(SIGINT, intHandler);
#endif

    server_agent_init(DEFAULT_SERVER_IP, SERVER_PORT);

    // Start server of the Agent
    printLine();
    printTable("Waiting for scenario message...", 0);
    printLine();
    while (server_run == 1) {

        // Clean the buffer
        memset(scenario_msg.data_buffer, '\0', scenario_msg_size);

        // Receive scenario message from the environment
        if (server_receive_from_client(&server_run, &message_id, &scenario_msg.data_struct) == 0) {
            // Init time
            static auto start = std::chrono::system_clock::now();
            auto time = std::chrono::system_clock::now()-start;
            double num_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(time).count()/1000.0;
            printLogTitle(message_id, "received message");

            // Data struct
            input_data_str *in = &scenario_msg.data_struct;
            manoeuvre_msg.data_struct.CycleNumber = in->CycleNumber;
            manoeuvre_msg.data_struct.Status = in->Status;

            // (Offline) PATH PLANNING: RRT Star
            static G2lib::ClothoidList path_clothoids;

            if (path_clothoids.length() == 0) {
                path_planning(message_id, in, path_clothoids);
            }

            // LATERAL CONTROL: clothoid-based
            double requested_steering_angle = 0;
            lateral_control(in, path_clothoids, requested_steering_angle);

            // ACTION SELECTION: using the inhibition mechanism and the mirroring, the best action is chosen
            std::string action_msg;
            double m_star[M_SIZE]; // best primitive

            action_selection(in, m_star, action_msg);

            // LOW LEVEL CONTROL: used to generate the requested acceleration
            double a_req;
            double requested_pedal;

            low_level_control(in, m_star, a_req, requested_pedal);

            // Manoeuvre message
//            manoeuvre_msg.data_struct.RequestedAcc = a_req; // no PID control
            manoeuvre_msg.data_struct.RequestedAcc = requested_pedal;
            manoeuvre_msg.data_struct.RequestedSteerWhlAg = requested_steering_angle;

            // log for longitudinal control
            logger.log_var(LNG_CTRL_LOG, "Time", num_seconds);
            logger.log_var(LNG_CTRL_LOG, "CycleNumber", in->CycleNumber);
            logger.log_var(LNG_CTRL_LOG, "TrfLightCurrState", in->TrfLightCurrState);
            logger.log_var(LNG_CTRL_LOG, "Dist", -in->TrfLightDist);
            logger.log_var(LNG_CTRL_LOG, "Vel", in->VLgtFild);
            logger.log_var(LNG_CTRL_LOG, "Acc", in->ALgtFild);
            logger.log_var(LNG_CTRL_LOG, "RequestedAcc", a_req);
            logger.log_var(LNG_CTRL_LOG, "RequestedPedal", requested_pedal);
            logger.log_var(LNG_CTRL_LOG,"Action",action_msg);

            // Write log
            logger.write_line(LNG_CTRL_LOG);

            // Screen print
            printLogVar(message_id, "Time", num_seconds);
            printLogVar(message_id, "Status", in->Status);
            printLogVar(message_id, "CycleNumber", in->CycleNumber);
            printLogVar(message_id, "TrfLightCurrState", in->TrfLightCurrState);
            printLogVar(message_id, "TrfLightDist", in->TrfLightDist);
            printLogVar(message_id, "VLgtFild", in->VLgtFild);
            printLogVar(message_id, "CurrentLane", in->CurrentLane);

            // Send manoeuvre message to the environment
            if (server_send_to_client(server_run, message_id, &manoeuvre_msg.data_struct) == -1) {
                perror("error send_message()");
                exit(EXIT_FAILURE);
            } else {
                printLogTitle(message_id, "sent message");
            }
        }
    }

    // Close the server of the agent
    server_agent_close();
    return 0;
}

void path_planning(uint32_t message_id, input_data_str *in, G2lib::ClothoidList &path_clothoids) {
    int max_step = 1;
    int max_iter = 10000;
    int radius = 10;
    double x_max = 182 + 20; // target+lookahead
    double x_min = 0;
    double y_min = in->LatOffsLineL;
    double y_max = in->LatOffsLineL + 2;

    RRT_Star planner = RRT_Star(max_step, max_iter, x_min, y_min, x_max, y_max, radius);
    std::vector<Point> path_points;

    printLogTitle(message_id, "RRT_Star: Initialize obstacles");
    int* objIdArr = reinterpret_cast<int*>(in->ObjID);
    double* objXArr = reinterpret_cast<double*>(in->ObjX);
    double* objYArr = reinterpret_cast<double*>(in->ObjY);
    double* objLenArr = reinterpret_cast<double*>(in->ObjLen);
    double* objWidArr = reinterpret_cast<double*>(in->ObjWidth);

    for (int i = 0; i < in->NrObjs; ++i) {
        logger.log_var(OBS_LOG, "ObjID", objIdArr[i]);
        logger.log_var(OBS_LOG, "ObjX", objXArr[i]);
        logger.log_var(OBS_LOG, "ObjY",  objYArr[i] + y_min);
        logger.log_var(OBS_LOG, "ObjLen", objLenArr[i]);
        logger.log_var(OBS_LOG, "ObjWidth", objWidArr[i]);
        logger.write_line(OBS_LOG);

        planner.add_obstacle(objIdArr[i], objXArr[i], objYArr[i] + y_min, objLenArr[i], objWidArr[i]);
    }

    printLogTitle(message_id, "RRT_Star: Generate Path");
    Point p_start = {x_min, y_min};
    Point p_goal = {x_max, y_min};
    path_points = planner.plan_route(p_start, p_goal);

//    // test
//    path_points.push_back({x_min,y_min});
//    path_points.push_back({40,y_min});
//    path_points.push_back({55,y_min + 2});
//    path_points.push_back({60,y_min});
//    path_points.push_back({200,y_min});

    // Build Clothoid curves from points
    for (int i = 0; i < path_points.size() - 1; ++i) {
        G2lib::real_type x0 = path_points[i].x;
        G2lib::real_type y0 = path_points[i].y;
        G2lib::real_type theta0 = 0;
        G2lib::real_type x1 = path_points[i+1].x;
        G2lib::real_type y1 = path_points[i+1].y;
        G2lib::real_type theta1 = 0;

        // build a curve by solving G1 problem and add to the list
        path_clothoids.push_back_G1(x0,y0,theta0,x1,y1,theta1);
    }
    printLogTitle(message_id, "RRT_Star: Clothoid path built");
    // log clothoid path
//                for (int s = 0; s < path_clothoids.length(); ++s) {
//                    double x;
//                    double y;
//                    path_clothoids.eval(s,x,y);
//                    logger.log_var(CLOTHOID_CURVES_LOG, "x", x);
//                    logger.log_var(CLOTHOID_CURVES_LOG, "y", y);
//                    logger.log_var(CLOTHOID_CURVES_LOG, "s", s);
//                    logger.write_line(CLOTHOID_CURVES_LOG);
//                }
}

void
lateral_control(const input_data_str *in, const G2lib::ClothoidList &reference_path, double &requested_steering_angle) {
    static double x_0 = in->TrfLightDist; //162
    // current car position
    G2lib::real_type x_car= x_0 - in->TrfLightDist;
    G2lib::real_type y_car= in->LatOffsLineL;
    // output from closest_point_ISO
    G2lib::real_type x_p;
    G2lib::real_type y_p;
    G2lib::real_type s_p; // curvilinear abscissa (distance along the curve)
    G2lib::real_type t_p; // curvilinear coordinate
    G2lib::real_type dist_p; // distance point projected point

    // find the closest point on path
    reference_path.closest_point_ISO(x_car, y_car, x_p, y_p, s_p, t_p, dist_p);

    // find lookahead point (x,y) at "s" on clothoid path
    double clothoid_lookahead = 5;
    G2lib::real_type x_lookahead;
    G2lib::real_type y_lookahead;
    reference_path.eval(s_p + clothoid_lookahead, x_lookahead, y_lookahead);

    //Build a clothoid curve from current position to target point
    G2lib::ClothoidCurve curve;
    G2lib::real_type theta_lookahead = reference_path.theta(s_p + clothoid_lookahead);
    G2lib::real_type heading_angle = in->LaneHeading;
    curve.build_G1(x_car, y_car, heading_angle, x_lookahead, y_lookahead, theta_lookahead);

    // compute steering angle
    double k_s = curve.kappa_begin(); // initial curvature
    double L = in->VehicleLen; // car's length
    double K_us = 0.15; // 0 = neutral steering
    double u = in->VLgtFild; // forward speed
    requested_steering_angle= k_s * (L + K_us * pow(u, 2));// initial distance

    logger.log_var(STEERING_LOG, "car_x", x_car);
    logger.log_var(STEERING_LOG, "car_y", y_car);
    logger.log_var(STEERING_LOG, "ref_x", x_p);
    logger.log_var(STEERING_LOG, "ref_y", y_p);
    logger.log_var(STEERING_LOG, "heading_angle", in->LaneHeading);
    logger.log_var(STEERING_LOG, "req_steering_angle", requested_steering_angle);
    logger.log_var(STEERING_LOG, "act_steering_angle", in->SteerWhlAg);
    logger.write_line(STEERING_LOG);
}

void low_level_control(const input_data_str *in, const double *m_star, double &a_req, double &requested_pedal) {
    // integrated jerk - trapezoidal with internal a0
    double a0 =  in->ALgtFild;
    static double a0_int = a0;

    a_req= a0_int + 0.5 * DT * (j_eval(0, m_star) + j_eval(DT, m_star));
    // update internal a0
    a0_int = a_req;

    // include saturation
    a_req = std::min(std::max(a_req,ACC_MIN),ACC_MAX);

    // PID Control
    double e = a_req - a0;
    static double e_int = 0.0; // integral of err

    requested_pedal= e * P_GAIN + e_int * I_GAIN;
    e_int += e * DT;

    // address the delay in restart the car when traffic light changes from red->green
    double jerk =j_eval(0, m_star);
    if (in->VLgtFild < 0.001 && jerk > 0.1 && a_req < 0) {
        a0_int = 0.0;
        e_int = 0.0;
    }
}

void action_selection(const input_data_str *in, double *m_star, std::string &action_msg) {
    double a0 = in->ALgtFild;
    double v0 = in->VLgtFild;
    double lookahead = std::max(50.0, v0 * 5);
    double v_min = 3.0;
    double v_max = 15.0;
    double v_r = in->RequestedCruisingSpeed;
    double x_s = 5.0;
    double T_s = x_s/v_min;
    double x_in = 10.0;
    double T_in = x_in/v_min;
    double x_tr = 0.0;
    double x_stop = 0.0;
    double m1[M_SIZE];
    double m2[M_SIZE];
    double T1 = 0.0;
    double T2 = 0.0;
    double T_green = 0.0;
    double T_red = 0.0;
    double x_max = 0.0;
    if (in->NrTrfLights != 0) {
        x_tr = in->TrfLightDist;
        x_stop = in->TrfLightDist - x_s/2;
    }
    if (in->NrTrfLights == 0 || x_tr >= lookahead) { // far from traffic light or no traffic light
        free_flow(v0, a0, lookahead, &v_r, m_star, &T1);
        action_msg = "action: free flow (no traffic light)";
    } else {
        switch (in->TrfLightCurrState) {
            case 1: { // green
                T_green = 0;
                T_red = in->TrfLightFirstTimeToChange - T_in;
                break;
            }
            case 2: { // yellow
                T_green = in->TrfLightSecondTimeToChange + T_s;
                T_red = in -> TrfLightThirdTimeToChange - T_in;
                break;
            }
            case 3: { // red
                T_green = in->TrfLightFirstTimeToChange + T_s;
                T_red = in -> TrfLightSecondTimeToChange - T_in;
                break;
            }
            default:
                break;
        }
        if (in->TrfLightCurrState == 1 && in->TrfLightDist <= x_s) {
            free_flow(v0, a0, lookahead, &v_r, m_star, &T1);
            action_msg = "action: free flow (green light)";
        } else {
            pass_primitive(a0,v0,x_tr, &v_min, &v_max, T_green, T_red, m2, &T2, m1, &T1);
            if (is_arr_zeros(m1, M_SIZE) && is_arr_zeros(m2, M_SIZE)) { // check m=0 (no solution)
                stop_primitive(a0, v0, x_stop, m_star, &x_max, &T1);
                action_msg = "action: stop";
            } else {
                if ((m1[3] < 0 && m2[3] > 0) || m1[3] > 0 && m2[3] <0) { // check j0
                    pass_primitive_j0(a0, v0, x_stop, v_min, v_max, m_star);
                    action_msg = "action: pass (j0)";
                } else {
                    if (std::abs(m1[3]) < std::abs(m2[3])) { // check smaller jerk
                        arr_copy(m_star, m1, M_SIZE); // m_star = m1
                        action_msg = "action: pass (1)";
                    } else {
                        arr_copy(m_star, m2, M_SIZE); // m_star = m2
                        action_msg = "action: pass (2)";
                    }
                }
            }
        }
    }
}

void free_flow(double v0,double a0,double xf,double *vr, double *m, double *T){
    return pass_primitive(a0,v0, xf,vr,vr,0,0, m, T, m, T);
}

bool is_arr_zeros (const double m[], int size) {
    for (int i = 0; i < size; ++i) {
        if (m[i] != 0) {
            return false;
        }
    }
    return true;
}

void arr_copy(double *m_target, const double m[], int size) {
    for (int i = 0; i < size; ++i) {
        m_target[i] = m[i];
    }
}

double j_eval(double t, const double m[] ) {
    double j_t = 0.0;
    j_t = m[3] + m[4]*t + 1.0/2 * m[5] * std::pow(t, 2); // j_opt solution
    return j_t;
}