-
Notifications
You must be signed in to change notification settings - Fork 94
/
trajectory_optimizer.h
56 lines (39 loc) · 1.7 KB
/
trajectory_optimizer.h
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
/***********************************************************************************
* C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on
* Frenet Frame: A Cartesian-based Trajectory Planning Method".
***********************************************************************************
* Copyright (C) 2022 Bai Li
* Users are suggested to cite the following article when they use the source codes.
* Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on
* Frenet Frame: A Cartesian-based Trajectory Planning Method",
* IEEE Transactions on Intelligent Transportation Systems, 2022.
***********************************************************************************/
#pragma once
#include "trajectory_nlp.h"
#include "math/aabox2d.h"
#include "math/polygon2d.h"
#include "discretized_trajectory.h"
#include "cartesian_planner_config.h"
namespace cartesian_planner {
using math::Polygon2d;
using math::AABox2d;
using math::Box2d;
class TrajectoryOptimizer {
public:
TrajectoryOptimizer(const CartesianPlannerConfig &config, const Env &env);
bool OptimizeIteratively(const DiscretizedTrajectory &coarse, const Constraints &constraints, States &result);
private:
CartesianPlannerConfig config_;
Env env_;
VehicleParam vehicle_;
TrajectoryNLP nlp_;
void CalculateInitialGuess(States &states) const;
bool FormulateCorridorConstraints(States &states, Constraints &constraints);
bool GenerateBox(double time, double &x, double &y, double radius, AABox2d &result) const;
inline bool CheckCollision(double time, double x, double y, const AABox2d &bound) const {
Box2d box(bound);
box.Shift({x, y});
return env_->CheckCollision(time, box);
}
};
}