Path-Planning/src/TrajectoryPlanner.cpp at master · aboerzel/Path-Planning · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
#include "TrajectoryPlanner.h"
#include "Point.h"
#include "LaneConverter.h"
#include "helpers.h"
#include "PointConverter.h"
#include "spline.h"
#include "SpeedConverter.h"
#include "GlobalSettings.h"

// parameter to adjust path planning
#define WAYPOINT_DISTANCE 48    // distance between the waypoints of the calculated path
#define WAYPOINT_COUNT 4        // number of new waypoints to calculate
#define PATH_LENGTH 50          // number of waypoints of the path

TrajectoryPlanner::TrajectoryPlanner()
{
    ref_speed = 0.0; // reference speed [m/s]
}

TrajectoryPlanner::~TrajectoryPlanner()
= default;

vector<Point> TrajectoryPlanner::generate_trajectory(const vector<double>& previous_path_x,
                                                     const vector<double>& previous_path_y,
                                                     const vector<double>& map_waypoints_s,
                                                     const vector<double>& map_waypoints_x,
                                                     const vector<double>& map_waypoints_y,
                                                     const vector<vector<double>>& sensor_fusion,
                                                     double car_x, double car_y, double car_yaw, double car_speed,
                                                     double car_s, double car_d)
{
    vector<Point> trajectory;

    int prev_path_size = previous_path_x.size();

    // start with previous path
    for (auto i = 0; i < prev_path_size; i++)
    {
        trajectory.emplace_back(previous_path_x[i], previous_path_y[i]);
    }

    double ref_x;
    double ref_y;
    double ref_yaw;

    vector<Point> path_points;

    if (prev_path_size < 2)
    {
        ref_x = car_x;
        ref_y = car_y;
        ref_yaw = deg2rad(car_yaw);

        auto prev_car_x = car_x - cos(car_yaw);
        auto prev_car_y = car_y - sin(car_yaw);

        path_points.emplace_back(prev_car_x, prev_car_y);
        path_points.emplace_back(car_x, car_y);

        // initialize reference speed [m/s] with current car speed [MPS], to get a smooth velocity
        ref_speed = SpeedConverter::miles_per_hour_to_m_per_sec(car_speed);
    }
    else
    {
        ref_y = previous_path_y[prev_path_size - 1];
        ref_x = previous_path_x[prev_path_size - 1];

        auto ref_x_prev = previous_path_x[prev_path_size - 2];
        auto ref_y_prev = previous_path_y[prev_path_size - 2];

        ref_yaw = atan2(ref_y - ref_y_prev, ref_x - ref_x_prev);

        path_points.emplace_back(ref_x_prev, ref_y_prev);
        path_points.emplace_back(ref_x, ref_y);
    }

    auto current_lane = LaneConverter::d_to_lane(car_d);

    printf("%-32s: %4.2f m/s (%4.2f MPS)\n", "current speed", ref_speed,
           SpeedConverter::km_per_sec_to_miles_per_hour(ref_speed));
    printf("%-32s: %d\n", "current lane", current_lane);
    printf("%-32s: %4.2f\n", "current d", car_d);

    highway_driving_behavior.update(car_s, current_lane, SpeedConverter::miles_per_hour_to_m_per_sec(car_speed), sensor_fusion);

    auto target_d = LaneConverter::lane_to_d(highway_driving_behavior.target_lane);

    printf("%-32s: %4.2f m/s (%4.2f MPS)\n", "target speed", highway_driving_behavior.target_speed,
           SpeedConverter::km_per_sec_to_miles_per_hour(highway_driving_behavior.target_speed));
    printf("%-32s: %d\n", "target lane", highway_driving_behavior.target_lane);
    printf("%-32s: %4.2f\n", "target d", target_d);

    // add new waypoints to following the desired lane
    for (auto i = 1; i <= WAYPOINT_COUNT; i++)
    {
        auto wp = getXY(car_s + (i * WAYPOINT_DISTANCE), target_d, map_waypoints_s, map_waypoints_x, map_waypoints_y);
        path_points.emplace_back(wp);
    }

    // convert path points from map coordinates to vehicle coordinates
    for (auto& path_point : path_points)
    {
        path_point = PointConverter::map_to_vehicle_coordinates(path_point, Point(ref_x, ref_y), ref_yaw);
    }

    // create spline from path points
    tk::spline spline;
    spline.set_points(get_x_values(path_points), get_y_values(path_points));

    auto target_x = WAYPOINT_DISTANCE;
    auto target_y = spline(target_x);
    auto target_dist = sqrt(pow(target_x, 2) + pow(target_y, 2));

    auto x_offset = 0.0;
    auto speed_diff = highway_driving_behavior.target_accel * DT; // speed difference per time interval DT [m/s]

    // create new path points and add them to the new path
    for (auto i = 0; i < PATH_LENGTH - prev_path_size; i++)
    {
        // accelerate / decelerate; ensure target speed
        if (ref_speed < highway_driving_behavior.target_speed - speed_diff)
        {
            ref_speed += speed_diff;
        }
        else if (ref_speed > highway_driving_behavior.target_speed + speed_diff)
        {
            ref_speed -= speed_diff;
        }

        // calculate points along new path
        auto delta_s = ref_speed * DT; // distance per time interval DT [m]
        auto x = x_offset + target_x * delta_s / target_dist;
        auto y = spline(x);

        x_offset = x;

        // convert back to map coordinates
        auto p = PointConverter::vehicle_to_map_coordinates(Point(x, y), Point(ref_x, ref_y), ref_yaw);

        trajectory.push_back(p);
    }

    return trajectory;
}