Skip to content

Commit

Permalink
#3 PurePursuit with pathplanning
Browse files Browse the repository at this point in the history
  • Loading branch information
@dongdongO
dongdongO committed Mar 21, 2024
1 parent 43cd8ed commit 922f15a
Show file tree
Hide file tree
Showing 4 changed files with 128 additions and 107 deletions.
2 changes: 1 addition & 1 deletion algorithm/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 3.5)

project(PathPlanning VERSION 1.0 LANGUAGES CXX)
project(Control VERSION 1.0 LANGUAGES CXX)

# Set the C++ standard to C++20
set(CMAKE_CXX_STANDARD 20)
Expand Down
192 changes: 91 additions & 101 deletions algorithm/controller/control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,164 +3,169 @@
#include <array>
#include <cmath>
#include <iostream>
#include <unistd.h>

class PurePursuit {

public:
PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double k, double Lfc);
PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double Kdd, double Ldc);

void purepursuit_control();

std::vector<std::vector<int>> getTrajectory();
std::vector<int> getTargetnode();

private:
// State
// Vehicle State
int x;
int y;
double yaw;
double v;
double acceleration;

int rear_x;
int rear_y;
double WB;

// PID
double pid_integral;
double dt;
double previous_error;
double kp;
double ki;
double kd;
// double ki;
// double kd;

double dt;

// node searching
int old_nearest_node;
double LF;
double k;
double Lfc;
// Target Node Searching
double Ld;
double Kdd;
double Ldc;

// purepursuit
double delta;

// Path
std::vector<std::vector<int>> route;
int route_size;

// Target Status
double target_speed;
int target_node;

std::vector<std::vector<int>> trajectory;
std::vector<int> target_nodes;

double pid_speed_control();
int search_target_node();
double purepursuit_steer_calc();
int update_target_node();
void update_state();
double calc_distance(int point_x, int point_y);
void purepursuit_steer_calc();
void update_state(double acceleration);
};

PurePursuit::PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double k, double Lfc) {
// Path
this->route = route;

this->target_speed = speed;
this->target_node = 0;
this->WB = WB;

PurePursuit::PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double Kdd, double Ldc) {
// Vehicle Status Init
this->x = cpoint[0];
this->y = cpoint[1];
this->yaw = cpoint[2];
this->v = 0.0;
this->rear_x = cpoint[0] - ((this->WB/2) * cos(cpoint[2]));
this->rear_y = cpoint[1] - ((this->WB/2) * sin(cpoint[2]));
this->acceleration = 0.0;

this->WB = WB;
this->rear_x = this->x - ((WB / 2) * cos(this->yaw));
this->rear_y = this->y - ((WB / 2) * sin(this->yaw));

// PID Init
this->pid_integral = 0.0;
this->dt = 0.1;
this->previous_error = 0.0;
this->kp = 1;
this->ki = 0.1;
this->kd = 0.01;
// this->ki = 0.1;
// this->kd = 0.01;

this->dt = 0.1;

// Target Node Searching Init
this->Ldc = Ldc;
this->Kdd = Kdd;
this->Ld = this->Kdd * this->v + this->Ldc;

this->old_nearest_node = 0;
this->LF = 0.0;
this->k = k;
this->Lfc = Lfc;
// PurePursuit Init
this->delta = 0.0;

// Path Init
this->route = route;
this->route_size = route.size();

// Target Status Init
this->target_speed = speed;
this->target_node = 0;
}

void PurePursuit::purepursuit_control() {
double accerlation = 0.0;
while(this->target_node != this->route.size()) {
accerlation = this->pid_speed_control();
// std::cout << "A : " << x << std::endl;
this->purepursuit_steer_calc();
this->update_state(accerlation);
// std::cout << "target node : " << this->x << std::endl;
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->update_state();
}
}

int PurePursuit::search_target_node() {
int cnode = this->target_node;
double distance_this_node = this->calc_distance(this->route[cnode][0], this->route[cnode][1]);
double distance_next_node = 0.0;

while (cnode + 1 < this->route.size()) {
distance_next_node = this->calc_distance(this->route[cnode + 1][0], this->route[cnode + 1][1]);
if (distance_this_node < distance_next_node) {
break;
}
std::cout << "test" << std::endl;
cnode += 1;
distance_this_node = distance_next_node;
}
double PurePursuit::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;

this->target_node = cnode; // 현재 가장 가까운 노드 업데이트
double acceleration = kp * (target_speed - v);

double Lf = this->k * this->v + this->Lfc; // 전방 주시 거리 업데이트
return acceleration;
}

while (cnode + 1 < this->route.size() && Lf > this->calc_distance(this->route[cnode][0], this->route[cnode][1])) {
std::cout << "test" << std::endl;
cnode += 1;
}
double PurePursuit::purepursuit_steer_calc() {
int tx = route[this->target_node][0];
int ty = route[this->target_node][1];

std::cout << "old_nearest_node : " << this->target_node << std::endl;
std::cout << "cnode : " << v << std::endl;
double alpha = atan2(ty - this->rear_y, tx - this->rear_x) - this->yaw;
double delta = atan2(2.0 * this->WB * sin(alpha)/this->Ld, 1.0);

return cnode;
return delta;
}

void PurePursuit::purepursuit_steer_calc() {
// int cnode = this->search_target_node();
int tx = -1;
int ty = -1;
int PurePursuit::update_target_node() {
int cnode = this->target_node;
if(cnode == this->route_size-1) { return cnode; }

int new_target_node = this->search_target_node();
if (this->target_node != new_target_node) {
this->target_node = new_target_node; // target_node 업데이트
}
double distance_cnode = this->calc_distance(this->route[cnode][0], this->route[cnode][1]);
double distance_nnode = 0.0;

if(new_target_node < route.size()) {
tx = route[new_target_node][0];
ty = route[new_target_node][1];
while (cnode < this->route.size()-1) {
distance_nnode = this->calc_distance(this->route[cnode + 1][0], this->route[cnode + 1][1]);
if (distance_cnode < distance_nnode) {
break;
}
cnode += 1;
distance_cnode = distance_nnode;
}
else {
tx = route[-1][0];
ty = route[-1][0];
new_target_node = route.size()-1;

this->Ld = this->Kdd * this->v + this->Ldc;

if(cnode < this->route_size-1) {
while(this->Ld > this->calc_distance(this->route[cnode][0], this->route[cnode][1])) {
cnode += 1;
}
}

double alpha = atan2(ty - this->rear_y, tx - this->rear_x) - this->yaw;
this->delta = atan2(2.0 * this->WB * sin(alpha)/this->LF, 1.0);
return cnode;
}

void PurePursuit::update_state(double acceleration) {
void PurePursuit::update_state() {
x += v * cos(yaw) * dt;
y += v * sin(yaw) * dt;
yaw += v / WB * tan(delta) * dt;
v += acceleration * dt;

double newX = x + v * cos(yaw) * dt;
double newY = y + v * sin(yaw) * dt;
x = static_cast<int>(round(newX));
y = static_cast<int>(round(newY));
yaw += v/WB * tan(delta)*dt;
v += acceleration*dt;
rear_x = x - ((WB/2) * cos(yaw));
rear_y = y - ((WB/2) * sin(yaw));
rear_x = x - ((WB / 2) * cos(yaw));
rear_y = y - ((WB / 2) * sin(yaw));

trajectory.push_back({x, y});
target_nodes.push_back(target_node);
Expand All @@ -173,21 +178,6 @@ double PurePursuit::calc_distance(int point_x, int point_y) {
return sqrt(dx * dx + dy * dy);
}


double PurePursuit::pid_speed_control() {
double error = target_speed - v;

pid_integral += error * dt;

double derivative = (error - previous_error)/dt;

double accerlation = kp * error + ki * pid_integral + kd * derivative;

previous_error = error;

return accerlation;
}

std::vector<std::vector<int>> PurePursuit::getTrajectory() {
return trajectory;
}
Expand Down
7 changes: 6 additions & 1 deletion algorithm/globalmap/waypoints.txt
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,7 @@
1
2
2
4
6
7
9
16
34 changes: 30 additions & 4 deletions algorithm/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,13 @@ int main() {
planner.plan_with_waypoints(waypoints);
std::vector<std::vector<int>> route = planner.get_waypoints_path();

std::vector<int> cpoint = {130, 64};
std::vector<int> cpoint = {130, 64, 0};

auto purepursuit = PurePursuit(route, 10.0, cpoint, 2.7, 0.1, 2.0);
auto purepursuit = PurePursuit(route, 20.0, cpoint, 2.7, 0.5, 4.0);
purepursuit.purepursuit_control();

std::vector<std::vector<int>> control_path = purepursuit.getTrajectory();
std::vector<int> target_nodes = purepursuit.getTargetnode();

// Only for drawing
std::cout << "Draw Map" << std::endl;
Expand Down Expand Up @@ -78,8 +79,33 @@ int main() {
path_y.push_back(waypoint[1]);
}

plt::imshow(&image_data[0], map_height, map_width, 1);
plt::plot(path_x, path_y, "r-");
for(size_t i = 0; i < control_path.size(); i += 5) {

plt::clf();
plt::imshow(&image_data[0], map_height, map_width, 1);

// Show path
plt::plot(path_x, path_y, "r-");

// Show Control
if(i < control_path.size()) {
std::vector<int> path_x = {control_path[i][0]};
std::vector<int> path_y = {control_path[i][1]};
plt::plot(path_x, path_y, "bo");
}

// if(i < target_nodes.size()) {
// int target_node_index = target_nodes[i];
// if(target_node_index < route.size()) {
// std::vector<int> target_x = {route[target_node_index][0]};
// std::vector<int> target_y = {route[target_node_index][1]};
// plt::plot(target_x, target_y, "go"); // 목표 노드는 초록색으로
// }
// }

plt::pause(0.01);
}

plt::show();

return 0;
Expand Down

0 comments on commit 922f15a

Please sign in to comment.