├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── algorithm ├── ilqr │ ├── barrier_function.h │ ├── corridor.cc │ ├── corridor.h │ ├── ilqr_optimizer.cc │ ├── ilqr_optimizer.h │ ├── tracker.cc │ ├── tracker.h │ ├── vehicle_model.cc │ └── vehicle_model.h ├── math │ ├── aabox2d.cpp │ ├── aabox2d.h │ ├── box2d.cpp │ ├── box2d.h │ ├── line_segment2d.cpp │ ├── line_segment2d.h │ ├── linear_quadratic_regulator.cc │ ├── linear_quadratic_regulator.h │ ├── math_utils.cpp │ ├── math_utils.h │ ├── polygon2d.cpp │ ├── polygon2d.h │ ├── pose.h │ ├── vec2d.cpp │ └── vec2d.h ├── params │ ├── planner_config.h │ └── vehicle_param.h ├── planner │ ├── dp_planner.cpp │ ├── dp_planner.h │ ├── trajectory_planner.cpp │ └── trajectory_planner.h ├── planning_node.cc ├── planning_node.h ├── slover │ ├── constraint.h │ ├── cost.h │ ├── dynamics.h │ ├── function.h │ ├── ilqr.h │ ├── knot_point.h │ └── typedefs.h ├── utils │ ├── discrete_points_math.cc │ ├── discrete_points_math.h │ ├── discretized_trajectory.cpp │ ├── discretized_trajectory.h │ ├── environment.cpp │ ├── environment.h │ └── timer.h └── visualization │ ├── color.cpp │ ├── color.h │ ├── figure_plot.h │ ├── matplotlibcpp.h │ ├── plot.cpp │ └── plot.h ├── cmake └── FindHSL.cmake ├── config └── config.rviz ├── launch ├── pedestrian_test.launch └── random_pedestrian_test.launch ├── main.cc ├── msg ├── CenterLine.msg ├── CenterLinePoint.msg ├── DynamicObstacle.msg ├── DynamicObstacles.msg ├── DynamicTrajectoryPoint.msg └── Obstacles.msg ├── package.xml ├── resources ├── cost.png ├── demo.png ├── demo.webm ├── iter_results.png └── results.png └── script ├── pickle_publisher.py ├── reference_publisher.py └── run.sh /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | .DS_Store 3 | *.pickle 4 | .vscode/ 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(planning) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_BUILD_TYPE Debug) 6 | 7 | add_compile_options(-std=c++14) 8 | 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2 -std=c++14") 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | message_generation 14 | geometry_msgs 15 | ) 16 | 17 | find_package(Eigen3 REQUIRED) 18 | find_package(OpenCV REQUIRED) 19 | find_package(PythonLibs 2.7) 20 | 21 | #指定库路径 22 | #file(GLOB_RECURSE Python2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 23 | #指定头文件路径 24 | set(Python2.7_INLCUDE_DIRS 25 | "/usr/include/python2.7" 26 | ) 27 | 28 | add_message_files( 29 | FILES 30 | CenterLinePoint.msg 31 | CenterLine.msg 32 | DynamicObstacle.msg 33 | DynamicObstacles.msg 34 | Obstacles.msg 35 | DynamicTrajectoryPoint.msg 36 | ) 37 | 38 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 39 | 40 | catkin_package( 41 | LIBRARIES planning 42 | CATKIN_DEPENDS roscpp message_runtime 43 | ) 44 | 45 | include_directories( 46 | ${CMAKE_CURRENT_SOURCE_DIR} 47 | ${catkin_INCLUDE_DIRS} 48 | ${EIGEN3_INCLUDE_DIR} 49 | ${Python2.7_INLCUDE_DIRS} 50 | ) 51 | 52 | option(WITH_HSL "Compile solver with coinhsl" ON) 53 | 54 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 55 | find_package(HSL) 56 | if(NOT ${HSL_FOUND}) 57 | set(WITH_HSL OFF) 58 | endif() 59 | 60 | if(WITH_HSL) 61 | add_definitions(-DWITH_HSL) 62 | endif() 63 | 64 | set(SRC 65 | algorithm/planning_node.cc 66 | 67 | algorithm/math/aabox2d.cpp 68 | algorithm/math/box2d.cpp 69 | algorithm/math/line_segment2d.cpp 70 | algorithm/math/math_utils.cpp 71 | algorithm/math/polygon2d.cpp 72 | algorithm/math/vec2d.cpp 73 | algorithm/math/linear_quadratic_regulator.cc 74 | 75 | algorithm/planner/dp_planner.cpp 76 | algorithm/planner/trajectory_planner.cpp 77 | 78 | algorithm/utils/discretized_trajectory.cpp 79 | algorithm/utils/environment.cpp 80 | algorithm/utils/discrete_points_math.cc 81 | 82 | algorithm/visualization/plot.cpp 83 | algorithm/visualization/color.cpp 84 | 85 | algorithm/ilqr/corridor.cc 86 | algorithm/ilqr/ilqr_optimizer.cc 87 | algorithm/ilqr/vehicle_model.cc 88 | algorithm/ilqr/tracker.cc) 89 | 90 | add_executable(${PROJECT_NAME}_node main.cc ${SRC}) 91 | 92 | set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 93 | 94 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 95 | 96 | target_link_libraries(${PROJECT_NAME}_node 97 | ${catkin_LIBRARIES} 98 | ${OpenCV_LIBS} 99 | ${PYTHON_LIBRARIES} 100 | ) 101 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Trajectory Planner ROS Package 2 | 3 | C++/ROS Source Codes for Trajectory planning for autonomous driving using ilqr solver. 4 | 5 | ![demo](./resources/demo.png) 6 | ## 1. Installation 7 | 8 | Requirements 9 | 10 | * ROS Melodic or later 11 | * Python3 12 | * Eigen 3.4.0 13 | 14 | Clone repository to any catkin workspace and compile workspace 15 | 16 | ```shell 17 | cd ~/catkin_ws/src 18 | git clone https://github.com/mpt0816/Cilqr.git 19 | cd .. 20 | catkin_make 21 | source devel/setup.bash 22 | ``` 23 | ## 2. Example 24 | 25 | https://github.com/mpt0816/Cilqr/blob/master/resources/demo.webm 26 | 27 | ### 2.1 Costs of iterations 28 | ![demo](./resources/cost.png) 29 | 30 | ### 2.2 Trajectory of iterations 31 | ![demo](./resources/iter_results.png) 32 | 33 | ### 2.3 Final Trajectory 34 | ![demo](./resources/results.png) 35 | 36 | ## 3. Run 37 | Example test case with 6 pedestrians, 3 moving vehicles and 2 static vehicles. 38 | 39 | ```shell 40 | roslaunch planning pedestrian_test.launch 41 | ``` 42 | 43 | or 44 | ```shell 45 | cd ~/catkin_ws/src 46 | bash src/Cilqr/scripts/run.sh 47 | ``` 48 | 49 | **Click anywhere in Rviz window with the `2D Nav Goal` Tool to start planning.** 50 | 51 | Generate and run new random case: 52 | 53 | ```shell 54 | roslaunch planning random_pedestrian_test.launch 55 | ``` 56 | 57 | - Red Trajectory: Coarse Trajectory from DP Planner; 58 | - Yellow Trajectory: Init guess Trajectory from Cilqr, whilch solved with LQR; 59 | - Green Trajectory: Final Trajectory from Cilqr; 60 | 61 | If you want to get a better result, use the `Tracker` to get the initial guess. 62 | 63 | ```cpp 64 | // in src/Cilqr/algorithm/ilqr_optimizer.cpp , line 168 ~ 169 65 | // InitGuess(coarse_traj, &states, &controls); 66 | OpenLoopRollout(coarse_traj, &states, &controls); 67 | ``` 68 | 69 | ## 4. Acknowledgement 70 | Special thanks to [Bai Li](https://github.com/libai1943/CartesianPlanner) for ros simulation environment and [Galaxy](https://github.com/StarryN/Galaxy) for the safe corridor. 71 | 72 | -------------------------------------------------------------------------------- /algorithm/ilqr/barrier_function.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "algorithm/ilqr/vehicle_model.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "algorithm/math/math_utils.h" 10 | #include "algorithm/math/vec2d.h" 11 | 12 | namespace planning { 13 | 14 | template 15 | class BarrierFunction { 16 | public: 17 | BarrierFunction() = default; 18 | 19 | virtual ~BarrierFunction() = default; 20 | 21 | virtual void SetParam(const double t) = 0; 22 | 23 | virtual double GetParam() = 0; 24 | 25 | virtual double value(const double x) = 0; 26 | 27 | virtual Eigen::Matrix Jacbian( 28 | const double x, const Eigen::Matrix& dx) = 0; 29 | 30 | virtual Eigen::Matrix 31 | Hessian( 32 | const double x, 33 | const Eigen::Matrix& dx, 34 | const Eigen::Matrix& ddx = Eigen::MatrixXd::Zero(N, N)) = 0; 35 | }; 36 | 37 | template 38 | class ExponentialBarrierFunction : public BarrierFunction { 39 | public: 40 | ExponentialBarrierFunction() { 41 | q1_q2_ = q1_ * q2_; 42 | q1_q2_q2_ = q1_ * q2_ * q2_; 43 | } 44 | 45 | double value(const double x) override { 46 | // return q1_ * std::exp(q2_ * x); 47 | double cost = q1_ * std::exp(q2_ * x); 48 | return cost < q1_ ? 0.0 : cost; 49 | } 50 | 51 | Eigen::Matrix Jacbian( 52 | const double x, const Eigen::Matrix& dx) override { 53 | // return q1_ * q2_ * std::exp(q2_ * x) * dx; 54 | if (value(x) < math::kMathEpsilon) { 55 | return 0.0 * q1_q2_ * std::exp(q2_ * x) * dx; 56 | } 57 | return q1_q2_ * std::exp(q2_ * x) * dx; 58 | } 59 | 60 | Eigen::Matrix 61 | Hessian( 62 | const double x, 63 | const Eigen::Matrix& dx, 64 | const Eigen::Matrix& ddx = Eigen::MatrixXd::Zero(N, N)) override { 65 | // return q1_ * q2_ * std::exp(q2_ * x) * ddx + q1_ * q2_ * q2_ * std::exp(q2_ * x) * dx * dx.transpose(); 66 | // return q1_q2_q2_ * std::exp(q2_ * x) * dx * dx.transpose(); 67 | if (value(x) < math::kMathEpsilon) { 68 | return 0.0 * (q1_q2_ * std::exp(q2_ * x) * ddx + q1_q2_q2_ * std::exp(q2_ * x) * dx * dx.transpose()); 69 | } 70 | return q1_q2_ * std::exp(q2_ * x) * ddx + q1_q2_q2_ * std::exp(q2_ * x) * dx * dx.transpose(); 71 | } 72 | 73 | private: 74 | double q1_ = 0.5; 75 | double q2_ = 2.5; 76 | 77 | double q1_q2_ = 0.0; 78 | double q1_q2_q2_ = 0.0; 79 | }; 80 | 81 | template 82 | class RelaxBarrierFunction : public BarrierFunction { 83 | public: 84 | RelaxBarrierFunction() { 85 | reciprocal_t_ = 1.0 / t_; 86 | } 87 | 88 | void SetParam(const double t) override { 89 | t_ = t; 90 | // t_ = std::fmin(20.0, t_); 91 | // t_ = std::fmax(1.0, t_); 92 | reciprocal_t_ = 1.0 / t_; 93 | } 94 | 95 | double GetParam() override { 96 | return t_; 97 | } 98 | 99 | void SetEpsilon(const double epsilon) { 100 | epsilon_ = epsilon; 101 | } 102 | 103 | 104 | double value(const double x) override { 105 | // if (x < 0.0) { 106 | // return 0.0; 107 | // } 108 | if (x < -epsilon_) { 109 | return -reciprocal_t_ * std::log(-x); 110 | } else { 111 | return 0.5 * reciprocal_t_ * (std::pow((-x - 2.0 * epsilon_) / epsilon_, 2.0) - 1) - reciprocal_t_ * std::log(epsilon_); 112 | } 113 | } 114 | 115 | Eigen::Matrix Jacbian( 116 | const double x, const Eigen::Matrix& dx) override { 117 | // if (x < 0.0) { 118 | // return 0 * dx; 119 | // } 120 | if (x < -epsilon_) { 121 | return - reciprocal_t_ / x * dx; 122 | } else { 123 | return reciprocal_t_ * (x + 2.0 * epsilon_) / epsilon_ / epsilon_ * dx; 124 | } 125 | } 126 | 127 | Eigen::Matrix 128 | Hessian( 129 | const double x, 130 | const Eigen::Matrix& dx, 131 | const Eigen::Matrix& ddx = Eigen::MatrixXd::Zero(N, N)) override { 132 | // if (x < 0.0) { 133 | // return Eigen::MatrixXd::Zero(N, N); 134 | // } 135 | if (x < -epsilon_) { 136 | return reciprocal_t_ / x / x * dx * dx.transpose() - reciprocal_t_ / x * ddx; 137 | } else { 138 | return reciprocal_t_ * (x + 2.0 * epsilon_) / epsilon_ / epsilon_ * dx * dx.transpose(); 139 | } 140 | } 141 | 142 | private: 143 | double k_ = 2.0; 144 | double t_ = 5.0; 145 | double epsilon_ = 0.01; 146 | double reciprocal_t_ = 0.0; 147 | }; 148 | 149 | template 150 | class QuadraticBarrierFunction : public BarrierFunction { 151 | public: 152 | QuadraticBarrierFunction() { 153 | param_ = 1000.0; 154 | } 155 | 156 | double value(const double x) override { 157 | if (x < math::kMathEpsilon) { 158 | return 0.0; 159 | } else { 160 | return param_ * std::pow(x, 2.0); 161 | } 162 | } 163 | 164 | Eigen::Matrix Jacbian( 165 | const double x, const Eigen::Matrix& dx) override { 166 | if (x < math::kMathEpsilon) { 167 | return Eigen::MatrixXd::Zero(N, 1); 168 | } else { 169 | // std::cout << "Jacbian: " << 2.0 * param_ * dx << std::endl; 170 | return 2.0 * param_ * dx; 171 | } 172 | } 173 | 174 | Eigen::Matrix 175 | Hessian( 176 | const double x, 177 | const Eigen::Matrix& dx, 178 | const Eigen::Matrix& ddx = Eigen::MatrixXd::Zero(N, N)) override { 179 | if (x < math::kMathEpsilon) { 180 | return Eigen::MatrixXd::Zero(N, N); 181 | } else { 182 | // std::cout << "Hessian: " << 2.0 * param_ * dx * dx.transpose() << std::endl; 183 | return 2.0 * param_ * dx * dx.transpose(); 184 | } 185 | } 186 | 187 | private: 188 | double param_ = 10.0; 189 | }; 190 | 191 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/corridor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "algorithm/params/planner_config.h" 12 | #include "algorithm/utils/discretized_trajectory.h" 13 | #include "algorithm/utils/environment.h" 14 | #include "algorithm/math/line_segment2d.h" 15 | 16 | namespace planning { 17 | 18 | using ConvexPolygon = std::vector; 19 | using ConvexPolygons = std::vector; 20 | // Note: ax + by < c, Vector3d << a, b, c 21 | using Constraints = std::vector; 22 | using CorridorConstraints = std::vector; 23 | 24 | using LaneConstraints = 25 | std::vector>; 26 | 27 | class Corridor { 28 | public: 29 | Corridor(const CorridorConfig& config, const Env& env); 30 | 31 | void Init(const CorridorConfig& config, const Env& env); 32 | 33 | bool Plan( 34 | const DiscretizedTrajectory& trajectory, 35 | CorridorConstraints* const corridor_constraints, 36 | ConvexPolygons* const convex_polygons, 37 | LaneConstraints* const left_lane_constraints, 38 | LaneConstraints* const right_lane_constraints); 39 | 40 | std::vector> points_for_corridors() { 41 | return points_for_corridors_; 42 | } 43 | 44 | private: 45 | bool BuildCorridorConstraints( 46 | const DiscretizedTrajectory& trajectory, 47 | CorridorConstraints* const corridor_constraints, 48 | ConvexPolygons* const convex_polygons); 49 | 50 | bool BuildCorridor( 51 | const double origin_x, const double origin_y, 52 | const std::vector& points, 53 | Constraints* const constraints, 54 | ConvexPolygon* const polygon); 55 | 56 | bool CalLeftLaneConstraints( 57 | LaneConstraints* const left_lane_constraints); 58 | 59 | bool CalRightLaneConstraints( 60 | LaneConstraints* const right_lane_constraints); 61 | 62 | bool CalConstraintFromPoints( 63 | const math::Vec2d& start_pt, 64 | const math::Vec2d& end_pt, 65 | Eigen::Vector3d* const constraint); 66 | 67 | void AddCorridorPoints( 68 | const TrajectoryPoint& pt, 69 | std::vector* const points); 70 | 71 | std::vector LaneBoundarySample( 72 | const std::vector& lane_boundary); 73 | 74 | Eigen::Vector3d HalfPlaneConstraint( 75 | const math::Vec2d& start_pt, 76 | const math::Vec2d& end_pt); 77 | 78 | void CheckLaneConstraints( 79 | const DiscretizedTrajectory& trajectory, 80 | const LaneConstraints& left_lane_constraints, 81 | const LaneConstraints& right_lane_constraints); 82 | 83 | std::pair 84 | FindNeastLaneSegment( 85 | const double x, const double y, 86 | const std::vector>& lane_segs); 87 | 88 | private: 89 | CorridorConfig config_; 90 | Env env_; 91 | std::vector> points_for_corridors_; 92 | }; 93 | 94 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/ilqr_optimizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "algorithm/params/planner_config.h" 6 | #include "algorithm/ilqr/corridor.h" 7 | #include "algorithm/utils/discretized_trajectory.h" 8 | #include "algorithm/ilqr/vehicle_model.h" 9 | #include "algorithm/ilqr/barrier_function.h" 10 | #include "algorithm/ilqr/tracker.h" 11 | 12 | namespace planning { 13 | 14 | struct Cost { 15 | double total_cost = 0.0; 16 | double target_cost = 0.0; 17 | double dynamic_cost = 0.0; 18 | double corridor_cost = 0.0; 19 | double lane_boundary_cost = 0.0; 20 | 21 | Cost() = default; 22 | 23 | Cost(const double c0, const double c1, const double c2, 24 | const double c3, const double c4) 25 | : total_cost(c0), target_cost(c1), dynamic_cost(c2) 26 | , corridor_cost(c3), lane_boundary_cost(c4) {} 27 | }; 28 | 29 | class IlqrOptimizer { 30 | public: 31 | IlqrOptimizer() = default; 32 | 33 | IlqrOptimizer( 34 | const IlqrConfig& config, const VehicleParam& param, 35 | const double horizon, const double dt); 36 | 37 | void Init( 38 | const IlqrConfig& config, const VehicleParam& param, 39 | const double horizon, const double dt); 40 | 41 | bool Plan( 42 | const TrajectoryPoint& start_state, 43 | const DiscretizedTrajectory& coarse_traj, 44 | const CorridorConstraints& corridor, 45 | const LaneConstraints& left_lane_cons, 46 | const LaneConstraints& right_lane_cons, 47 | DiscretizedTrajectory* const opt_trajectory, 48 | std::vector* const iter_trajs); 49 | 50 | std::vector cost() { 51 | return cost_; 52 | } 53 | 54 | private: 55 | void iqr( 56 | const DiscretizedTrajectory& coarse_traj, 57 | std::vector* const guess_state, 58 | std::vector* const guess_control); 59 | 60 | void CalculateDiscRadius(); 61 | 62 | void TransformGoals(const DiscretizedTrajectory& coarse_traj); 63 | 64 | void InitGuess( 65 | const DiscretizedTrajectory& coarse_traj, 66 | std::vector* const state, 67 | std::vector* const control); 68 | 69 | void Optimize( 70 | const TrajectoryPoint& start_state, 71 | const DiscretizedTrajectory& coarse_traj, 72 | const CorridorConstraints& corridor, 73 | const LaneConstraints& left_lane_cons, 74 | const LaneConstraints& right_lane_cons, 75 | DiscretizedTrajectory* const opt_trajectory, 76 | std::vector* const iter_trajs); 77 | 78 | bool Backward( 79 | const double lambda, 80 | const std::vector& states, 81 | const std::vector& controls, 82 | std::vector>* const Ks, 83 | std::vector>* const ks, 84 | std::vector>* const Qus, 85 | std::vector>* const Quus); 86 | 87 | void Forward( 88 | const double alpha, 89 | std::vector* const states, 90 | std::vector* const controls, 91 | const std::vector>& Ks, 92 | const std::vector>& ks, 93 | const std::vector>& Qus, 94 | const std::vector>& Quus); 95 | 96 | double TotalCost( 97 | const std::vector& states, 98 | const std::vector& controls, 99 | Cost* const cost); 100 | 101 | double JCost( 102 | const std::vector& states, 103 | const std::vector& controls); 104 | 105 | double DynamicsCost( 106 | const std::vector& states, 107 | const std::vector& controls); 108 | 109 | double CorridorCost( 110 | const std::vector& states); 111 | 112 | double LaneBoundaryCost( 113 | const std::vector& states); 114 | 115 | void CostJacbian( 116 | const int index, 117 | const State& state, const Control& control, 118 | State* const cost_Jx, Control* cost_Ju); 119 | 120 | void CostHessian( 121 | const int index, 122 | const State& state, const Control& control, 123 | Eigen::Matrix* const cost_Hx, 124 | Eigen::Matrix* const cost_Hu); 125 | 126 | void ShrinkConstraints( 127 | const CorridorConstraints& corridor, 128 | const LaneConstraints& left_lane_cons, 129 | const LaneConstraints& right_lane_cons); 130 | 131 | Eigen::Vector3d FindNeastLaneSegment( 132 | const double x, const double y, 133 | std::vector> lane_segs); 134 | 135 | void DynamicsConsJacbian( 136 | const State& state, const Control& control, 137 | State* const cost_Jx, Control* cost_Ju); 138 | 139 | void DynamicsConsHessian( 140 | const State& state, const Control& control, 141 | Eigen::Matrix* const cost_Hx, 142 | Eigen::Matrix* const cost_Hu); 143 | 144 | void CorridorConsJacbian( 145 | const int index, 146 | const State& state, State* const cost_Jx); 147 | 148 | void CorridorConsHessian( 149 | const int index, const State& state, 150 | Eigen::Matrix* const cost_Hx); 151 | 152 | void LaneBoundaryConsJacbian( 153 | const State& state, State* const cost_Jx); 154 | 155 | void LaneBoundaryConsHessian( 156 | const State& state, 157 | Eigen::Matrix* const cost_Hx); 158 | 159 | DiscretizedTrajectory TransformToTrajectory( 160 | const std::vector& states, 161 | const std::vector& controls); 162 | 163 | void NormalizeHalfPlane(); 164 | 165 | double CalGradientNorm( 166 | const std::vector>& ks, 167 | const std::vector& controls); 168 | 169 | private: 170 | double horizon_; 171 | double delta_t_; 172 | int num_of_knots_; 173 | double disc_radius_; 174 | 175 | double rho_ = 1e-10; 176 | 177 | IlqrConfig config_; 178 | VehicleParam vehicle_param_; 179 | 180 | VehicleModel vehicle_model_; 181 | // ExponentialBarrierFunction state_barrier_; 182 | // ExponentialBarrierFunction control_barrier_; 183 | 184 | // QuadraticBarrierFunction state_barrier_; 185 | // QuadraticBarrierFunction control_barrier_; 186 | 187 | RelaxBarrierFunction state_barrier_; 188 | RelaxBarrierFunction control_barrier_; 189 | 190 | TrajectoryPoint start_state_; 191 | std::vector goals_; 192 | 193 | CorridorConstraints shrinked_corridor_; 194 | LaneConstraints shrinked_left_lane_cons_; 195 | LaneConstraints shrinked_right_lane_cons_; 196 | 197 | Tracker tracker_; 198 | 199 | std::vector cost_; 200 | 201 | double rhoMin_ = 1e-8; 202 | double rhoMax_ = 1e11; 203 | double drho_ = 1.0; 204 | double rhoFactor_ = 1.6; 205 | 206 | std::vector As; 207 | std::vector Bs; 208 | 209 | std::vector> cost_Jx; 210 | std::vector> cost_Ju; 211 | std::vector> cost_Hx; 212 | std::vector> cost_Hu; 213 | 214 | double delta_V_[2]; 215 | }; 216 | 217 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/tracker.cc: -------------------------------------------------------------------------------- 1 | #include "algorithm/ilqr/tracker.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "algorithm/math/vec2d.h" 7 | #include "algorithm/math/math_utils.h" 8 | #include "algorithm/math/linear_quadratic_regulator.h" 9 | 10 | namespace planning { 11 | 12 | bool Tracker::Plan( 13 | const TrajectoryPoint& start_state, 14 | const DiscretizedTrajectory& coarse_traj, 15 | DiscretizedTrajectory* const opt_trajectory) { 16 | return lqr(start_state, coarse_traj, opt_trajectory); 17 | } 18 | 19 | std::pair 20 | Tracker::CalcaulateInitState( 21 | const TrajectoryPoint& current_state) { 22 | 23 | double preveiw_x = current_state.x + std::cos(current_state.theta) * 24 | current_state.velocity * config_.lateral_config.preview_time; 25 | double preveiw_y = current_state.y + std::sin(current_state.theta) * 26 | current_state.velocity * config_.lateral_config.preview_time; 27 | 28 | math::Vec2d cur_xy{preveiw_x, preveiw_y}; 29 | TrajectoryPoint project_pt; 30 | math::Vec2d proj = follow_trajectory_.GetProjection(cur_xy, &project_pt); 31 | 32 | double dx = current_state.x - project_pt.x; 33 | double dy = current_state.y - project_pt.y; 34 | double l = std::sin(project_pt.theta) * dx - std::cos(project_pt.theta) *dy; 35 | 36 | double theta_error = math::NormalizeAngle(project_pt.theta - current_state.theta); 37 | State lateral_state{ 38 | l, 39 | theta_error, 40 | current_state.delta}; 41 | 42 | TrajectoryPoint match_pt = follow_trajectory_.EvaluateTime(current_state.time + 0.0); 43 | dx = current_state.x - match_pt.x; 44 | dy = current_state.y - match_pt.y; 45 | double s = -std::cos(match_pt.theta) * dx - std::sin(match_pt.theta) * dy; 46 | double v_error = match_pt.velocity - current_state.velocity; 47 | State longitudinal_state{ 48 | match_pt.s - project_pt.s, 49 | v_error, 50 | current_state.a}; 51 | 52 | return {lateral_state, longitudinal_state}; 53 | } 54 | 55 | double Tracker::LateralControl( 56 | const Tracker::State& state, const double v) { 57 | double v_amend = std::fmax(2, v); 58 | double dt = 0.1; 59 | lateral_A_(0, 1) = v_amend * dt; 60 | lateral_A_(1, 2) = -v_amend / vehicle_param_.wheel_base * dt; 61 | 62 | Eigen::MatrixXd K; 63 | math::SolveLQRProblem( 64 | lateral_A_, lateral_B_, 65 | lateral_Q_, lateral_R_, 66 | config_.tolerance, config_.max_num_iteration, 67 | &K); 68 | 69 | return -(K * state)(0, 0); 70 | } 71 | 72 | double Tracker::LongitudinalControl(const Tracker::State& state) { 73 | Eigen::MatrixXd K; 74 | math::SolveLQRProblem( 75 | longitudinal_A_, longitudinal_B_, 76 | longitudinal_Q_, longitudinal_R_, 77 | config_.tolerance, config_.max_num_iteration, 78 | &K); 79 | 80 | return -(K * state)(0, 0); 81 | } 82 | 83 | TrajectoryPoint Tracker::VehicleDynamic( 84 | const TrajectoryPoint& cur_state, 85 | const double delta_rate, 86 | const double jerk) { 87 | 88 | const double dt = config_.sumulation_dt; 89 | const double dt_2 = dt / 2.0; 90 | VehicleState ss_tk1 = vehicle_mode(cur_state.theta, 91 | cur_state.velocity, 92 | cur_state.delta, 93 | cur_state.a, 94 | jerk, 95 | delta_rate); 96 | 97 | VehicleState ss_tk2 = vehicle_mode(cur_state.theta + ss_tk1.theta * dt_2, 98 | cur_state.velocity + ss_tk1.v * dt_2, 99 | cur_state.delta + ss_tk1.delta * dt_2, 100 | cur_state.a + ss_tk1.a * dt_2, 101 | jerk, 102 | delta_rate); 103 | 104 | VehicleState ss_tk3 = vehicle_mode(cur_state.theta + ss_tk2.theta * dt_2, 105 | cur_state.velocity + ss_tk2.v * dt_2, 106 | cur_state.delta + ss_tk2.delta * dt_2, 107 | cur_state.a + ss_tk2.a * dt_2, 108 | jerk, 109 | delta_rate); 110 | 111 | VehicleState ss_tk4 = vehicle_mode(cur_state.theta + ss_tk3.theta * dt, 112 | cur_state.velocity + ss_tk3.v * dt, 113 | cur_state.delta + ss_tk3.delta * dt, 114 | cur_state.a + ss_tk3.a * dt, 115 | jerk, 116 | delta_rate); 117 | 118 | TrajectoryPoint next_state; 119 | next_state.time = cur_state.time + dt; 120 | next_state.x = cur_state.x + (ss_tk1.x + ss_tk2.x * 2.0 + ss_tk3.x * 2.0 + ss_tk4.x) / 6.0 * dt; 121 | next_state.y = cur_state.y + (ss_tk1.y + ss_tk2.y * 2.0 + ss_tk3.y * 2.0 + ss_tk4.y) / 6.0 * dt; 122 | next_state.theta = math::NormalizeAngle(cur_state.theta + 123 | (ss_tk1.theta + ss_tk2.theta * 2.0 + ss_tk3.theta * 2.0 + ss_tk4.theta) / 6.0 * dt); 124 | next_state.velocity = std::fmax(0.0, cur_state.velocity + (ss_tk1.v + ss_tk2.v * 2.0 + ss_tk3.v * 2.0 + ss_tk4.v) / 6.0 * dt); 125 | next_state.delta = math::NormalizeAngle( 126 | std::fmin(vehicle_param_.delta_max, 127 | std::fmax(vehicle_param_.delta_min, 128 | cur_state.delta + (ss_tk1.delta + ss_tk2.delta * 2.0 + ss_tk3.delta * 2.0 + ss_tk4.delta) / 6.0 * dt))); 129 | next_state.a = std::fmin(vehicle_param_.max_acceleration, 130 | std::fmax(vehicle_param_.min_acceleration, cur_state.a + (ss_tk1.a + ss_tk2.a * 2.0 + ss_tk3.a * 2.0 + ss_tk4.a) / 6.0 * dt)); 131 | next_state.kappa = std::tan(next_state.delta) / vehicle_param_.wheel_base; 132 | 133 | double ds = std::hypot(next_state.x - cur_state.x, next_state.y - cur_state.y); 134 | next_state.s = cur_state.s + ds; 135 | return next_state; 136 | } 137 | 138 | void Tracker::InitMatrix() { 139 | double dt = config_.dt; 140 | 141 | lateral_A_(0, 0) = 1.0; 142 | lateral_A_(1, 1) = 1.0; 143 | lateral_A_(2, 2) = 1.0; 144 | 145 | lateral_B_(2, 0) = 1.0 * dt; 146 | 147 | lateral_Q_(0, 0) = lateral_config_.weight_l; 148 | lateral_Q_(1, 1) = lateral_config_.weight_theta; 149 | lateral_Q_(2, 2) = lateral_config_.weight_delta; 150 | 151 | lateral_R_(0, 0) = lateral_config_.weight_delta_rate; 152 | 153 | longitudinal_A_(0, 0) = 1.0; 154 | longitudinal_A_(1, 1) = 1.0; 155 | longitudinal_A_(2, 2) = 1.0; 156 | 157 | longitudinal_A_(0, 1) = dt; 158 | longitudinal_A_(1, 2) = -dt; 159 | 160 | longitudinal_B_(2, 0) = 1.0 * dt; 161 | 162 | longitudinal_Q_(0, 0) = longitudinal_config_.weight_s; 163 | longitudinal_Q_(1, 1) = longitudinal_config_.weight_v; 164 | longitudinal_Q_(2, 2) = longitudinal_config_.weight_a; 165 | 166 | longitudinal_R_(0, 0) = longitudinal_config_.weight_j; 167 | } 168 | 169 | bool Tracker::lqr( 170 | const TrajectoryPoint& start_state, 171 | const DiscretizedTrajectory& coarse_traj, 172 | DiscretizedTrajectory* const opt_trajectory) { 173 | follow_trajectory_ = coarse_traj; 174 | std::vector trajectory; 175 | 176 | TrajectoryPoint cur_state = start_state; 177 | trajectory.push_back(cur_state); 178 | 179 | double start_time = follow_trajectory_.trajectory().front().time; 180 | double end_time = follow_trajectory_.trajectory().back().time; 181 | cur_state.time = start_time; 182 | cur_state.s = 0.0; 183 | 184 | int i = 1; 185 | for (double t = start_time; t < end_time + math::kMathEpsilon; t += config_.sumulation_dt) { 186 | auto init_state = CalcaulateInitState(cur_state); 187 | double delta_rate = LateralControl(init_state.first, cur_state.velocity); 188 | double jerk = LongitudinalControl(init_state.second); 189 | 190 | delta_rate = std::fmax(vehicle_param_.delta_rate_min, 191 | std::fmin(vehicle_param_.delta_rate_max, delta_rate)); 192 | jerk = std::fmax(vehicle_param_.jerk_min, 193 | std::fmin(vehicle_param_.jerk_max, jerk)); 194 | trajectory.back().delta_rate = delta_rate; 195 | trajectory.back().jerk = jerk; 196 | 197 | cur_state = VehicleDynamic(cur_state, delta_rate, jerk); 198 | cur_state.time = t; 199 | if (cur_state.time > follow_trajectory_.trajectory().at(i).time - math::kMathEpsilon) { 200 | trajectory.push_back(cur_state); 201 | ++i; 202 | } 203 | } 204 | 205 | if (trajectory.size() != follow_trajectory_.trajectory().size()) { 206 | std::cout << "tacker failed." << std::endl; 207 | return false; 208 | } 209 | 210 | std::cout << "coarse traj pts size: " << follow_trajectory_.trajectory().size() << std::endl; 211 | std::cout << "init guess traj pts size: " << trajectory.size() << std::endl; 212 | 213 | *opt_trajectory = DiscretizedTrajectory(trajectory); 214 | return true; 215 | } 216 | 217 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "algorithm/params/planner_config.h" 8 | #include "algorithm/params/vehicle_param.h" 9 | #include "algorithm/utils/discretized_trajectory.h" 10 | 11 | namespace planning { 12 | 13 | class Tracker { 14 | public: 15 | using State = Eigen::Matrix; 16 | struct VehicleState { 17 | double x; 18 | double y; 19 | double theta; 20 | double v; 21 | double delta; 22 | double a; 23 | double j; 24 | double delta_rate; 25 | }; 26 | 27 | public: 28 | Tracker() = default; 29 | 30 | Tracker(const TrackerConfig& config, const VehicleParam& param) 31 | : config_(config) 32 | , vehicle_param_(param) { 33 | lateral_config_ = config_.lateral_config; 34 | longitudinal_config_ = config_.longitudinal_config; 35 | 36 | InitMatrix(); 37 | } 38 | 39 | void Init(const TrackerConfig& config, const VehicleParam& param) { 40 | vehicle_param_ = param; 41 | config_ = config; 42 | lateral_config_ = config_.lateral_config; 43 | longitudinal_config_ = config_.longitudinal_config; 44 | 45 | InitMatrix(); 46 | } 47 | 48 | bool Plan( 49 | const TrajectoryPoint& start_state, 50 | const DiscretizedTrajectory& coarse_traj, 51 | DiscretizedTrajectory* const opt_trajectory); 52 | 53 | private: 54 | bool lqr( 55 | const TrajectoryPoint& start_state, 56 | const DiscretizedTrajectory& coarse_traj, 57 | DiscretizedTrajectory* const opt_trajectory); 58 | 59 | std::pair CalcaulateInitState( 60 | const TrajectoryPoint& current_state); 61 | 62 | double LateralControl(const State& state, const double v); 63 | 64 | double LongitudinalControl(const State& state); 65 | 66 | TrajectoryPoint VehicleDynamic( 67 | const TrajectoryPoint& cur_state, 68 | const double delta_rate, 69 | const double jerk); 70 | 71 | void InitMatrix(); 72 | 73 | VehicleState vehicle_mode( 74 | const double theta, 75 | const double v, 76 | const double delta, 77 | const double a, 78 | const double j, 79 | const double delta_rate) { 80 | VehicleState dot_x; 81 | dot_x.x = v * std::cos(theta); 82 | dot_x.y = v * std::sin(theta); 83 | dot_x.theta = v * std::tan(delta) / vehicle_param_.wheel_base; 84 | dot_x.v = a; 85 | dot_x.a = j; 86 | dot_x.delta = delta_rate; 87 | return dot_x; 88 | }; 89 | 90 | private: 91 | TrackerConfig config_; 92 | VehicleParam vehicle_param_; 93 | 94 | LateralTrackerConfig lateral_config_; 95 | LongitudinalTrackerConfig longitudinal_config_; 96 | 97 | DiscretizedTrajectory follow_trajectory_; 98 | 99 | Eigen::Matrix lateral_A_ = Eigen::MatrixXd::Zero(3, 3); 100 | Eigen::Matrix lateral_B_ = Eigen::MatrixXd::Zero(3, 1); 101 | Eigen::Matrix lateral_Q_ = Eigen::MatrixXd::Zero(3, 3); 102 | Eigen::Matrix lateral_R_ = Eigen::MatrixXd::Zero(1, 1); 103 | 104 | Eigen::Matrix longitudinal_A_ = Eigen::MatrixXd::Zero(3, 3); 105 | Eigen::Matrix longitudinal_B_ = Eigen::MatrixXd::Zero(3, 1); 106 | Eigen::Matrix longitudinal_Q_ = Eigen::MatrixXd::Zero(3, 3); 107 | Eigen::Matrix longitudinal_R_ = Eigen::MatrixXd::Zero(1, 1); 108 | }; 109 | 110 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/vehicle_model.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "algorithm/ilqr/vehicle_model.h" 3 | 4 | #include 5 | #include 6 | 7 | #include "algorithm/math/math_utils.h" 8 | 9 | namespace planning { 10 | 11 | VehicleModel::VehicleModel( 12 | const IlqrConfig& config, 13 | const VehicleParam& param, 14 | const double horizon, 15 | const double dt) 16 | : config_(config) 17 | , param_(param) 18 | , horizon_(horizon) 19 | , delta_t_(dt) {} 20 | 21 | void VehicleModel::DynamicsJacbian( 22 | const State& state, const Control& control, 23 | SystemMatrix* const A, InputMatrix* const B) { 24 | double L = param_.wheel_base; 25 | // Euler Method is too poor. 26 | // double theta = state(2, 0); 27 | // double delta = state(5, 0); 28 | // double v = state(3, 0); 29 | 30 | // *A << 1.0, 0.0, -std::sin(theta) * v * delta_t_, std::cos(theta) * delta_t_, 0.0, 0.0, 31 | // 0.0, 1.0, std::cos(theta) * v * delta_t_, std::sin(theta) * delta_t_, 0.0, 0.0, 32 | // 0.0, 0.0, 1.0, std::tan(delta) / L * delta_t_, 0.0, v * delta_t_ * (1.0 + std::pow(std::tan(delta), 2)) / L, 33 | // 0.0, 0.0, 0.0, 1.0, delta_t_, 0.0, 34 | // 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 35 | // 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 36 | 37 | // *B << 0.0, 0.0, 38 | // 0.0, 0.0, 39 | // 0.0, 0.0, 40 | // 0.0, 0.0, 41 | // delta_t_, 0.0, 42 | // 0.0, delta_t_; 43 | 44 | double v = state(3, 0); 45 | double theta = math::NormalizeAngle(state(2, 0)); 46 | double delta = math::NormalizeAngle(state(5, 0)); 47 | 48 | double a = state(4, 0); 49 | double jerk = control(0, 0); 50 | double delta_rate = control(1, 0); 51 | 52 | double theta_mid = theta + 0.5 * delta_t_ * v * std::tan(delta) / L; 53 | double tan_delta = std::tan(delta); 54 | double tan_delta_rate = std::tan(delta + 0.5 * delta_t_ * delta_rate); 55 | double cos_theta_mid = std::cos(theta_mid); 56 | double sin_theta_mid = std::sin(theta_mid); 57 | double tan_delta_square = tan_delta * tan_delta; 58 | double tan_delta_rate_square = tan_delta_rate * tan_delta_rate; 59 | double v_tan_delta_rate = v * (tan_delta_rate_square + 1); 60 | 61 | *A << 1.0, 0.0, -delta_t_ * (0.5 * a * delta_t_ + v) * sin_theta_mid, 62 | delta_t_ * cos_theta_mid - 0.5 * delta_t_ * delta_t_ * (0.5 * a * delta_t_ + v) * sin_theta_mid * tan_delta / L, 63 | 0.5 * delta_t_ * delta_t_ * cos_theta_mid, 64 | -0.5 * delta_t_ * delta_t_ * v * (0.5 * a * delta_t_ + v) * (tan_delta_square + 1) * sin_theta_mid / L, 65 | 66 | 0.0, 1.0, delta_t_ * (0.5 * a * delta_t_ + v) * cos_theta_mid, 67 | delta_t_ * sin_theta_mid + 0.5 * delta_t_ * delta_t_ * (0.5 * a * delta_t_ + v) * cos_theta_mid * tan_delta / L, 68 | 0.5 * delta_t_ * delta_t_ * sin_theta_mid, 69 | 0.5 * delta_t_ * delta_t_ * v * (0.5 * a * delta_t_ + v) * (tan_delta_square + 1) * cos_theta_mid / L, 70 | 71 | 0.0, 0.0, 1.0, 72 | delta_t_ * tan_delta_rate / L, 73 | 0.5 * delta_t_ * delta_t_ * tan_delta_rate / L, 74 | delta_t_ * v_tan_delta_rate / L, 75 | 76 | 0.0, 0.0, 0.0, 1.0, delta_t_, 0.0, 77 | 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 78 | 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 79 | 80 | *B << 0.0, 0.0, 81 | 0.0, 0.0, 82 | 0.0, 0.5 * delta_t_ * delta_t_ * v * (tan_delta_rate_square + 1) / L, 83 | 0.5 * delta_t_ * delta_t_, 0.0, 84 | delta_t_, 0.0, 85 | 0.0, delta_t_; 86 | } 87 | 88 | void VehicleModel::Dynamics( 89 | const State& state, const Control& control, 90 | State* const next_state) { 91 | // std::cout << "current state: " << state << std::endl; 92 | // std::cout << "control: " << control << std::endl; 93 | // SystemMatrix A; 94 | // InputMatrix B; 95 | // DynamicsJacbian(state, control, &A, &B); 96 | // *next_state = A * state + B * control; 97 | 98 | // State inc; 99 | // inc << state(3, 0) * std::cos(state(2, 0)) * delta_t_, 100 | // state(3, 0) * std::sin(state(2, 0)) * delta_t_, 101 | // state(3, 0) * std::tan(state(2, 0)) / param_.wheel_base * delta_t_, 102 | // state(4, 0) * delta_t_, 103 | // control(0, 0) * delta_t_, 104 | // control(1, 0) * delta_t_; 105 | // *next_state = state + inc; 106 | 107 | double theta = state(2, 0); 108 | double v = state(3, 0); 109 | double a = state(4, 0); 110 | double delta = state(5, 0); 111 | 112 | State k1 = DynamicsContinuous(state, control); 113 | State mid_state = state + 0.5 * delta_t_ * k1; 114 | State k2 = DynamicsContinuous(mid_state, control); 115 | *next_state = state + delta_t_ * k2; 116 | (*next_state)(2, 0) = math::NormalizeAngle((*next_state)(2, 0)); 117 | (*next_state)(5, 0) = math::NormalizeAngle((*next_state)(5, 0)); 118 | 119 | // std::cout << "next_state: " << *next_state << std::endl; 120 | // std::cout <<"**************************************" << std::endl; 121 | } 122 | 123 | State VehicleModel::DynamicsContinuous( 124 | const State& state, const Control& control) { 125 | double theta = math::NormalizeAngle(state(2, 0)); 126 | double v = state(3, 0); 127 | double a = state(4, 0); 128 | double delta = math::NormalizeAngle(state(5, 0)); 129 | 130 | State res; 131 | res << v * std::cos(theta), 132 | v * std::sin(theta), 133 | v * std::tan(delta) / param_.wheel_base, 134 | a, 135 | control(0, 0), 136 | control(1, 0); 137 | return res; 138 | } 139 | 140 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/ilqr/vehicle_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "algorithm/params/planner_config.h" 8 | 9 | namespace planning { 10 | 11 | constexpr int kStateNum = 6; 12 | constexpr int kControlNum = 2; 13 | 14 | using State = Eigen::Matrix; 15 | using Control = Eigen::Matrix; 16 | using SystemMatrix = Eigen::Matrix; 17 | using InputMatrix = Eigen::Matrix; 18 | 19 | class VehicleModel { 20 | public: 21 | VehicleModel() = default; 22 | 23 | VehicleModel( 24 | const IlqrConfig& config, 25 | const VehicleParam& param, 26 | const double horizon, 27 | const double dt); 28 | 29 | 30 | void DynamicsJacbian( 31 | const State& state, const Control& control, 32 | SystemMatrix* const A, InputMatrix* const B); 33 | 34 | void Dynamics( 35 | const State& state, const Control& control, 36 | State* const next_state); 37 | 38 | 39 | private: 40 | 41 | State DynamicsContinuous( 42 | const State& state, const Control& control); 43 | 44 | private: 45 | double horizon_; 46 | double delta_t_; 47 | int num_of_knots_; 48 | IlqrConfig config_; 49 | VehicleParam param_; 50 | double k = 0.0001; 51 | 52 | }; 53 | 54 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/math/aabox2d.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include "algorithm/math/aabox2d.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "algorithm/math/math_utils.h" 24 | 25 | namespace planning { 26 | namespace math { 27 | 28 | AABox2d::AABox2d(const Vec2d ¢er, const double length, const double width) 29 | : center_(center), 30 | length_(length), 31 | width_(width), 32 | half_length_(length / 2.0), 33 | half_width_(width / 2.0) { 34 | assert(length_ > -kMathEpsilon); 35 | assert(width_ > -kMathEpsilon); 36 | } 37 | 38 | AABox2d::AABox2d(const Vec2d &one_corner, const Vec2d &opposite_corner) 39 | : AABox2d((one_corner + opposite_corner) / 2.0, 40 | std::abs(one_corner.x() - opposite_corner.x()), 41 | std::abs(one_corner.y() - opposite_corner.y())) {} 42 | 43 | AABox2d::AABox2d(const std::vector &points) { 44 | assert(!points.empty()); 45 | double min_x = points[0].x(); 46 | double max_x = points[0].x(); 47 | double min_y = points[0].y(); 48 | double max_y = points[0].y(); 49 | for (const auto &point : points) { 50 | min_x = std::min(min_x, point.x()); 51 | max_x = std::max(max_x, point.x()); 52 | min_y = std::min(min_y, point.y()); 53 | max_y = std::max(max_y, point.y()); 54 | } 55 | 56 | center_ = {(min_x + max_x) / 2.0, (min_y + max_y) / 2.0}; 57 | length_ = max_x - min_x; 58 | width_ = max_y - min_y; 59 | half_length_ = length_ / 2.0; 60 | half_width_ = width_ / 2.0; 61 | } 62 | 63 | void AABox2d::GetAllCorners(std::vector *const corners) const { 64 | assert(corners); 65 | corners->clear(); 66 | corners->reserve(4); 67 | corners->emplace_back(center_.x() + half_length_, center_.y() - half_width_); 68 | corners->emplace_back(center_.x() + half_length_, center_.y() + half_width_); 69 | corners->emplace_back(center_.x() - half_length_, center_.y() + half_width_); 70 | corners->emplace_back(center_.x() - half_length_, center_.y() - half_width_); 71 | } 72 | 73 | bool AABox2d::IsPointIn(const Vec2d &point) const { 74 | return std::abs(point.x() - center_.x()) <= half_length_ + kMathEpsilon && 75 | std::abs(point.y() - center_.y()) <= half_width_ + kMathEpsilon; 76 | } 77 | 78 | bool AABox2d::IsPointOnBoundary(const Vec2d &point) const { 79 | const double dx = std::abs(point.x() - center_.x()); 80 | const double dy = std::abs(point.y() - center_.y()); 81 | return (std::abs(dx - half_length_) <= kMathEpsilon && 82 | dy <= half_width_ + kMathEpsilon) || 83 | (std::abs(dy - half_width_) <= kMathEpsilon && 84 | dx <= half_length_ + kMathEpsilon); 85 | } 86 | 87 | double AABox2d::DistanceTo(const Vec2d &point) const { 88 | const double dx = std::abs(point.x() - center_.x()) - half_length_; 89 | const double dy = std::abs(point.y() - center_.y()) - half_width_; 90 | if (dx <= 0.0) { 91 | return std::max(0.0, dy); 92 | } 93 | if (dy <= 0.0) { 94 | return dx; 95 | } 96 | return hypot(dx, dy); 97 | } 98 | 99 | double AABox2d::DistanceTo(const AABox2d &box) const { 100 | const double dx = 101 | std::abs(box.center_x() - center_.x()) - box.half_length() - half_length_; 102 | const double dy = 103 | std::abs(box.center_y() - center_.y()) - box.half_width() - half_width_; 104 | if (dx <= 0.0) { 105 | return std::max(0.0, dy); 106 | } 107 | if (dy <= 0.0) { 108 | return dx; 109 | } 110 | return hypot(dx, dy); 111 | } 112 | 113 | bool AABox2d::HasOverlap(const AABox2d &box) const { 114 | return std::abs(box.center_x() - center_.x()) <= 115 | box.half_length() + half_length_ && 116 | std::abs(box.center_y() - center_.y()) <= 117 | box.half_width() + half_width_; 118 | } 119 | 120 | void AABox2d::Shift(const Vec2d &shift_vec) { center_ += shift_vec; } 121 | 122 | void AABox2d::MergeFrom(const AABox2d &other_box) { 123 | const double x1 = std::min(min_x(), other_box.min_x()); 124 | const double x2 = std::max(max_x(), other_box.max_x()); 125 | const double y1 = std::min(min_y(), other_box.min_y()); 126 | const double y2 = std::max(max_y(), other_box.max_y()); 127 | center_ = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0); 128 | length_ = x2 - x1; 129 | width_ = y2 - y1; 130 | half_length_ = length_ / 2.0; 131 | half_width_ = width_ / 2.0; 132 | } 133 | 134 | void AABox2d::MergeFrom(const Vec2d &other_point) { 135 | const double x1 = std::min(min_x(), other_point.x()); 136 | const double x2 = std::max(max_x(), other_point.x()); 137 | const double y1 = std::min(min_y(), other_point.y()); 138 | const double y2 = std::max(max_y(), other_point.y()); 139 | center_ = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0); 140 | length_ = x2 - x1; 141 | width_ = y2 - y1; 142 | half_length_ = length_ / 2.0; 143 | half_width_ = width_ / 2.0; 144 | } 145 | 146 | } // namespace math 147 | } // namespace planning 148 | -------------------------------------------------------------------------------- /algorithm/math/aabox2d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief Defines the AABox2d class. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | 27 | #include "algorithm/math/vec2d.h" 28 | 29 | /** 30 | * @namespace apollo::common::math 31 | * @brief apollo::common::math 32 | */ 33 | namespace planning { 34 | namespace math { 35 | 36 | /** 37 | * @class AABox2d 38 | * @brief Implements a class of (undirected) axes-aligned bounding boxes in 2-D. 39 | * This class is referential-agnostic. 40 | */ 41 | class AABox2d { 42 | public: 43 | /** 44 | * @brief Default constructor. 45 | * Creates an axes-aligned box with zero length and width at the origin. 46 | */ 47 | AABox2d() = default; 48 | /** 49 | * @brief Parameterized constructor. 50 | * Creates an axes-aligned box with given center, length, and width. 51 | * @param center The center of the box 52 | * @param length The size of the box along the x-axis 53 | * @param width The size of the box along the y-axis 54 | */ 55 | AABox2d(const Vec2d ¢er, const double length, const double width); 56 | /** 57 | * @brief Parameterized constructor. 58 | * Creates an axes-aligned box from two opposite corners. 59 | * @param one_corner One corner of the box 60 | * @param opposite_corner The opposite corner to the first one 61 | */ 62 | AABox2d(const Vec2d &one_corner, const Vec2d &opposite_corner); 63 | /** 64 | * @brief Parameterized constructor. 65 | * Creates an axes-aligned box containing all points in a given vector. 66 | * @param points Vector of points to be included inside the box. 67 | */ 68 | explicit AABox2d(const std::vector &points); 69 | 70 | /** 71 | * @brief Getter of center_ 72 | * @return Center of the box 73 | */ 74 | const Vec2d ¢er() const { return center_; } 75 | 76 | /** 77 | * @brief Getter of x-component of center_ 78 | * @return x-component of the center of the box 79 | */ 80 | double center_x() const { return center_.x(); } 81 | 82 | /** 83 | * @brief Getter of y-component of center_ 84 | * @return y-component of the center of the box 85 | */ 86 | double center_y() const { return center_.y(); } 87 | 88 | /** 89 | * @brief Getter of length_ 90 | * @return The length of the box 91 | */ 92 | double length() const { return length_; } 93 | 94 | /** 95 | * @brief Getter of width_ 96 | * @return The width of the box 97 | */ 98 | double width() const { return width_; } 99 | 100 | /** 101 | * @brief Getter of half_length_ 102 | * @return Half of the length of the box 103 | */ 104 | double half_length() const { return half_length_; } 105 | 106 | /** 107 | * @brief Getter of half_width_ 108 | * @return Half of the width of the box 109 | */ 110 | double half_width() const { return half_width_; } 111 | 112 | /** 113 | * @brief Getter of length_*width_ 114 | * @return The area of the box 115 | */ 116 | double area() const { return length_ * width_; } 117 | 118 | /** 119 | * @brief Returns the minimum x-coordinate of the box 120 | * 121 | * @return x-coordinate 122 | */ 123 | double min_x() const { return center_.x() - half_length_; } 124 | 125 | /** 126 | * @brief Returns the maximum x-coordinate of the box 127 | * 128 | * @return x-coordinate 129 | */ 130 | double max_x() const { return center_.x() + half_length_; } 131 | 132 | /** 133 | * @brief Returns the minimum y-coordinate of the box 134 | * 135 | * @return y-coordinate 136 | */ 137 | double min_y() const { return center_.y() - half_width_; } 138 | 139 | /** 140 | * @brief Returns the maximum y-coordinate of the box 141 | * 142 | * @return y-coordinate 143 | */ 144 | double max_y() const { return center_.y() + half_width_; } 145 | 146 | /** 147 | * @brief Gets all corners in counter clockwise order. 148 | * 149 | * @param corners Output where the corners are written 150 | */ 151 | void GetAllCorners(std::vector *const corners) const; 152 | 153 | /** 154 | * @brief Determines whether a given point is in the box. 155 | * 156 | * @param point The point we wish to test for containment in the box 157 | */ 158 | bool IsPointIn(const Vec2d &point) const; 159 | 160 | /** 161 | * @brief Determines whether a given point is on the boundary of the box. 162 | * 163 | * @param point The point we wish to test for boundary membership 164 | */ 165 | bool IsPointOnBoundary(const Vec2d &point) const; 166 | 167 | /** 168 | * @brief Determines the distance between a point and the box. 169 | * 170 | * @param point The point whose distance to the box we wish to determine. 171 | */ 172 | double DistanceTo(const Vec2d &point) const; 173 | 174 | /** 175 | * @brief Determines the distance between two boxes. 176 | * 177 | * @param box Another box. 178 | */ 179 | double DistanceTo(const AABox2d &box) const; 180 | 181 | /** 182 | * @brief Determines whether two boxes overlap. 183 | * 184 | * @param box Another box 185 | */ 186 | bool HasOverlap(const AABox2d &box) const; 187 | 188 | /** 189 | * @brief Shift the center of AABox by the input vector. 190 | * 191 | * @param shift_vec The vector by which we wish to shift the box 192 | */ 193 | void Shift(const Vec2d &shift_vec); 194 | 195 | /** 196 | * @brief Changes box to include another given box, as well as the current 197 | * one. 198 | * 199 | * @param other_box Another box 200 | */ 201 | void MergeFrom(const AABox2d &other_box); 202 | 203 | /** 204 | * @brief Changes box to include a given point, as well as the current box. 205 | * 206 | * @param other_point Another point 207 | */ 208 | void MergeFrom(const Vec2d &other_point); 209 | 210 | private: 211 | Vec2d center_; 212 | double length_ = 0.0; 213 | double width_ = 0.0; 214 | double half_length_ = 0.0; 215 | double half_width_ = 0.0; 216 | }; 217 | 218 | } // namespace math 219 | } // namespace planning 220 | -------------------------------------------------------------------------------- /algorithm/math/box2d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief The class of Box2d. Here, the x/y axes are respectively Forward/Left, 20 | * as opposed to what happens in euler_angles_zxy.h (Right/Forward). 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "algorithm/math/aabox2d.h" 30 | #include "algorithm/math/line_segment2d.h" 31 | #include "algorithm/math/vec2d.h" 32 | 33 | /** 34 | * @namespace apollo::common::math 35 | * @brief apollo::common::math 36 | */ 37 | namespace planning { 38 | namespace math { 39 | 40 | /** 41 | * @class Box2d 42 | * @brief Rectangular (undirected) bounding box in 2-D. 43 | * 44 | * This class is referential-agnostic, although our convention on the use of 45 | * the word "heading" in this project (permanently set to be 0 at East) 46 | * forces us to assume that the X/Y frame here is East/North. 47 | * For disambiguation, we call the axis of the rectangle parallel to the 48 | * heading direction the "heading-axis". The size of the heading-axis is 49 | * called "length", and the size of the axis perpendicular to it "width". 50 | */ 51 | class Box2d { 52 | public: 53 | Box2d() = default; 54 | /** 55 | * @brief Constructor which takes the center, heading, length and width. 56 | * @param center The center of the rectangular bounding box. 57 | * @param heading The angle between the x-axis and the heading-axis, 58 | * measured counter-clockwise. 59 | * @param length The size of the heading-axis. 60 | * @param width The size of the axis perpendicular to the heading-axis. 61 | */ 62 | Box2d(const Vec2d ¢er, const double heading, const double length, 63 | const double width); 64 | 65 | /** 66 | * @brief Constructor which takes the heading-axis and the width of the box 67 | * @param axis The heading-axis 68 | * @param width The width of the box, which is taken perpendicularly 69 | * to the heading direction. 70 | */ 71 | Box2d(const LineSegment2d &axis, const double width); 72 | 73 | /** 74 | * @brief Constructor which takes an AABox2d (axes-aligned box). 75 | * @param aabox The input AABox2d. 76 | */ 77 | explicit Box2d(const AABox2d &aabox); 78 | 79 | /** 80 | * @brief Creates an axes-aligned Box2d from two opposite corners 81 | * @param one_corner One of the corners 82 | * @param opposite_corner The opposite corner to the first one 83 | * @return An axes-aligned Box2d 84 | */ 85 | static Box2d CreateAABox(const Vec2d &one_corner, 86 | const Vec2d &opposite_corner); 87 | 88 | /** 89 | * @brief Getter of the center of the box 90 | * @return The center of the box 91 | */ 92 | const Vec2d ¢er() const { return center_; } 93 | 94 | /** 95 | * @brief Getter of the x-coordinate of the center of the box 96 | * @return The x-coordinate of the center of the box 97 | */ 98 | double center_x() const { return center_.x(); } 99 | 100 | /** 101 | * @brief Getter of the y-coordinate of the center of the box 102 | * @return The y-coordinate of the center of the box 103 | */ 104 | double center_y() const { return center_.y(); } 105 | 106 | /** 107 | * @brief Getter of the length 108 | * @return The length of the heading-axis 109 | */ 110 | double length() const { return length_; } 111 | 112 | /** 113 | * @brief Getter of the width 114 | * @return The width of the box taken perpendicularly to the heading 115 | */ 116 | double width() const { return width_; } 117 | 118 | /** 119 | * @brief Getter of half the length 120 | * @return Half the length of the heading-axis 121 | */ 122 | double half_length() const { return half_length_; } 123 | 124 | /** 125 | * @brief Getter of half the width 126 | * @return Half the width of the box taken perpendicularly to the heading 127 | */ 128 | double half_width() const { return half_width_; } 129 | 130 | /** 131 | * @brief Getter of the heading 132 | * @return The counter-clockwise angle between the x-axis and the heading-axis 133 | */ 134 | double heading() const { return heading_; } 135 | 136 | /** 137 | * @brief Getter of the cosine of the heading 138 | * @return The cosine of the heading 139 | */ 140 | double cos_heading() const { return cos_heading_; } 141 | 142 | /** 143 | * @brief Getter of the sine of the heading 144 | * @return The sine of the heading 145 | */ 146 | double sin_heading() const { return sin_heading_; } 147 | 148 | /** 149 | * @brief Getter of the area of the box 150 | * @return The product of its length and width 151 | */ 152 | double area() const { return length_ * width_; } 153 | 154 | /** 155 | * @brief Getter of the size of the diagonal of the box 156 | * @return The diagonal size of the box 157 | */ 158 | double diagonal() const { return std::hypot(length_, width_); } 159 | 160 | /** 161 | * @brief Getter of the corners of the box 162 | * @param corners The vector where the corners are listed 163 | */ 164 | void GetAllCorners(std::vector *const corners) const; 165 | 166 | /** 167 | * @brief Getter of the corners of the box 168 | * @param corners The vector where the corners are listed 169 | */ 170 | const std::vector &corners() const { return corners_; } 171 | 172 | /** 173 | * @brief Tests points for membership in the box 174 | * @param point A point that we wish to test for membership in the box 175 | * @return True iff the point is contained in the box 176 | */ 177 | bool IsPointIn(const Vec2d &point) const; 178 | 179 | /** 180 | * @brief Tests points for membership in the boundary of the box 181 | * @param point A point that we wish to test for membership in the boundary 182 | * @return True iff the point is a boundary point of the box 183 | */ 184 | bool IsPointOnBoundary(const Vec2d &point) const; 185 | 186 | /** 187 | * @brief Determines the distance between the box and a given point 188 | * @param point The point whose distance to the box we wish to compute 189 | * @return A distance 190 | */ 191 | double DistanceTo(const Vec2d &point) const; 192 | 193 | /** 194 | * @brief Determines the distance between the box and a given line segment 195 | * @param line_segment The line segment whose distance to the box we compute 196 | * @return A distance 197 | */ 198 | double DistanceTo(const LineSegment2d &line_segment) const; 199 | 200 | /** 201 | * @brief Determines the distance between two boxes 202 | * @param box The box whose distance to this box we want to compute 203 | * @return A distance 204 | */ 205 | double DistanceTo(const Box2d &box) const; 206 | 207 | /** 208 | * @brief Determines whether this box overlaps a given line segment 209 | * @param line_segment The line-segment 210 | * @return True if they overlap 211 | */ 212 | bool HasOverlap(const LineSegment2d &line_segment) const; 213 | 214 | /** 215 | * @brief Determines whether these two boxes overlap 216 | * @param line_segment The other box 217 | * @return True if they overlap 218 | */ 219 | bool HasOverlap(const Box2d &box) const; 220 | 221 | /** 222 | * @brief Gets the smallest axes-aligned box containing the current one 223 | * @return An axes-aligned box 224 | */ 225 | AABox2d GetAABox() const; 226 | 227 | /** 228 | * @brief Rotate from center. 229 | * @param rotate_angle Angle to rotate. 230 | */ 231 | void RotateFromCenter(const double rotate_angle); 232 | 233 | /** 234 | * @brief Shifts this box by a given vector 235 | * @param shift_vec The vector determining the shift 236 | */ 237 | void Shift(const Vec2d &shift_vec); 238 | 239 | /** 240 | * @brief Extend the box longitudinally 241 | * @param extension_length the length to extend 242 | */ 243 | void LongitudinalExtend(const double extension_length); 244 | 245 | void LateralExtend(const double extension_length); 246 | 247 | /** 248 | * @brief Gets a human-readable description of the box 249 | * @return A debug-string 250 | */ 251 | std::string DebugString() const; 252 | 253 | void InitCorners(); 254 | 255 | double max_x() const { return max_x_; } 256 | double min_x() const { return min_x_; } 257 | double max_y() const { return max_y_; } 258 | double min_y() const { return min_y_; } 259 | 260 | private: 261 | Vec2d center_; 262 | double length_ = 0.0; 263 | double width_ = 0.0; 264 | double half_length_ = 0.0; 265 | double half_width_ = 0.0; 266 | double heading_ = 0.0; 267 | double cos_heading_ = 1.0; 268 | double sin_heading_ = 0.0; 269 | 270 | std::vector corners_; 271 | 272 | double max_x_ = std::numeric_limits::lowest(); 273 | double min_x_ = std::numeric_limits::max(); 274 | double max_y_ = std::numeric_limits::lowest(); 275 | double min_y_ = std::numeric_limits::max(); 276 | }; 277 | 278 | } // namespace math 279 | } // namespace planning 280 | -------------------------------------------------------------------------------- /algorithm/math/line_segment2d.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include "algorithm/math/line_segment2d.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "algorithm/math/math_utils.h" 24 | 25 | namespace planning { 26 | namespace math { 27 | namespace { 28 | 29 | bool IsWithin(double val, double bound1, double bound2) { 30 | if (bound1 > bound2) { 31 | std::swap(bound1, bound2); 32 | } 33 | return val >= bound1 - kMathEpsilon && val <= bound2 + kMathEpsilon; 34 | } 35 | 36 | } // namespace 37 | 38 | LineSegment2d::LineSegment2d() { unit_direction_ = Vec2d(1, 0); } 39 | 40 | LineSegment2d::LineSegment2d(const Vec2d &start, const Vec2d &end) 41 | : start_(start), end_(end) { 42 | const double dx = end_.x() - start_.x(); 43 | const double dy = end_.y() - start_.y(); 44 | length_ = hypot(dx, dy); 45 | unit_direction_ = 46 | (length_ <= kMathEpsilon ? Vec2d(0, 0) 47 | : Vec2d(dx / length_, dy / length_)); 48 | heading_ = unit_direction_.Angle(); 49 | } 50 | 51 | Vec2d LineSegment2d::rotate(const double angle) { 52 | Vec2d diff_vec = end_ - start_; 53 | diff_vec.SelfRotate(angle); 54 | return start_ + diff_vec; 55 | } 56 | 57 | double LineSegment2d::length() const { return length_; } 58 | 59 | double LineSegment2d::length_sqr() const { return length_ * length_; } 60 | 61 | double LineSegment2d::DistanceTo(const Vec2d &point) const { 62 | if (length_ <= kMathEpsilon) { 63 | return point.DistanceTo(start_); 64 | } 65 | const double x0 = point.x() - start_.x(); 66 | const double y0 = point.y() - start_.y(); 67 | const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y(); 68 | if (proj <= 0.0) { 69 | return hypot(x0, y0); 70 | } 71 | if (proj >= length_) { 72 | return point.DistanceTo(end_); 73 | } 74 | return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x()); 75 | } 76 | 77 | double LineSegment2d::DistanceTo(const Vec2d &point, 78 | Vec2d *const nearest_pt) const { 79 | assert(nearest_pt); 80 | if (length_ <= kMathEpsilon) { 81 | *nearest_pt = start_; 82 | return point.DistanceTo(start_); 83 | } 84 | const double x0 = point.x() - start_.x(); 85 | const double y0 = point.y() - start_.y(); 86 | const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y(); 87 | if (proj < 0.0) { 88 | *nearest_pt = start_; 89 | return hypot(x0, y0); 90 | } 91 | if (proj > length_) { 92 | *nearest_pt = end_; 93 | return point.DistanceTo(end_); 94 | } 95 | *nearest_pt = start_ + unit_direction_ * proj; 96 | return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x()); 97 | } 98 | 99 | double LineSegment2d::DistanceSquareTo(const Vec2d &point) const { 100 | if (length_ <= kMathEpsilon) { 101 | return point.DistanceSquareTo(start_); 102 | } 103 | const double x0 = point.x() - start_.x(); 104 | const double y0 = point.y() - start_.y(); 105 | const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y(); 106 | if (proj <= 0.0) { 107 | return Square(x0) + Square(y0); 108 | } 109 | if (proj >= length_) { 110 | return point.DistanceSquareTo(end_); 111 | } 112 | return Square(x0 * unit_direction_.y() - y0 * unit_direction_.x()); 113 | } 114 | 115 | double LineSegment2d::DistanceToRay(const Vec2d &ray_origin, double ray_direction) { 116 | auto v1 = ray_origin - start_; 117 | auto v2 = end_ - start_; 118 | Vec2d v3(-sin(ray_direction), cos(ray_direction)); 119 | 120 | auto dot = v2.InnerProd(v3); 121 | if (std::abs(dot) < kMathEpsilon) 122 | return -1.0; 123 | 124 | auto t1 = v2.CrossProd(v1) / dot; 125 | auto t2 = v1.InnerProd(v3) / dot; 126 | 127 | if (t1 >= 0.0 && (t2 >= 0.0 && t2 <= 1.0)) 128 | return t1; 129 | 130 | return -1.0; 131 | } 132 | 133 | double LineSegment2d::DistanceSquareTo(const Vec2d &point, 134 | Vec2d *const nearest_pt) const { 135 | assert(nearest_pt); 136 | if (length_ <= kMathEpsilon) { 137 | *nearest_pt = start_; 138 | return point.DistanceSquareTo(start_); 139 | } 140 | const double x0 = point.x() - start_.x(); 141 | const double y0 = point.y() - start_.y(); 142 | const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y(); 143 | if (proj <= 0.0) { 144 | *nearest_pt = start_; 145 | return Square(x0) + Square(y0); 146 | } 147 | if (proj >= length_) { 148 | *nearest_pt = end_; 149 | return point.DistanceSquareTo(end_); 150 | } 151 | *nearest_pt = start_ + unit_direction_ * proj; 152 | return Square(x0 * unit_direction_.y() - y0 * unit_direction_.x()); 153 | } 154 | 155 | bool LineSegment2d::IsPointIn(const Vec2d &point) const { 156 | if (length_ <= kMathEpsilon) { 157 | return std::abs(point.x() - start_.x()) <= kMathEpsilon && 158 | std::abs(point.y() - start_.y()) <= kMathEpsilon; 159 | } 160 | const double prod = CrossProd(point, start_, end_); 161 | if (std::abs(prod) > kMathEpsilon) { 162 | return false; 163 | } 164 | return IsWithin(point.x(), start_.x(), end_.x()) && 165 | IsWithin(point.y(), start_.y(), end_.y()); 166 | } 167 | 168 | double LineSegment2d::ProjectOntoUnit(const Vec2d &point) const { 169 | return unit_direction_.InnerProd(point - start_); 170 | } 171 | 172 | double LineSegment2d::ProductOntoUnit(const Vec2d &point) const { 173 | return unit_direction_.CrossProd(point - start_); 174 | } 175 | 176 | bool LineSegment2d::HasIntersect(const LineSegment2d &other_segment) const { 177 | Vec2d point; 178 | return GetIntersect(other_segment, &point); 179 | } 180 | 181 | bool LineSegment2d::GetIntersect(const LineSegment2d &other_segment, 182 | Vec2d *const point) const { 183 | assert(point); 184 | if (IsPointIn(other_segment.start())) { 185 | *point = other_segment.start(); 186 | return true; 187 | } 188 | if (IsPointIn(other_segment.end())) { 189 | *point = other_segment.end(); 190 | return true; 191 | } 192 | if (other_segment.IsPointIn(start_)) { 193 | *point = start_; 194 | return true; 195 | } 196 | if (other_segment.IsPointIn(end_)) { 197 | *point = end_; 198 | return true; 199 | } 200 | if (length_ <= kMathEpsilon || other_segment.length() <= kMathEpsilon) { 201 | return false; 202 | } 203 | const double cc1 = CrossProd(start_, end_, other_segment.start()); 204 | const double cc2 = CrossProd(start_, end_, other_segment.end()); 205 | if (cc1 * cc2 >= -kMathEpsilon) { 206 | return false; 207 | } 208 | const double cc3 = 209 | CrossProd(other_segment.start(), other_segment.end(), start_); 210 | const double cc4 = 211 | CrossProd(other_segment.start(), other_segment.end(), end_); 212 | if (cc3 * cc4 >= -kMathEpsilon) { 213 | return false; 214 | } 215 | const double ratio = cc4 / (cc4 - cc3); 216 | *point = Vec2d(start_.x() * ratio + end_.x() * (1.0 - ratio), 217 | start_.y() * ratio + end_.y() * (1.0 - ratio)); 218 | return true; 219 | } 220 | 221 | // return distance with perpendicular foot point. 222 | double LineSegment2d::GetPerpendicularFoot(const Vec2d &point, 223 | Vec2d *const foot_point) const { 224 | assert(foot_point); 225 | if (length_ <= kMathEpsilon) { 226 | *foot_point = start_; 227 | return point.DistanceTo(start_); 228 | } 229 | const double x0 = point.x() - start_.x(); 230 | const double y0 = point.y() - start_.y(); 231 | const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y(); 232 | *foot_point = start_ + unit_direction_ * proj; 233 | return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x()); 234 | } 235 | 236 | std::vector LineSegment2d::SamplePoints(double step) { 237 | int num = int(length_ / step); 238 | if(length_ / step - num > 1e-5) { 239 | // warning 240 | } 241 | std::vector points(num); 242 | 243 | for (int i = 0; i < num; i++) { 244 | points[i] = start_ + unit_direction_ * step * i; 245 | } 246 | 247 | return points; 248 | } 249 | 250 | } // namespace math 251 | } // namespace planning 252 | -------------------------------------------------------------------------------- /algorithm/math/line_segment2d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief Define the LineSegment2d class. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | 27 | #include "algorithm/math/vec2d.h" 28 | 29 | /** 30 | * @namespace apollo::common::math 31 | * @brief apollo::common::math 32 | */ 33 | namespace planning { 34 | namespace math { 35 | 36 | /** 37 | * @class LineSegment2d 38 | * @brief Line segment in 2-D. 39 | */ 40 | class LineSegment2d { 41 | public: 42 | /** 43 | * @brief Empty constructor. 44 | */ 45 | LineSegment2d(); 46 | 47 | /** 48 | * @brief Constructor with start point and end point. 49 | * @param start The start point of the line segment. 50 | * @param end The end point of the line segment. 51 | */ 52 | LineSegment2d(const Vec2d &start, const Vec2d &end); 53 | 54 | /** 55 | * @brief Get the start point. 56 | * @return The start point of the line segment. 57 | */ 58 | const Vec2d &start() const { return start_; } 59 | 60 | /** 61 | * @brief Get the end point. 62 | * @return The end point of the line segment. 63 | */ 64 | const Vec2d &end() const { return end_; } 65 | 66 | /** 67 | * @brief Get the unit direction from the start point to the end point. 68 | * @return The start point of the line segment. 69 | */ 70 | const Vec2d &unit_direction() const { return unit_direction_; } 71 | 72 | /** 73 | * @brief Get the center of the line segment. 74 | * @return The center of the line segment. 75 | */ 76 | Vec2d center() const { return (start_ + end_) / 2.0; } 77 | 78 | /** @brief Get a new line-segment with the same start point, but rotated 79 | * counterclock-wise by the given amount. 80 | * @return The rotated line-segment's end-point. 81 | */ 82 | Vec2d rotate(const double angle); 83 | 84 | /** 85 | * @brief Get the heading of the line segment. 86 | * @return The heading, which is the angle between unit direction and x-axis. 87 | */ 88 | double heading() const { return heading_; } 89 | 90 | /** 91 | * @brief Get the cosine of the heading. 92 | * @return The cosine of the heading. 93 | */ 94 | double cos_heading() const { return unit_direction_.x(); } 95 | 96 | /** 97 | * @brief Get the sine of the heading. 98 | * @return The sine of the heading. 99 | */ 100 | double sin_heading() const { return unit_direction_.y(); } 101 | 102 | /** 103 | * @brief Get the length of the line segment. 104 | * @return The length of the line segment. 105 | */ 106 | double length() const; 107 | 108 | /** 109 | * @brief Get the square of length of the line segment. 110 | * @return The square of length of the line segment. 111 | */ 112 | double length_sqr() const; 113 | 114 | double DistanceToRay(const Vec2d &ray_origin, double ray_direction); 115 | 116 | /** 117 | * @brief Compute the shortest distance from a point on the line segment 118 | * to a point in 2-D. 119 | * @param point The point to compute the distance to. 120 | * @return The shortest distance from points on the line segment to point. 121 | */ 122 | double DistanceTo(const Vec2d &point) const; 123 | 124 | /** 125 | * @brief Compute the shortest distance from a point on the line segment 126 | * to a point in 2-D, and get the nearest point on the line segment. 127 | * @param point The point to compute the distance to. 128 | * @param nearest_pt The nearest point on the line segment 129 | * to the input point. 130 | * @return The shortest distance from points on the line segment 131 | * to the input point. 132 | */ 133 | double DistanceTo(const Vec2d &point, Vec2d *const nearest_pt) const; 134 | 135 | /** 136 | * @brief Compute the square of the shortest distance from a point 137 | * on the line segment to a point in 2-D. 138 | * @param point The point to compute the squared of the distance to. 139 | * @return The square of the shortest distance from points 140 | * on the line segment to the input point. 141 | */ 142 | double DistanceSquareTo(const Vec2d &point) const; 143 | 144 | /** 145 | * @brief Compute the square of the shortest distance from a point 146 | * on the line segment to a point in 2-D, 147 | * and get the nearest point on the line segment. 148 | * @param point The point to compute the squared of the distance to. 149 | * @param nearest_pt The nearest point on the line segment 150 | * to the input point. 151 | * @return The shortest distance from points on the line segment 152 | * to the input point. 153 | */ 154 | double DistanceSquareTo(const Vec2d &point, Vec2d *const nearest_pt) const; 155 | 156 | /** 157 | * @brief Check if a point is within the line segment. 158 | * @param point The point to check if it is within the line segment. 159 | * @return Whether the input point is within the line segment or not. 160 | */ 161 | bool IsPointIn(const Vec2d &point) const; 162 | 163 | /** 164 | * @brief Check if the line segment has an intersect 165 | * with another line segment in 2-D. 166 | * @param other_segment The line segment to check if it has an intersect. 167 | * @return Whether the line segment has an intersect 168 | * with the input other_segment. 169 | */ 170 | bool HasIntersect(const LineSegment2d &other_segment) const; 171 | 172 | /** 173 | * @brief Compute the intersect with another line segment in 2-D if any. 174 | * @param other_segment The line segment to compute the intersect. 175 | * @param point the computed intersect between the line segment and 176 | * the input other_segment. 177 | * @return Whether the line segment has an intersect 178 | * with the input other_segment. 179 | */ 180 | bool GetIntersect(const LineSegment2d &other_segment, 181 | Vec2d *const point) const; 182 | 183 | /** 184 | * @brief Compute the projection of a vector onto the line segment. 185 | * @param point The end of the vector (starting from the start point of the 186 | * line segment) to compute the projection onto the line segment. 187 | * @return The projection of the vector, which is from the start point of 188 | * the line segment to the input point, onto the unit direction. 189 | */ 190 | double ProjectOntoUnit(const Vec2d &point) const; 191 | 192 | /** 193 | * @brief Compute the cross product of a vector onto the line segment. 194 | * @param point The end of the vector (starting from the start point of the 195 | * line segment) to compute the cross product onto the line segment. 196 | * @return The cross product of the unit direction and 197 | * the vector, which is from the start point of 198 | * the line segment to the input point. 199 | */ 200 | double ProductOntoUnit(const Vec2d &point) const; 201 | 202 | /** 203 | * @brief Compute perpendicular foot of a point in 2-D on the straight line 204 | * expanded from the line segment. 205 | * @param point The point to compute the perpendicular foot from. 206 | * @param foot_point The computed perpendicular foot from the input point to 207 | * the straight line expanded from the line segment. 208 | * @return The distance from the input point to the perpendicular foot. 209 | */ 210 | double GetPerpendicularFoot(const Vec2d &point, 211 | Vec2d *const foot_point) const; 212 | 213 | inline std::vector SamplePoints(int num) { 214 | return SamplePoints(length_ / (num - 1)); 215 | } 216 | 217 | std::vector SamplePoints(double step); 218 | 219 | 220 | /** 221 | * @brief Get the debug string including the essential information. 222 | * @return Information of the line segment for debugging. 223 | */ 224 | std::string DebugString() const; 225 | 226 | private: 227 | Vec2d start_; 228 | Vec2d end_; 229 | Vec2d unit_direction_; 230 | double heading_ = 0.0; 231 | double length_ = 0.0; 232 | }; 233 | 234 | } // namespace math 235 | } // namespace planning 236 | -------------------------------------------------------------------------------- /algorithm/math/linear_quadratic_regulator.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include 18 | #include 19 | 20 | #include "Eigen/Dense" 21 | 22 | #include "algorithm/math/linear_quadratic_regulator.h" 23 | 24 | namespace planning { 25 | namespace math { 26 | 27 | using Matrix = Eigen::MatrixXd; 28 | 29 | // solver with cross term 30 | void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q, 31 | const Matrix &R, const Matrix &M, const double tolerance, 32 | const uint max_num_iteration, Matrix *ptr_K) { 33 | if (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() || 34 | Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() || 35 | M.rows() != Q.rows() || M.cols() != R.cols()) { 36 | std::cout << "LQR solver: one or more matrices have incompatible dimensions." << std::endl; 37 | return; 38 | } 39 | 40 | Matrix AT = A.transpose(); 41 | Matrix BT = B.transpose(); 42 | Matrix MT = M.transpose(); 43 | 44 | // Solves a discrete-time Algebraic Riccati equation (DARE) 45 | // Calculate Matrix Difference Riccati Equation, initialize P and Q 46 | Matrix P = Q; 47 | uint num_iteration = 0; 48 | double diff = std::numeric_limits::max(); 49 | while (num_iteration++ < max_num_iteration && diff > tolerance) { 50 | Matrix P_next = 51 | AT * P * A - 52 | (AT * P * B + M) * (R + BT * P * B).inverse() * (BT * P * A + MT) + Q; 53 | // check the difference between P and P_next 54 | diff = fabs((P_next - P).maxCoeff()); 55 | // std::cout << "diff: " << diff << std::endl; 56 | P = P_next; 57 | } 58 | 59 | if (num_iteration >= max_num_iteration) { 60 | std::cout << "LQR solver cannot converge to a solution, " 61 | "last consecutive result diff is: " 62 | << diff 63 | << std::endl; 64 | } else { 65 | // std::cout << "LQR solver converged at iteration: " << num_iteration 66 | // << ", max consecutive result diff.: " << diff 67 | // << std::endl; 68 | } 69 | *ptr_K = (R + BT * P * B).inverse() * (BT * P * A + MT); 70 | } 71 | 72 | void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q, 73 | const Matrix &R, const double tolerance, 74 | const uint max_num_iteration, Matrix *ptr_K) { 75 | // create M as zero matrix of the right size: 76 | // M.rows() == Q.rows() && M.cols() == R.cols() 77 | Matrix M = Matrix::Zero(Q.rows(), R.cols()); 78 | SolveLQRProblem(A, B, Q, R, M, tolerance, max_num_iteration, ptr_K); 79 | } 80 | 81 | } // namespace math 82 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/math/linear_quadratic_regulator.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief Solver for discrete-time linear quadratic problem. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include "Eigen/Core" 25 | 26 | /** 27 | * @namespace apollo::common::math 28 | * @brief apollo::common::math 29 | */ 30 | 31 | namespace planning { 32 | namespace math { 33 | 34 | /** 35 | * @brief Solver for discrete-time linear quadratic problem. 36 | * @param A The system dynamic matrix 37 | * @param B The control matrix 38 | * @param Q The cost matrix for system state 39 | * @param R The cost matrix for control output 40 | * @param M is the cross term between x and u, i.e. x'Qx + u'Ru + 2x'Mu 41 | * @param tolerance The numerical tolerance for solving Discrete 42 | * Algebraic Riccati equation (DARE) 43 | * @param max_num_iteration The maximum iterations for solving ARE 44 | * @param ptr_K The feedback control matrix (pointer) 45 | */ 46 | void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, 47 | const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, 48 | const Eigen::MatrixXd &M, const double tolerance, 49 | const uint max_num_iteration, Eigen::MatrixXd *ptr_K); 50 | 51 | /** 52 | * @brief Solver for discrete-time linear quadratic problem. 53 | * @param A The system dynamic matrix 54 | * @param B The control matrix 55 | * @param Q The cost matrix for system state 56 | * @param R The cost matrix for control output 57 | * @param tolerance The numerical tolerance for solving Discrete 58 | * Algebraic Riccati equation (DARE) 59 | * @param max_num_iteration The maximum iterations for solving ARE 60 | * @param ptr_K The feedback control matrix (pointer) 61 | */ 62 | void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, 63 | const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, 64 | const double tolerance, const uint max_num_iteration, 65 | Eigen::MatrixXd *ptr_K); 66 | 67 | } // namespace math 68 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/math/math_utils.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include "algorithm/math/math_utils.h" 18 | 19 | #define _USE_MATH_DEFINES 20 | #include 21 | #include 22 | 23 | namespace planning { 24 | namespace math { 25 | 26 | double Sqr(const double x) { return x * x; } 27 | 28 | double CrossProd(const Vec2d& start_point, const Vec2d& end_point_1, 29 | const Vec2d& end_point_2) { 30 | return (end_point_1 - start_point).CrossProd(end_point_2 - start_point); 31 | } 32 | 33 | double InnerProd(const Vec2d& start_point, const Vec2d& end_point_1, 34 | const Vec2d& end_point_2) { 35 | return (end_point_1 - start_point).InnerProd(end_point_2 - start_point); 36 | } 37 | 38 | double CrossProd(const double x0, const double y0, const double x1, 39 | const double y1) { 40 | return x0 * y1 - x1 * y0; 41 | } 42 | 43 | double InnerProd(const double x0, const double y0, const double x1, 44 | const double y1) { 45 | return x0 * x1 + y0 * y1; 46 | } 47 | 48 | double WrapAngle(const double angle) { 49 | const double new_angle = std::fmod(angle, M_PI * 2.0); 50 | return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle; 51 | } 52 | 53 | double NormalizeAngle(const double angle) { 54 | double a = std::fmod(angle + M_PI, 2.0 * M_PI); 55 | if (a < 0.0) { 56 | a += (2.0 * M_PI); 57 | } 58 | return a - M_PI; 59 | } 60 | 61 | double AngleDiff(const double from, const double to) { 62 | return NormalizeAngle(to - from); 63 | } 64 | 65 | // Gaussian 66 | double Gaussian(const double u, const double std, const double x) { 67 | return (1.0 / std::sqrt(2 * M_PI * std * std)) * 68 | std::exp(-(x - u) * (x - u) / (2 * std * std)); 69 | } 70 | 71 | Vec2d RotateVector2d(const Vec2d& v_in, 72 | const double theta) { 73 | const double cos_theta = std::cos(theta); 74 | const double sin_theta = std::sin(theta); 75 | 76 | auto x = cos_theta * v_in.x() - sin_theta * v_in.y(); 77 | auto y = sin_theta * v_in.x() + cos_theta * v_in.y(); 78 | 79 | return {x, y}; 80 | } 81 | 82 | std::pair Cartesian2Polar(double x, double y) { 83 | double r = std::sqrt(x * x + y * y); 84 | double theta = std::atan2(y, x); 85 | return std::make_pair(r, theta); 86 | } 87 | 88 | std::vector ToContinuousAngle(const std::vector &angle) { 89 | std::vector ret; 90 | ret = angle; 91 | for (size_t i = 1; i < ret.size(); i++) { 92 | while(ret[i] - ret[i-1] > M_PI + 0.001) 93 | ret[i] = ret[i] - 2 * M_PI; 94 | 95 | while(ret[i] - ret[i-1] < -M_PI- 0.001) 96 | ret[i] = ret[i] + 2 * M_PI; 97 | } 98 | return ret; 99 | } 100 | 101 | } // namespace math 102 | } // namespace planning 103 | -------------------------------------------------------------------------------- /algorithm/math/math_utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief Math-related util functions. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "algorithm/math/vec2d.h" 32 | 33 | /** 34 | * @namespace apollo::common::math 35 | * @brief apollo::common::math 36 | */ 37 | namespace planning { 38 | namespace math { 39 | 40 | double Sqr(const double x); 41 | 42 | /** 43 | * @brief Cross product between two 2-D vectors from the common start point, 44 | * and end at two other points. 45 | * @param start_point The common start point of two vectors in 2-D. 46 | * @param end_point_1 The end point of the first vector. 47 | * @param end_point_2 The end point of the second vector. 48 | * 49 | * @return The cross product result. 50 | */ 51 | double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1, 52 | const Vec2d &end_point_2); 53 | 54 | /** 55 | * @brief Inner product between two 2-D vectors from the common start point, 56 | * and end at two other points. 57 | * @param start_point The common start point of two vectors in 2-D. 58 | * @param end_point_1 The end point of the first vector. 59 | * @param end_point_2 The end point of the second vector. 60 | * 61 | * @return The inner product result. 62 | */ 63 | double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1, 64 | const Vec2d &end_point_2); 65 | 66 | /** 67 | * @brief Cross product between two vectors. 68 | * One vector is formed by 1st and 2nd parameters of the function. 69 | * The other vector is formed by 3rd and 4th parameters of the function. 70 | * @param x0 The x coordinate of the first vector. 71 | * @param y0 The y coordinate of the first vector. 72 | * @param x1 The x coordinate of the second vector. 73 | * @param y1 The y coordinate of the second vector. 74 | * 75 | * @return The cross product result. 76 | */ 77 | double CrossProd(const double x0, const double y0, const double x1, 78 | const double y1); 79 | 80 | /** 81 | * @brief Inner product between two vectors. 82 | * One vector is formed by 1st and 2nd parameters of the function. 83 | * The other vector is formed by 3rd and 4th parameters of the function. 84 | * @param x0 The x coordinate of the first vector. 85 | * @param y0 The y coordinate of the first vector. 86 | * @param x1 The x coordinate of the second vector. 87 | * @param y1 The y coordinate of the second vector. 88 | * 89 | * @return The inner product result. 90 | */ 91 | double InnerProd(const double x0, const double y0, const double x1, 92 | const double y1); 93 | 94 | /** 95 | * @brief Wrap angle to [0, 2 * PI). 96 | * @param angle the original value of the angle. 97 | * @return The wrapped value of the angle. 98 | */ 99 | double WrapAngle(const double angle); 100 | 101 | /** 102 | * @brief Normalize angle to [-PI, PI). 103 | * @param angle the original value of the angle. 104 | * @return The normalized value of the angle. 105 | */ 106 | double NormalizeAngle(const double angle); 107 | 108 | /** 109 | * @brief Calculate the difference between angle from and to 110 | * @param from the start angle 111 | * @param from the end angle 112 | * @return The difference between from and to. The range is between [-PI, PI). 113 | */ 114 | double AngleDiff(const double from, const double to); 115 | 116 | /** 117 | * @brief Compute squared value. 118 | * @param value The target value to get its squared value. 119 | * @return Squared value of the input value. 120 | */ 121 | template 122 | inline T Square(const T value) { 123 | return value * value; 124 | } 125 | 126 | /** 127 | * @brief Clamp a value between two bounds. 128 | * If the value goes beyond the bounds, return one of the bounds, 129 | * otherwise, return the original value. 130 | * @param value The original value to be clamped. 131 | * @param bound1 One bound to clamp the value. 132 | * @param bound2 The other bound to clamp the value. 133 | * @return The clamped value. 134 | */ 135 | template 136 | T Clamp(const T value, T bound1, T bound2) { 137 | if (bound1 > bound2) { 138 | std::swap(bound1, bound2); 139 | } 140 | 141 | if (value < bound1) { 142 | return bound1; 143 | } else if (value > bound2) { 144 | return bound2; 145 | } 146 | return value; 147 | } 148 | 149 | // Gaussian 150 | double Gaussian(const double u, const double std, const double x); 151 | 152 | inline double Sigmoid(const double x) { return 1.0 / (1.0 + std::exp(-x)); } 153 | 154 | // Rotate a 2d vector counter-clockwise by theta 155 | Vec2d RotateVector2d(const Vec2d &v_in, const double theta); 156 | 157 | inline std::pair RFUToFLU(const double x, const double y) { 158 | return std::make_pair(y, -x); 159 | } 160 | 161 | inline std::pair FLUToRFU(const double x, const double y) { 162 | return std::make_pair(-y, x); 163 | } 164 | 165 | inline void L2Norm(int feat_dim, float *feat_data) { 166 | if (feat_dim == 0) { 167 | return; 168 | } 169 | // feature normalization 170 | float l2norm = 0.0f; 171 | for (int i = 0; i < feat_dim; ++i) { 172 | l2norm += feat_data[i] * feat_data[i]; 173 | } 174 | if (l2norm == 0) { 175 | float val = 1.f / std::sqrt(static_cast(feat_dim)); 176 | for (int i = 0; i < feat_dim; ++i) { 177 | feat_data[i] = val; 178 | } 179 | } else { 180 | l2norm = std::sqrt(l2norm); 181 | for (int i = 0; i < feat_dim; ++i) { 182 | feat_data[i] /= l2norm; 183 | } 184 | } 185 | } 186 | 187 | /** 188 | * @brief Linear interpolation between two points of type T. 189 | * @param x0 The coordinate of the first point. 190 | * @param t0 The interpolation parameter of the first point. 191 | * @param x1 The coordinate of the second point. 192 | * @param t1 The interpolation parameter of the second point. 193 | * @param t The interpolation parameter for interpolation. 194 | * @param x The coordinate of the interpolated point. 195 | * @return Interpolated point. 196 | */ 197 | template 198 | T lerp(const T &x0, const double t0, const T &x1, const double t1, 199 | const double t) { 200 | if (std::abs(t1 - t0) <= 1.0e-6) { 201 | return x0; 202 | } 203 | const double r = (t - t0) / (t1 - t0); 204 | const T x = x0 + r * (x1 - x0); 205 | return x; 206 | } 207 | 208 | inline double slerp(const double a0, const double t0, const double a1, const double t1, 209 | const double t) { 210 | if (std::abs(t1 - t0) <= kMathEpsilon) { 211 | return NormalizeAngle(a0); 212 | } 213 | const double a0_n = NormalizeAngle(a0); 214 | const double a1_n = NormalizeAngle(a1); 215 | double d = a1_n - a0_n; 216 | if (d > M_PI) { 217 | d = d - 2 * M_PI; 218 | } else if (d < -M_PI) { 219 | d = d + 2 * M_PI; 220 | } 221 | 222 | const double r = (t - t0) / (t1 - t0); 223 | const double a = a0_n + d * r; 224 | return NormalizeAngle(a); 225 | } 226 | 227 | // Cartesian coordinates to Polar coordinates 228 | std::pair Cartesian2Polar(double x, double y); 229 | 230 | template 231 | typename std::enable_if::is_integer, bool>::type 232 | almost_equal(T x, T y, int ulp) { 233 | // the machine epsilon has to be scaled to the magnitude of the values used 234 | // and multiplied by the desired precision in ULPs (units in the last place) 235 | // unless the result is subnormal 236 | return std::fabs(x - y) <= 237 | std::numeric_limits::epsilon() * std::fabs(x + y) * ulp || 238 | std::fabs(x - y) < std::numeric_limits::min(); 239 | } 240 | 241 | std::vector ToContinuousAngle(const std::vector &angle); 242 | 243 | 244 | template 245 | inline std::array LinSpaced(double start, double end) { 246 | std::array res; 247 | double step = (end - start) / (N - 1); 248 | 249 | for(int i = 0; i < N; i++) { 250 | res[i] = start + step * i; 251 | } 252 | 253 | return res; 254 | } 255 | 256 | inline std::vector LinSpaced(double start, double end, int count) { 257 | std::vector res(count, 0); 258 | double step = (end - start) / (count - 1); 259 | 260 | for(int i = 0; i < count; i++) { 261 | res[i] = start + step * i; 262 | } 263 | 264 | return res; 265 | } 266 | 267 | inline std::vector ARange(double start, double end, double step) { 268 | std::vector res; 269 | 270 | for(int i = 0; ; i++) { 271 | auto val = start + step * i; 272 | if(val > end) { 273 | break; 274 | } 275 | res.push_back(val); 276 | } 277 | 278 | return res; 279 | } 280 | 281 | } // namespace math 282 | } // namespace planning 283 | -------------------------------------------------------------------------------- /algorithm/math/pose.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yenkn on 2021/2/24. 3 | // 4 | #pragma once 5 | 6 | #include "algorithm/math/vec2d.h" 7 | 8 | namespace planning { 9 | namespace math { 10 | 11 | class Pose { 12 | public: 13 | Pose() = default; 14 | Pose(double x, double y, double theta): x_(x), y_(y), theta_(theta) {} 15 | 16 | inline double x() const { return x_; } 17 | inline double y() const { return y_; } 18 | inline double theta() const { return theta_; } 19 | 20 | inline void setX(double x) { x_ = x; } 21 | inline void setY(double y) { y_ = y; } 22 | inline void setTheta(double theta) { theta_ = theta; } 23 | 24 | inline operator Vec2d() const { return { x_, y_}; } 25 | 26 | inline Pose relativeTo(const Pose &coord) const { 27 | double dx = x_ - coord.x(); 28 | double dy = y_ - coord.y(); 29 | return { 30 | dx * cos(coord.theta()) + dy * sin(coord.theta()), 31 | -dx * sin(coord.theta()) + dy * cos(coord.theta()), 32 | theta_ - coord.theta() 33 | }; 34 | } 35 | 36 | inline Pose extend(double length) const { 37 | return transform({ length, 0, 0 }); 38 | } 39 | 40 | inline Pose transform(const Pose &relative) const { 41 | return { 42 | x_ + relative.x() * cos(theta_) - relative.y() * sin(theta_), 43 | y_ + relative.x() * sin(theta_) + relative.y() * cos(theta_), 44 | theta_ + relative.theta_ 45 | }; 46 | } 47 | 48 | private: 49 | double x_, y_, theta_; 50 | }; 51 | 52 | } 53 | } 54 | 55 | -------------------------------------------------------------------------------- /algorithm/math/vec2d.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include "algorithm/math/vec2d.h" 18 | 19 | #include 20 | 21 | namespace planning { 22 | namespace math { 23 | 24 | Vec2d Vec2d::CreateUnitVec2d(const double angle) { 25 | return Vec2d(std::cos(angle), std::sin(angle)); 26 | } 27 | 28 | double Vec2d::Length() const { return std::hypot(x_, y_); } 29 | 30 | double Vec2d::LengthSquare() const { return x_ * x_ + y_ * y_; } 31 | 32 | double Vec2d::Angle() const { return std::atan2(y_, x_); } 33 | 34 | void Vec2d::Normalize() { 35 | const double l = Length(); 36 | if (l > kMathEpsilon) { 37 | x_ /= l; 38 | y_ /= l; 39 | } 40 | } 41 | 42 | double Vec2d::DistanceTo(const Vec2d &other) const { 43 | return std::hypot(x_ - other.x_, y_ - other.y_); 44 | } 45 | 46 | double Vec2d::DistanceSquareTo(const Vec2d &other) const { 47 | const double dx = x_ - other.x_; 48 | const double dy = y_ - other.y_; 49 | return dx * dx + dy * dy; 50 | } 51 | 52 | double Vec2d::CrossProd(const Vec2d &other) const { 53 | return x_ * other.y() - y_ * other.x(); 54 | } 55 | 56 | double Vec2d::InnerProd(const Vec2d &other) const { 57 | return x_ * other.x() + y_ * other.y(); 58 | } 59 | 60 | Vec2d Vec2d::rotate(const double angle) const { 61 | return Vec2d(x_ * cos(angle) - y_ * sin(angle), 62 | x_ * sin(angle) + y_ * cos(angle)); 63 | } 64 | 65 | void Vec2d::SelfRotate(const double angle) { 66 | double tmp_x = x_; 67 | x_ = x_ * cos(angle) - y_ * sin(angle); 68 | y_ = tmp_x * sin(angle) + y_ * cos(angle); 69 | } 70 | 71 | Vec2d Vec2d::operator+(const Vec2d &other) const { 72 | return Vec2d(x_ + other.x(), y_ + other.y()); 73 | } 74 | 75 | Vec2d Vec2d::operator-(const Vec2d &other) const { 76 | return Vec2d(x_ - other.x(), y_ - other.y()); 77 | } 78 | 79 | Vec2d Vec2d::operator*(const double ratio) const { 80 | return Vec2d(x_ * ratio, y_ * ratio); 81 | } 82 | 83 | Vec2d Vec2d::operator/(const double ratio) const { 84 | return Vec2d(x_ / ratio, y_ / ratio); 85 | } 86 | 87 | Vec2d &Vec2d::operator+=(const Vec2d &other) { 88 | x_ += other.x(); 89 | y_ += other.y(); 90 | return *this; 91 | } 92 | 93 | Vec2d &Vec2d::operator-=(const Vec2d &other) { 94 | x_ -= other.x(); 95 | y_ -= other.y(); 96 | return *this; 97 | } 98 | 99 | Vec2d &Vec2d::operator*=(const double ratio) { 100 | x_ *= ratio; 101 | y_ *= ratio; 102 | return *this; 103 | } 104 | 105 | Vec2d &Vec2d::operator/=(const double ratio) { 106 | x_ /= ratio; 107 | y_ /= ratio; 108 | return *this; 109 | } 110 | 111 | bool Vec2d::operator==(const Vec2d &other) const { 112 | return (std::abs(x_ - other.x()) < kMathEpsilon && 113 | std::abs(y_ - other.y()) < kMathEpsilon); 114 | } 115 | 116 | Vec2d operator*(const double ratio, const Vec2d &vec) { return vec * ratio; } 117 | 118 | } // namespace math 119 | } // namespace planning 120 | -------------------------------------------------------------------------------- /algorithm/math/vec2d.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file 19 | * @brief Defines the Vec2d class. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | 26 | /** 27 | * @namespace apollo::common::math 28 | * @brief apollo::common::math 29 | */ 30 | namespace planning { 31 | namespace math { 32 | 33 | constexpr double kMathEpsilon = 1e-10; 34 | 35 | /** 36 | * @class Vec2d 37 | * 38 | * @brief Implements a class of 2-dimensional vectors. 39 | */ 40 | class Vec2d { 41 | public: 42 | //! Constructor which takes x- and y-coordinates. 43 | constexpr Vec2d(const double x, const double y) noexcept : x_(x), y_(y) {} 44 | 45 | //! Constructor returning the zero vector. 46 | constexpr Vec2d() noexcept : Vec2d(0, 0) {} 47 | 48 | //! Creates a unit-vector with a given angle to the positive x semi-axis 49 | static Vec2d CreateUnitVec2d(const double angle); 50 | 51 | //! Getter for x component 52 | double x() const { return x_; } 53 | 54 | //! Getter for y component 55 | double y() const { return y_; } 56 | 57 | //! Setter for x component 58 | void set_x(const double x) { x_ = x; } 59 | 60 | //! Setter for y component 61 | void set_y(const double y) { y_ = y; } 62 | 63 | //! Gets the length of the vector 64 | double Length() const; 65 | 66 | //! Gets the squared length of the vector 67 | double LengthSquare() const; 68 | 69 | //! Gets the angle between the vector and the positive x semi-axis 70 | double Angle() const; 71 | 72 | //! Returns the unit vector that is co-linear with this vector 73 | void Normalize(); 74 | 75 | //! Returns the distance to the given vector 76 | double DistanceTo(const Vec2d &other) const; 77 | 78 | //! Returns the squared distance to the given vector 79 | double DistanceSquareTo(const Vec2d &other) const; 80 | 81 | //! Returns the "cross" product between these two Vec2d (non-standard). 82 | double CrossProd(const Vec2d &other) const; 83 | 84 | //! Returns the inner product between these two Vec2d. 85 | double InnerProd(const Vec2d &other) const; 86 | 87 | //! rotate the vector by angle. 88 | Vec2d rotate(const double angle) const; 89 | 90 | //! rotate the vector itself by angle. 91 | void SelfRotate(const double angle); 92 | 93 | //! Sums two Vec2d 94 | Vec2d operator+(const Vec2d &other) const; 95 | 96 | //! Subtracts two Vec2d 97 | Vec2d operator-(const Vec2d &other) const; 98 | 99 | //! Multiplies Vec2d by a scalar 100 | Vec2d operator*(const double ratio) const; 101 | 102 | //! Divides Vec2d by a scalar 103 | Vec2d operator/(const double ratio) const; 104 | 105 | //! Sums another Vec2d to the current one 106 | Vec2d &operator+=(const Vec2d &other); 107 | 108 | //! Subtracts another Vec2d to the current one 109 | Vec2d &operator-=(const Vec2d &other); 110 | 111 | //! Multiplies this Vec2d by a scalar 112 | Vec2d &operator*=(const double ratio); 113 | 114 | //! Divides this Vec2d by a scalar 115 | Vec2d &operator/=(const double ratio); 116 | 117 | //! Compares two Vec2d 118 | bool operator==(const Vec2d &other) const; 119 | 120 | protected: 121 | double x_ = 0.0; 122 | double y_ = 0.0; 123 | }; 124 | 125 | //! Multiplies the given Vec2d by a given scalar 126 | Vec2d operator*(const double ratio, const Vec2d &vec); 127 | 128 | } // namespace math 129 | } // namespace planning 130 | -------------------------------------------------------------------------------- /algorithm/params/planner_config.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "algorithm/params/vehicle_param.h" 15 | 16 | namespace planning { 17 | 18 | struct LateralTrackerConfig { 19 | double weight_l = 1e-1; 20 | double weight_theta = 1e-12; 21 | double weight_delta = 1e-12; 22 | double weight_delta_rate = 0.1; 23 | 24 | double preview_time = 0.2; 25 | }; 26 | 27 | struct LongitudinalTrackerConfig { 28 | double weight_s = 5.0 * 1e-1; 29 | double weight_v = 1e-12; 30 | double weight_a = 1e-12; 31 | double weight_j = 0.1; 32 | 33 | double preview_time = 0.0; 34 | }; 35 | 36 | struct TrackerConfig { 37 | double sumulation_dt = 0.01; 38 | double dt = 0.1; 39 | double tolerance = 0.01; 40 | uint max_num_iteration = 150; 41 | LateralTrackerConfig lateral_config; 42 | LongitudinalTrackerConfig longitudinal_config; 43 | }; 44 | 45 | struct Weights { 46 | double jerk = 1; 47 | double delta_rate = 1; 48 | 49 | double x_target = 0.5; 50 | double y_target = 0.5; 51 | double theta = 1e-3; 52 | double v = 0.0; 53 | double a = 0.0; 54 | double delta = 0.0; 55 | }; 56 | 57 | struct IlqrConfig { 58 | int num_of_disc = 5; 59 | double safe_margin = 0.2; 60 | double t = 100.0; 61 | double t_rate = 10.0; 62 | Weights weights; 63 | int max_iter_num = 200; 64 | 65 | double abs_cost_tol = 1e-2; 66 | double rel_cost_tol = 1e-2; 67 | 68 | double alpha = 1.0; 69 | double gamma = 0.5; 70 | double rho = 1e-9; 71 | 72 | TrackerConfig tracker_config; 73 | }; 74 | 75 | struct CorridorConfig { 76 | bool is_multiple_sample = false; 77 | double max_diff_x = 25.0; 78 | double max_diff_y = 25.0; 79 | double radius = 150.0; 80 | 81 | double max_axis_x = 10.0; 82 | double max_axis_y = 10.0; 83 | 84 | // 85 | double lane_segment_length = 5.0; 86 | }; 87 | 88 | struct PlannerConfig { 89 | /** 90 | * Number of finite elements used to discretize an OCP 91 | */ 92 | int nfe = 320; 93 | 94 | double delta_t = 0.1; 95 | 96 | /** 97 | * Time horizon length (s) 98 | */ 99 | double tf = 8; 100 | 101 | /** 102 | * nominal velocity 103 | */ 104 | double dp_nominal_velocity = 10.0; 105 | 106 | /** 107 | * cost of obstacles, should be set larger to avoid collision with obstacles 108 | */ 109 | double dp_w_obstacle = 1000; 110 | 111 | /** 112 | * lateral cost, the larger the trajectory the closer to the reference line 113 | */ 114 | double dp_w_lateral = 0.1; 115 | 116 | /** 117 | * lateral change cost, dl/ds, penalty for lateral change 118 | */ 119 | double dp_w_lateral_change = 0.5; 120 | 121 | /** 122 | * lateral change cost, dl/dt, penalty for sudden lateral change 123 | */ 124 | double dp_w_lateral_velocity_change = 1.0; 125 | 126 | /** 127 | * longitudinal velocity cost, velocity to the nominal velocity 128 | */ 129 | double dp_w_longitudinal_velocity_bias = 10.0; 130 | 131 | /** 132 | * Cost of longitudinal velocity change, ds/dt 133 | */ 134 | double dp_w_longitudinal_velocity_change = 1.0; 135 | 136 | /** 137 | * maximum iteration count for corridor expansion 138 | */ 139 | int corridor_max_iter = 1000; 140 | 141 | /** 142 | * increment limit for corridor expansion 143 | */ 144 | double corridor_incremental_limit = 20.0; 145 | 146 | /** 147 | * Weighting parameter in Eq.(2) 148 | */ 149 | double opti_w_u = 0.5; 150 | 151 | /** 152 | * weighting parameter in Eq.(3) 153 | */ 154 | double opti_w_r_theta = 2.0; 155 | 156 | /** 157 | * weighting parameter in Eq.(4) 158 | */ 159 | double opti_w_rw = 5.0; 160 | 161 | /** 162 | * Maximum iteration number in Alg.1 163 | */ 164 | int opti_iter_max = 5; 165 | 166 | /** 167 | * Initial value of weighting parameter w_penalty 168 | */ 169 | double opti_w_penalty0 = 1e5; 170 | 171 | /** 172 | * Multiplier to enlarge w_penalty during the iterations 173 | */ 174 | double opti_alpha = 10; 175 | 176 | /** 177 | * Violation tolerance w.r.t. the softened nonlinear constraints 178 | */ 179 | double opti_varepsilon_tol = 1e-4; 180 | 181 | VehicleParam vehicle; 182 | 183 | CorridorConfig corridor_config; 184 | 185 | IlqrConfig ilqr_config; 186 | 187 | TrackerConfig tracker_config; 188 | }; 189 | 190 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/params/vehicle_param.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | 16 | #include "algorithm/math/pose.h" 17 | #include "algorithm/math/box2d.h" 18 | 19 | namespace planning { 20 | 21 | class VehicleParam { 22 | public: 23 | /** 24 | * L_F, front hang length of the ego vehicle (m) 25 | */ 26 | double front_hang_length = 0.96; 27 | 28 | /** 29 | * L_W, wheelbase of the ego vehicle (m) 30 | */ 31 | double wheel_base = 1.0; 32 | 33 | /** 34 | * L_R, rear hang length of the ego vehicle (m) 35 | */ 36 | double rear_hang_length = 0.929; 37 | 38 | /** 39 | * L_B, width of the ego vehicle (m) 40 | */ 41 | double width = 1.942; 42 | 43 | /** 44 | * Upper bound of v(t) (m/s) 45 | */ 46 | double max_velocity = 20.0; 47 | 48 | /** 49 | * Lower and upper bounds of a(t) (m/s^2) 50 | */ 51 | double min_acceleration = -5.0; 52 | double max_acceleration = 5.0; 53 | 54 | /** 55 | * Upper bound of |jerk(t)| (m/s^3) 56 | */ 57 | double jerk_min = -10.0; 58 | double jerk_max = 10.0; 59 | 60 | double delta_min = -40.0 / 180 * M_PI; 61 | double delta_max = 40.0 / 180 * M_PI; 62 | 63 | double delta_rate_min = delta_min / 3.0; 64 | double delta_rate_max = delta_max / 3.0; 65 | 66 | /** 67 | * Upper bound of |\phi(t)| (rad) 68 | */ 69 | double phi_max = 0.85; 70 | 71 | /** 72 | * Upper bound of |\omega(t)| (rad/s) 73 | */ 74 | double omega_max = 1.5; 75 | 76 | double radius = 0.0; 77 | double f2x = 0.0; 78 | double r2x = 0.0; 79 | 80 | VehicleParam() { 81 | double length = (wheel_base + rear_hang_length + front_hang_length); 82 | radius = hypot(0.25 * length, 0.5 * width); 83 | r2x = 0.25 * length - rear_hang_length; 84 | f2x = 0.75 * length - rear_hang_length; 85 | } 86 | 87 | template 88 | std::tuple GetDiscPositions( 89 | const T& x, const T& y, const T& theta) const { 90 | auto xf = x + f2x * cos(theta); 91 | auto xr = x + r2x * cos(theta); 92 | auto yf = y + f2x * sin(theta); 93 | auto yr = y + r2x * sin(theta); 94 | return std::make_tuple(xf, yf, xr, yr); 95 | } 96 | 97 | math::Box2d GenerateBox(const math::Pose& pose) const { 98 | double length = (wheel_base + rear_hang_length + front_hang_length); 99 | double distance = length / 2 - rear_hang_length; 100 | return {pose.extend(distance), pose.theta(), length, width}; 101 | } 102 | 103 | }; 104 | 105 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/planner/dp_planner.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "algorithm/math/pose.h" 20 | #include "algorithm/math/vec2d.h" 21 | #include "algorithm/math/polygon2d.h" 22 | #include "algorithm/utils/environment.h" 23 | #include "algorithm/params/planner_config.h" 24 | 25 | namespace planning { 26 | 27 | constexpr int NT = 5; 28 | constexpr int NS = 7; 29 | constexpr int NL = 10; 30 | 31 | class DpPlanner { 32 | public: 33 | DpPlanner(const PlannerConfig& config, const Env& env); 34 | 35 | bool Plan( 36 | const double start_x, const double start_y, 37 | const double start_theta, DiscretizedTrajectory &result); 38 | 39 | private: 40 | struct StateCell { 41 | double cost = std::numeric_limits::max(); 42 | double current_s = std::numeric_limits::min(); 43 | int parent_s_ind = -1; 44 | int parent_l_ind = -1; 45 | 46 | StateCell() = default; 47 | 48 | StateCell(double cost, double cur_s, int parent_s_ind, int parent_l_ind) 49 | : cost(cost), current_s(cur_s) 50 | , parent_s_ind(parent_s_ind), parent_l_ind(parent_l_ind) {} 51 | }; 52 | 53 | struct StateIndex { 54 | int t = -1, s = -1, l = -1; 55 | 56 | StateIndex() = default; 57 | 58 | StateIndex(int tt, int ss, int ll) : t(tt), s(ss), l(ll) {} 59 | }; 60 | 61 | struct StartState { 62 | double start_s = 0; 63 | double start_l = 0; 64 | double start_theta = 0; 65 | }; 66 | 67 | Env env_; 68 | PlannerConfig config_; 69 | 70 | double unit_time_; 71 | std::array time_; 72 | std::array station_; 73 | std::array lateral_; 74 | 75 | StartState state_; 76 | StateCell state_space_[NT][NS][NL]; 77 | 78 | double safe_margin_; 79 | 80 | double GetCollisionCost(StateIndex parent_l_ind, StateIndex cur_ind); 81 | 82 | std::pair GetCost(StateIndex parent_ind, StateIndex cur_ind); 83 | 84 | double GetLateralOffset(double s, int l_ind) { 85 | if (l_ind == NL - 1) return 0.0; 86 | 87 | auto ref = env_->reference().EvaluateStation(s); 88 | double lb = -ref.right_bound + safe_margin_; 89 | double ub = ref.left_bound - safe_margin_; 90 | 91 | return lb + (ub - lb) * lateral_[l_ind]; 92 | } 93 | 94 | std::vector InterpolateLinearly( 95 | double parent_s, int parent_l_ind, 96 | int cur_t_ind, int cur_s_ind, int cur_l_ind); 97 | }; 98 | 99 | 100 | } // namespace planning 101 | -------------------------------------------------------------------------------- /algorithm/planner/trajectory_planner.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #include "algorithm/planner/trajectory_planner.h" 13 | 14 | #include 15 | 16 | #include "algorithm/visualization/plot.h" 17 | #include "algorithm/utils/timer.h" 18 | #include "algorithm/visualization/figure_plot.h" 19 | 20 | namespace planning { 21 | 22 | TrajectoryPlanner::TrajectoryPlanner(const PlannerConfig& config, const Env& env) 23 | : config_(config) 24 | , dp_(config, env) 25 | , corridor_(config.corridor_config, env) 26 | , ilqr_optimizer_(config.ilqr_config, config.vehicle, config.tf, config.delta_t) {} 27 | 28 | bool TrajectoryPlanner::Plan( 29 | const StartState& state, DiscretizedTrajectory& result) { 30 | DiscretizedTrajectory coarse_trajectory; 31 | utils::time dp_start_time = utils::Time(); 32 | if(!dp_.Plan(state.x, state.y, state.theta, coarse_trajectory)) { 33 | ROS_ERROR("DP failed"); 34 | return false; 35 | } 36 | utils::time dp_end_time = utils::Time(); 37 | double dp_time_cost = utils::Duration(dp_start_time, dp_end_time); 38 | std::cout << "DP time cost: " << dp_time_cost << std::endl; 39 | 40 | visualization::FigurePlot figure_plot; 41 | // figure_plot.Plot(coarse_trajectory); 42 | 43 | CorridorConstraints corridor_constraints; 44 | ConvexPolygons convex_polygons; 45 | LaneConstraints left_lane_constraints; 46 | LaneConstraints right_lane_constraints; 47 | 48 | utils::time corridor_start_time = utils::Time(); 49 | if (!corridor_.Plan(coarse_trajectory, 50 | &corridor_constraints, 51 | &convex_polygons, 52 | &left_lane_constraints, 53 | &right_lane_constraints)) { 54 | ROS_ERROR("Corridor failed"); 55 | return false; 56 | } 57 | utils::time corridor_end_time = utils::Time(); 58 | double corridor_time_cost = utils::Duration(corridor_start_time, corridor_end_time); 59 | std::cout << "Corridor time cost: " << corridor_time_cost << std::endl; 60 | 61 | // visualization::PlotConvexPolygons(convex_polygons, 0.1, visualization::Color::Cyan, 1, "Safe Corridors"); 62 | // visualization::Trigger(); 63 | 64 | std::vector coarse_x, coarse_y; 65 | for(auto &pt: coarse_trajectory.trajectory()) { 66 | coarse_x.push_back(pt.x); coarse_y.push_back(pt.y); 67 | } 68 | 69 | visualization::Plot(coarse_x, coarse_y, 0.1, visualization::Color::Red, 1, "Coarse Trajectory"); 70 | // visualization::PlotPoints(coarse_x, coarse_y, 0.3, visualization::Color::Cyan, 2, "Coarse Trajectory"); 71 | visualization::Trigger(); 72 | 73 | TrajectoryPoint start_state; 74 | start_state.x = state.x; start_state.y = state.y; 75 | start_state.velocity = state.v; start_state.theta = state.theta; 76 | DiscretizedTrajectory opt_trajectory; 77 | std::vector iter_trajs; 78 | 79 | utils::time ilqr_start_time = utils::Time(); 80 | bool status = ilqr_optimizer_.Plan(start_state, 81 | coarse_trajectory, 82 | corridor_constraints, 83 | left_lane_constraints, 84 | right_lane_constraints, 85 | &opt_trajectory, 86 | &iter_trajs); 87 | utils::time ilqr_end_time = utils::Time(); 88 | double ilqr_time_cost = utils::Duration(ilqr_start_time, ilqr_end_time); 89 | std::cout << "ilqr time cost: " << ilqr_time_cost << std::endl; 90 | 91 | if (opt_trajectory.empty()) { 92 | ROS_ERROR("ilqr failed"); 93 | return false; 94 | } 95 | 96 | figure_plot.Plot(coarse_trajectory, opt_trajectory); 97 | figure_plot.Plot(coarse_trajectory, iter_trajs, ilqr_optimizer_.cost()); 98 | 99 | // opt_trajectory = coarse_trajectory; 100 | std::vector opti_x, opti_y, opti_v; 101 | Trajectory result_data; 102 | double incremental_s = 0.0; 103 | int nfe = config_.tf / config_.delta_t + 1; 104 | for(int i = 0; i < nfe; i++) { 105 | TrajectoryPoint tp; 106 | tp.time = config_.delta_t * i; 107 | incremental_s += i > 0 ? hypot(opt_trajectory.trajectory()[i].x - opt_trajectory.trajectory()[i - 1].x, 108 | opt_trajectory.trajectory()[i].y - opt_trajectory.trajectory()[i - 1].y) : 0.0; 109 | tp.s = incremental_s; 110 | tp.x = opt_trajectory.trajectory()[i].x; 111 | tp.y = opt_trajectory.trajectory()[i].y; 112 | tp.theta = opt_trajectory.trajectory()[i].theta; 113 | tp.velocity = opt_trajectory.trajectory()[i].velocity; 114 | tp.kappa = std::tan(opt_trajectory.trajectory()[i].delta) / config_.vehicle.wheel_base; 115 | tp.a = opt_trajectory.trajectory()[i].a; 116 | tp.jerk = opt_trajectory.trajectory()[i].jerk; 117 | tp.delta = opt_trajectory.trajectory()[i].delta; 118 | tp.delta_rate = opt_trajectory.trajectory()[i].delta_rate; 119 | 120 | opti_x.push_back(tp.x); 121 | opti_y.push_back(tp.y); 122 | opti_v.push_back(tp.velocity); 123 | 124 | result_data.push_back(tp); 125 | } 126 | 127 | visualization::PlotTrajectory(opti_x, opti_y, opti_v, config_.vehicle.max_velocity, 0.1, visualization::Color::Green, 1, "Optimized Trajectory"); 128 | visualization::Trigger(); 129 | 130 | // for (int i = 0; i < iter_trajs.size(); ++i) { 131 | std::vector tem_opti_x, tem_opti_y, tem_opti_v; 132 | for(int j = 0; j < nfe; j++) { 133 | tem_opti_x.push_back(iter_trajs[0].trajectory()[j].x); 134 | tem_opti_y.push_back(iter_trajs[0].trajectory()[j].y); 135 | tem_opti_v.push_back(iter_trajs[0].trajectory()[j].velocity); 136 | } 137 | visualization::PlotTrajectory(tem_opti_x, tem_opti_y, tem_opti_v, config_.vehicle.max_velocity, 0.1, visualization::Color::Yellow, 1, "guess Optimized Trajectory"); 138 | visualization::Trigger(); 139 | // ros::Duration(0.1 * 5).sleep(); 140 | // break; 141 | // } 142 | 143 | 144 | result = DiscretizedTrajectory(result_data); 145 | convex_polygons_ = convex_polygons; 146 | 147 | left_lane_boundary_.clear(); 148 | for (const auto& seg : left_lane_constraints) { 149 | left_lane_boundary_.push_back(seg.second); 150 | } 151 | 152 | right_lane_boundary_.clear(); 153 | for (const auto& seg : right_lane_constraints) { 154 | right_lane_boundary_.push_back(seg.second); 155 | } 156 | 157 | visualization::PlotLineSegments(left_lane_boundary_, 0.1, visualization::Color::Cyan, 1, "Left Lane Segments"); 158 | visualization::PlotLineSegments(right_lane_boundary_, 0.1, visualization::Color::Cyan, 1, "Right Lane Segments"); 159 | visualization::Trigger(); 160 | 161 | return true; 162 | } 163 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/planner/trajectory_planner.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "algorithm/params/planner_config.h" 15 | #include "algorithm/planner/dp_planner.h" 16 | #include "algorithm/ilqr/corridor.h" 17 | #include "algorithm/ilqr/ilqr_optimizer.h" 18 | #include "algorithm/utils/discretized_trajectory.h" 19 | 20 | namespace planning { 21 | 22 | class TrajectoryPlanner { 23 | public: 24 | explicit TrajectoryPlanner(const PlannerConfig& config, const Env& env); 25 | 26 | bool Plan(const StartState& state, DiscretizedTrajectory& result); 27 | 28 | 29 | ConvexPolygons SafeCorridors() { 30 | return convex_polygons_; 31 | } 32 | 33 | std::vector> points_for_corridors() { 34 | return corridor_.points_for_corridors(); 35 | } 36 | 37 | std::vector left_lane_boundary() { 38 | return left_lane_boundary_; 39 | } 40 | 41 | std::vector right_lane_boundary() { 42 | return right_lane_boundary_; 43 | } 44 | 45 | private: 46 | PlannerConfig config_; 47 | DpPlanner dp_; 48 | Corridor corridor_; 49 | IlqrOptimizer ilqr_optimizer_; 50 | 51 | ConvexPolygons convex_polygons_; 52 | std::vector left_lane_boundary_; 53 | std::vector right_lane_boundary_; 54 | }; 55 | 56 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/planning_node.cc: -------------------------------------------------------------------------------- 1 | #include "algorithm/planning_node.h" 2 | 3 | #include 4 | 5 | #include "algorithm/math/math_utils.h" 6 | 7 | namespace planning { 8 | 9 | PlanningNode::PlanningNode(const ros::NodeHandle& nh) : nh_(nh) { 10 | env_ = std::make_shared(config_); 11 | planner_ = std::make_shared(config_, env_); 12 | 13 | center_line_subscriber_ = nh_.subscribe( 14 | "/center_line", 1, &PlanningNode::CenterLineCallback, this); 15 | obstacles_subscriber_ = nh_.subscribe( 16 | "/obstacles", 1, &PlanningNode::ObstaclesCallback, this); 17 | dynamic_obstacles_subscriber_ = nh_.subscribe( 18 | "/dynamic_obstacles", 1, 19 | &PlanningNode::DynamicObstaclesCallback, this); 20 | 21 | goal_subscriber_ = nh_.subscribe( 22 | "/move_base_simple/goal", 1, &PlanningNode::PlanCallback, this); 23 | 24 | state_.x = 0.0; 25 | state_.y = 0.0; 26 | state_.theta = 0.0; 27 | state_.v = 10.0; 28 | state_.phi = 0.0; 29 | state_.a = 0.0; 30 | state_.omega = 0.0; 31 | } 32 | 33 | void PlanningNode::CenterLineCallback(const CenterLineConstPtr& msg) { 34 | Trajectory data; 35 | for (auto &pt: msg->points) { 36 | TrajectoryPoint tp; 37 | tp.s = pt.s; 38 | tp.x = pt.x; 39 | tp.y = pt.y; 40 | tp.theta = pt.theta; 41 | tp.kappa = pt.kappa; 42 | tp.left_bound = pt.left_bound; 43 | tp.right_bound = pt.right_bound; 44 | data.push_back(tp); 45 | } 46 | 47 | env_->set_reference(DiscretizedTrajectory(data)); 48 | env_->Visualize(); 49 | } 50 | 51 | void PlanningNode::ObstaclesCallback(const ObstaclesConstPtr& msg) { 52 | env_->obstacles().clear(); 53 | for (auto &obstacle: msg->obstacles) { 54 | std::vector points; 55 | for (auto &pt: obstacle.points) { 56 | points.emplace_back(pt.x, pt.y); 57 | } 58 | env_->obstacles().emplace_back(points); 59 | } 60 | env_->Visualize(); 61 | } 62 | 63 | void PlanningNode::DynamicObstaclesCallback(const DynamicObstaclesConstPtr& msg) { 64 | env_->dynamic_obstacles().clear(); 65 | for (auto& obstacle: msg->obstacles) { 66 | Environment::DynamicObstacle dynamic_obstacle; 67 | 68 | for (auto& tp: obstacle.trajectory) { 69 | math::Pose coord(tp.x, tp.y, tp.theta); 70 | std::vector points; 71 | for (auto& pt: obstacle.polygon.points) { 72 | points.push_back(coord.transform({pt.x, pt.y, 0.0})); 73 | } 74 | math::Polygon2d polygon(points); 75 | dynamic_obstacle.emplace_back(tp.time, points); 76 | } 77 | env_->dynamic_obstacles().push_back(dynamic_obstacle); 78 | } 79 | env_->Visualize(); 80 | } 81 | 82 | void PlanningNode::PlanCallback(const geometry_msgs::PoseStampedConstPtr& msg) { 83 | DiscretizedTrajectory result; 84 | double dt = config_.delta_t; 85 | int nfe = config_.tf / config_.delta_t + 1; 86 | if (planner_->Plan(state_, result)) { 87 | auto convex_polygons = planner_->SafeCorridors(); 88 | auto points_for_corridors = planner_->points_for_corridors(); 89 | for (int i = 0; i < nfe; i++) { 90 | double time = dt * i; 91 | auto dynamic_obstacles = env_->QueryDynamicObstacles(time); 92 | for (auto& obstacle: dynamic_obstacles) { 93 | int hue = int((double) obstacle.first / env_->dynamic_obstacles().size() * 320); 94 | 95 | visualization::PlotPolygon( 96 | obstacle.second, 0.2, visualization::Color::fromHSV(hue, 1.0, 1.0), 97 | obstacle.first, "Online Obstacle"); 98 | } 99 | visualization::PlotConvexPolygon( 100 | convex_polygons[i], 0.1, visualization::Color::Cyan, 1, "Safe Corridors"); 101 | 102 | visualization::PlotPoints( 103 | points_for_corridors[i], 0.3, visualization::Color::Cyan, 1, "Corridor Points"); 104 | 105 | auto &pt = result.trajectory().at(i); 106 | PlotVehicle(1, {pt.x, pt.y, pt.theta}, atan(pt.kappa * config_.vehicle.wheel_base)); 107 | ros::Duration(dt * 1.5).sleep(); 108 | } 109 | 110 | visualization::Trigger(); 111 | } 112 | } 113 | 114 | void PlanningNode::PlotVehicle(const int id, const math::Pose& pt, const double phi) { 115 | auto tires = GenerateTireBoxes({pt.x(), pt.y(), pt.theta()}, phi); 116 | 117 | int tire_id = 1; 118 | for (auto &tire: tires) { 119 | visualization::PlotPolygon(math::Polygon2d(tire), 0.1, visualization::Color::White, id * (tire_id++), 120 | "Tires"); 121 | } 122 | visualization::PlotPolygon(math::Polygon2d(config_.vehicle.GenerateBox({pt.x(), pt.y(), pt.theta()})), 0.2, 123 | visualization::Color::Yellow, id, "Footprint"); 124 | visualization::Trigger(); 125 | } 126 | 127 | std::array PlanningNode::GenerateTireBoxes( 128 | const math::Pose& pose, const double phi) const { 129 | auto front_pose = pose.extend(config_.vehicle.wheel_base); 130 | auto track_width = config_.vehicle.width - 0.195; 131 | double rear_track_width_2 = track_width / 2, front_track_width_2 = track_width / 2; 132 | double box_length = 0.6345; 133 | double sin_t = sin(pose.theta()); 134 | double cos_t = cos(pose.theta()); 135 | return { 136 | math::Box2d({pose.x() - rear_track_width_2 * sin_t, pose.y() + rear_track_width_2 * cos_t}, pose.theta(), 137 | box_length, 0.195), 138 | math::Box2d({pose.x() + rear_track_width_2 * sin_t, pose.y() - rear_track_width_2 * cos_t}, pose.theta(), 139 | box_length, 0.195), 140 | math::Box2d({front_pose.x() - front_track_width_2 * sin_t, front_pose.y() + front_track_width_2 * cos_t}, 141 | front_pose.theta() + phi, box_length, 0.195), 142 | math::Box2d({front_pose.x() + front_track_width_2 * sin_t, front_pose.y() - front_track_width_2 * cos_t}, 143 | front_pose.theta() + phi, box_length, 0.195), 144 | }; 145 | } 146 | 147 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/planning_node.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geometry_msgs/PoseStamped.h" 4 | #include "planning/CenterLine.h" 5 | #include "planning/Obstacles.h" 6 | #include "planning/DynamicObstacles.h" 7 | 8 | #include "algorithm/planner/trajectory_planner.h" 9 | #include "algorithm/visualization/plot.h" 10 | 11 | namespace planning { 12 | 13 | class PlanningNode { 14 | public: 15 | explicit PlanningNode(const ros::NodeHandle& nh); 16 | 17 | private: 18 | void CenterLineCallback(const CenterLineConstPtr& msg); 19 | 20 | void ObstaclesCallback(const ObstaclesConstPtr& msg); 21 | 22 | void DynamicObstaclesCallback(const DynamicObstaclesConstPtr& msg); 23 | 24 | void PlanCallback(const geometry_msgs::PoseStampedConstPtr& msg); 25 | 26 | private: 27 | ros::NodeHandle nh_; 28 | PlannerConfig config_; 29 | Env env_; 30 | std::shared_ptr planner_; 31 | StartState state_; 32 | 33 | ros::Subscriber center_line_subscriber_; 34 | ros::Subscriber obstacles_subscriber_; 35 | ros::Subscriber dynamic_obstacles_subscriber_; 36 | ros::Subscriber goal_subscriber_; 37 | 38 | void PlotVehicle(const int id, const math::Pose& pt, const double phi); 39 | 40 | std::array GenerateTireBoxes( 41 | const math::Pose& pose, const double phi = 0.0) const; 42 | }; 43 | 44 | } // namsepace planning -------------------------------------------------------------------------------- /algorithm/slover/constraint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "typedefs.h" 6 | 7 | namespace ilqr { 8 | 9 | class Constraint { 10 | public: 11 | virtual double Evalute( 12 | const Eigen::Ref& x, 13 | const Eigen::Ref& u, 14 | const size_t step) = 0; 15 | 16 | virtual void Jacobian( 17 | const Eigen::Ref& x, 18 | const Eigen::Ref& u, 19 | const size_t step, 20 | Eigen::Ref lx, 21 | Eigen::Ref lu) = 0; 22 | 23 | virtual void Hessian( 24 | const Eigen::Ref& x, 25 | const Eigen::Ref& u, 26 | const size_t step, 27 | Eigen::Ref lxx, 28 | Eigen::Ref luu) = 0; 29 | 30 | virtual void Hessian( 31 | const Eigen::Ref& x, 32 | const Eigen::Ref& u, 33 | const size_t step, 34 | Eigen::Ref lxx, 35 | Eigen::Ref luu, 36 | Eigen::Ref lux) = 0; 37 | 38 | virtual ~Constraint() = default; 39 | } 40 | 41 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/slover/cost.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "typedefs.h" 6 | 7 | namespace ilqr { 8 | 9 | class Cost { 10 | public: 11 | virtual double Evalute( 12 | const Eigen::Ref& x, 13 | const Eigen::Ref& u, 14 | const size_t step) = 0; 15 | 16 | virtual void Jacobian( 17 | const Eigen::Ref& x, 18 | const Eigen::Ref& u, 19 | const size_t step, 20 | Eigen::Ref lx, 21 | Eigen::Ref lu) = 0; 22 | 23 | // for OCP, states and control are independent 24 | virtual void Hessian( 25 | const Eigen::Ref& x, 26 | const Eigen::Ref& u, 27 | const size_t step, 28 | Eigen::Ref lxx, 29 | Eigen::Ref luu) = 0; 30 | 31 | virtual void Hessian( 32 | const Eigen::Ref& x, 33 | const Eigen::Ref& u, 34 | const size_t step, 35 | Eigen::Ref lxx, 36 | Eigen::Ref luu, 37 | Eigen::Ref lux) = 0; 38 | 39 | virtual ~Cost() = default; 40 | } 41 | 42 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/slover/dynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "typedefs.h" 6 | 7 | namespace ilqr { 8 | 9 | class Dynamics { 10 | public: 11 | virtual void Evalute( 12 | const Eigen::Ref& x, 13 | const Eigen::Ref& u, 14 | const size_t step， 15 | Eigen::Ref x_next) = 0; 16 | 17 | virtual void Jacobian( 18 | const Eigen::Ref& x, 19 | const Eigen::Ref& u, 20 | const size_t step， 21 | Eigen::Ref fx, 22 | Eigen::Ref fu) = 0; 23 | 24 | virtual ~Dynamics() = default; 25 | }; 26 | 27 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/slover/function.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "typedefs.h" 6 | 7 | namespace ilqr { 8 | 9 | class FunctionBase { 10 | public: 11 | static constexpr int kNStates = Eigen::Dynamic; 12 | static constexpr int kNControls = Eigen::Dynamic; 13 | 14 | public: 15 | virtual ~FunctionBase() = default; 16 | 17 | virtual int StateDimesion() const { 18 | return 0; 19 | } 20 | 21 | virtual int ControlDimesion() const { 22 | return 0; 23 | } 24 | 25 | virtual void Evalute( 26 | const VectorXdRef& x, const VectorXdRef& u, Eigen::Ref out) = 0; 27 | 28 | virtual void Jacobian( 29 | const VectorXdRef& x, const VectorXdRef& u, Eigen::Ref jac) = 0; 30 | 31 | virtual void Hessian( 32 | const VectorXdRef& x, const VectorXdRef& u, const VectorXdRef& b, 33 | Eigen::Ref hess) = 0; 34 | 35 | virtual bool HasHessian() const = 0; 36 | 37 | protected: 38 | static constexpr double kDefaultTolerance = 1e-4; 39 | }; 40 | 41 | using CostFunction = std::function; 42 | using CostJacobian = std::function; 43 | using CostHessian = std::function; 44 | 45 | using DynamicsFunction = std::function; 46 | using DynamicsJacobian = std::function; 47 | 48 | using ConstraintFunction = std::function; 49 | using ConstraintJacobian = std::function; 50 | using ConstraintHessian = std::function; 51 | 52 | 53 | 54 | 55 | 56 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/slover/knot_point.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "typedefs.h" 7 | #include "function.h" 8 | 9 | namespace ilqr { 10 | 11 | template 12 | class KnotPoint { 13 | public: 14 | explicit KnotPoint(const int index, const bool is_terminal); 15 | 16 | KnotPoint(const KnotPoint& other) = delete; 17 | 18 | KnotPoint operator=(const KnotPoint& other) = delete; 19 | 20 | KnotPoint(KnotPoint&& other) = delete; 21 | 22 | void SetTimestep(const double dt); 23 | 24 | void SetCost(CostFunction cost_function, CostJacobian cost_jacobian, CostHessian cost_hessian); 25 | 26 | void SetDynamics(DynamicsFunction dynamic_function, DynamicsJacobian dynamic_jacobian); 27 | 28 | void SetConstraint( 29 | ConstraintFunction constraint_function, 30 | ConstraintJacobian constraint_jacobian, 31 | ConstraintHessian constraint_hessian); 32 | 33 | void SetBarrierFunction(); 34 | 35 | double CalcCost(); 36 | 37 | void CalcCostJacobian(); 38 | 39 | void CalcCostHessian(); 40 | 41 | void CalcDynamics(double* x_next); 42 | 43 | double CalcConstraintCosts(); 44 | 45 | void CalcConstraintJacobians(); 46 | 47 | void CalcConstraintHessians(); 48 | 49 | private: 50 | int knot_point_index = -1; 51 | double dt_ = 0.0; 52 | 53 | // cost function 54 | CostFunction cost_function_; 55 | CostJacobian cost_jacobian_; 56 | CostHessian cost_hessian_; 57 | 58 | DynamicsFunction dynamic_function_; 59 | DynamicsJacobian dynamic_jacobian_; 60 | 61 | std::vector constraint_functions_; 62 | std::vector constraint_jacobians_; 63 | std::vector constraint_hessians_; 64 | 65 | // flags 66 | bool is_terminal_ = false; 67 | 68 | // states and controls 69 | Eigen::VectorXd x_; 70 | Eigen::VectorXd u_; 71 | 72 | // backward pass 73 | Eigen::MatrixXd lxx_; 74 | Eigen::MatrixXd luu_; 75 | Eigen::MatrixXd lux_; 76 | Eigen::VectorXd lx_; 77 | Eigen::VectorXd lu_; 78 | 79 | Eigen::MatrixXd A_; 80 | Eigen::MatrixXd B_; 81 | 82 | Eigen::MatrixXd Qxx_; 83 | Eigen::MatrixXd Quu_; 84 | Eigen::MatrixXd Qux_; 85 | Eigen::MatrixXd Qx_; 86 | Eigen::MatrixXd Qu_; 87 | 88 | Eigen::MatrixXd Qxx_tmp_; 89 | Eigen::MatrixXd Quu_tmp_; 90 | Eigen::MatrixXd Qux_tmp_; 91 | Eigen::MatrixXd Qx_tmp_; 92 | Eigen::MatrixXd Qu_tmp_; 93 | 94 | Eigen::LLT Quu_fact_; 95 | // linear feedback term 96 | Eigen::MatrixXd K_; 97 | // feedforward term 98 | Eigen::VectorXd d_; 99 | 100 | // cost to go 101 | Eigen::MatrixXd P_; 102 | Eigen::VectorXd p_; 103 | 104 | double delta_V_[2]; 105 | 106 | }; 107 | 108 | template 109 | KnotPoint::KnotPoint( 110 | const int index, const bool is_terminal) 111 | : knot_point_index (index) 112 | , is_terminal_(is_terminal) { 113 | x_.resize(dim_state); 114 | u_.resize(dim_control); 115 | lxx_.resize(dim_state, dim_state); 116 | luu_.resize(dim_control, dim_control); 117 | lux_.resize(dim_control, dim_state); 118 | lx_.resize(dim_state, 1); 119 | lu_.resize(dim_control, 1); 120 | 121 | A_.resize(dim_state, dim_state); 122 | B_.resize(dim_state, dim_control); 123 | 124 | Qxx_.resize(dim_state, dim_state); 125 | Quu_.resize(dim_control, dim_control); 126 | Qux_.resize(dim_control, dim_state); 127 | Qx_.resize(dim_state, 1); 128 | Qu_.resize(dim_control, 1); 129 | 130 | Qxx_tmp_.resize(dim_state, dim_state); 131 | Quu_tmp_.resize(dim_control, dim_control); 132 | Qux_tmp_.resize(dim_control, dim_state); 133 | Qx_tmp_.resize(dim_state, 1); 134 | Qu_tmp_.resize(dim_control, 1); 135 | 136 | K_.resize(dim_control, dim_state); 137 | // feedforward term 138 | d_.resize(dim_control); 139 | 140 | // cost to go 141 | P_.resize(dim_state, dim_state); 142 | p_.resize(dim_state); 143 | 144 | } 145 | 146 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/slover/typedefs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace ilqr { 8 | 9 | using Vector = Eigen::Vector; 10 | using Matrix = Eigen::Matrix; 11 | using VectorXdRef = Eigen::Ref; 12 | 13 | typedef std::vector> VectorXdList; 14 | typedef std::vector> MatrixXdList; 15 | 16 | enum class ErrorCode { 17 | kNoError, 18 | kStateDimUnknown, 19 | kControlDimUnknown, 20 | }; 21 | 22 | enum class SolveiLQRMethod { 23 | kBarrier; 24 | kAL; 25 | }; 26 | 27 | enum class SolveGainMethod { 28 | kCholeshy, 29 | KBoxQp, 30 | }; 31 | 32 | enum class SolverStatus { 33 | kSuccess, 34 | kUnsolved, 35 | kMaxIterations, 36 | kMaxObjectiveExceeded, 37 | kStateOutOfBounds, 38 | kInputOutOfBounds, 39 | kMeritFunGradientTooSmall, 40 | }; 41 | 42 | struct Options { 43 | Options() = default; 44 | 45 | SolveiLQRMethod solve_ilqr_method = SolveiLQRMethod::kBarrier; 46 | 47 | int iter_max = 200; 48 | double tol_abs = 1e-3; 49 | double tol_rel = 1e-3; 50 | double regularization_ratio = 1.6; // 1.5 ~ 2.0 51 | double regularization_min = 1e-8; 52 | double regularization_max = 1e11; 53 | double gradient_norm_min = 1e-6; 54 | double beta_min = 1e-4; 55 | double beta_max = 10.0; 56 | SolveGainMethod solve_gain_method = SolveGainMethod::kCholeshy; 57 | }; 58 | 59 | 60 | } // namespace ilqr -------------------------------------------------------------------------------- /algorithm/utils/discrete_points_math.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2019 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file discrete_points_math.cc 19 | **/ 20 | 21 | #include "algorithm/utils/discrete_points_math.h" 22 | 23 | #include 24 | 25 | namespace planning { 26 | 27 | bool DiscretePointsMath::ComputePathProfile( 28 | const double dt, 29 | const std::vector>& xy_points, 30 | std::vector* headings, 31 | std::vector* accumulated_s, 32 | std::vector* speeds, 33 | std::vector* accelerations, 34 | std::vector* kappas) { 35 | 36 | headings->clear(); 37 | accumulated_s->clear(); 38 | speeds->clear(); 39 | accelerations->clear(); 40 | kappas->clear(); 41 | 42 | if (xy_points.size() < 2) { 43 | return false; 44 | } 45 | 46 | std::vector dxs; 47 | std::vector dys; 48 | std::vector y_over_s_first_derivatives; 49 | std::vector x_over_s_first_derivatives; 50 | std::vector y_over_s_second_derivatives; 51 | std::vector x_over_s_second_derivatives; 52 | 53 | // Get finite difference approximated dx and dy for heading and kappa 54 | // calculation 55 | std::size_t points_size = xy_points.size(); 56 | for (std::size_t i = 0; i < points_size; ++i) { 57 | double x_delta = 0.0; 58 | double y_delta = 0.0; 59 | if (i == 0) { 60 | x_delta = (xy_points[i + 1].first - xy_points[i].first); 61 | y_delta = (xy_points[i + 1].second - xy_points[i].second); 62 | } else if (i == points_size - 1) { 63 | x_delta = (xy_points[i].first - xy_points[i - 1].first); 64 | y_delta = (xy_points[i].second - xy_points[i - 1].second); 65 | } else { 66 | x_delta = 0.5 * (xy_points[i + 1].first - xy_points[i - 1].first); 67 | y_delta = 0.5 * (xy_points[i + 1].second - xy_points[i - 1].second); 68 | } 69 | dxs.push_back(x_delta); 70 | dys.push_back(y_delta); 71 | } 72 | 73 | // Heading calculation 74 | for (std::size_t i = 0; i < points_size; ++i) { 75 | headings->push_back(std::atan2(dys[i], dxs[i])); 76 | } 77 | 78 | // Get linear interpolated s for dkappa calculation 79 | double distance = 0.0; 80 | accumulated_s->push_back(distance); 81 | double fx = xy_points[0].first; 82 | double fy = xy_points[0].second; 83 | double nx = 0.0; 84 | double ny = 0.0; 85 | for (std::size_t i = 1; i < points_size; ++i) { 86 | nx = xy_points[i].first; 87 | ny = xy_points[i].second; 88 | double end_segment_s = 89 | std::sqrt((fx - nx) * (fx - nx) + (fy - ny) * (fy - ny)); 90 | accumulated_s->push_back(end_segment_s + distance); 91 | distance += end_segment_s; 92 | fx = nx; 93 | fy = ny; 94 | } 95 | 96 | for (int i = 1; i < accumulated_s->size(); ++i) { 97 | speeds->push_back((accumulated_s->at(i) - accumulated_s->at(i - 1)) / dt); 98 | } 99 | double v = speeds->back(); 100 | speeds->push_back(v); 101 | 102 | for (int i = 1; i < speeds->size(); ++i) { 103 | accelerations->push_back((speeds->at(i) - speeds->at(i - 1)) / dt); 104 | } 105 | double a = accelerations->back(); 106 | accelerations->push_back(a); 107 | 108 | 109 | // Get finite difference approximated first derivative of y and x respective 110 | // to s for kappa calculation 111 | for (std::size_t i = 0; i < points_size; ++i) { 112 | double xds = 0.0; 113 | double yds = 0.0; 114 | if (i == 0) { 115 | xds = (xy_points[i + 1].first - xy_points[i].first) / 116 | (accumulated_s->at(i + 1) - accumulated_s->at(i)); 117 | yds = (xy_points[i + 1].second - xy_points[i].second) / 118 | (accumulated_s->at(i + 1) - accumulated_s->at(i)); 119 | } else if (i == points_size - 1) { 120 | xds = (xy_points[i].first - xy_points[i - 1].first) / 121 | (accumulated_s->at(i) - accumulated_s->at(i - 1)); 122 | yds = (xy_points[i].second - xy_points[i - 1].second) / 123 | (accumulated_s->at(i) - accumulated_s->at(i - 1)); 124 | } else { 125 | xds = (xy_points[i + 1].first - xy_points[i - 1].first) / 126 | (accumulated_s->at(i + 1) - accumulated_s->at(i - 1)); 127 | yds = (xy_points[i + 1].second - xy_points[i - 1].second) / 128 | (accumulated_s->at(i + 1) - accumulated_s->at(i - 1)); 129 | } 130 | x_over_s_first_derivatives.push_back(xds); 131 | y_over_s_first_derivatives.push_back(yds); 132 | } 133 | 134 | // Get finite difference approximated second derivative of y and x respective 135 | // to s for kappa calculation 136 | for (std::size_t i = 0; i < points_size; ++i) { 137 | double xdds = 0.0; 138 | double ydds = 0.0; 139 | if (i == 0) { 140 | xdds = 141 | (x_over_s_first_derivatives[i + 1] - x_over_s_first_derivatives[i]) / 142 | (accumulated_s->at(i + 1) - accumulated_s->at(i)); 143 | ydds = 144 | (y_over_s_first_derivatives[i + 1] - y_over_s_first_derivatives[i]) / 145 | (accumulated_s->at(i + 1) - accumulated_s->at(i)); 146 | } else if (i == points_size - 1) { 147 | xdds = 148 | (x_over_s_first_derivatives[i] - x_over_s_first_derivatives[i - 1]) / 149 | (accumulated_s->at(i) - accumulated_s->at(i - 1)); 150 | ydds = 151 | (y_over_s_first_derivatives[i] - y_over_s_first_derivatives[i - 1]) / 152 | (accumulated_s->at(i) - accumulated_s->at(i - 1)); 153 | } else { 154 | xdds = (x_over_s_first_derivatives[i + 1] - 155 | x_over_s_first_derivatives[i - 1]) / 156 | (accumulated_s->at(i + 1) - accumulated_s->at(i - 1)); 157 | ydds = (y_over_s_first_derivatives[i + 1] - 158 | y_over_s_first_derivatives[i - 1]) / 159 | (accumulated_s->at(i + 1) - accumulated_s->at(i - 1)); 160 | } 161 | x_over_s_second_derivatives.push_back(xdds); 162 | y_over_s_second_derivatives.push_back(ydds); 163 | } 164 | 165 | for (std::size_t i = 0; i < points_size; ++i) { 166 | double xds = x_over_s_first_derivatives[i]; 167 | double yds = y_over_s_first_derivatives[i]; 168 | double xdds = x_over_s_second_derivatives[i]; 169 | double ydds = y_over_s_second_derivatives[i]; 170 | double kappa = 171 | (xds * ydds - yds * xdds) / 172 | (std::sqrt(xds * xds + yds * yds) * (xds * xds + yds * yds) + 1e-6); 173 | kappas->push_back(kappa); 174 | } 175 | return true; 176 | } 177 | 178 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/utils/discrete_points_math.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2019 The Apollo Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | /** 18 | * @file discrete_points_math.h 19 | **/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace planning { 27 | 28 | class DiscretePointsMath { 29 | public: 30 | DiscretePointsMath() = delete; 31 | 32 | static bool ComputePathProfile( 33 | const double dt, 34 | const std::vector>& xy_points, 35 | std::vector* headings, 36 | std::vector* accumulated_s, 37 | std::vector* speeds, 38 | std::vector* accelerations, 39 | std::vector* kappas); 40 | }; 41 | 42 | } // namespace planning 43 | -------------------------------------------------------------------------------- /algorithm/utils/discretized_trajectory.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #include "algorithm/utils/discretized_trajectory.h" 13 | 14 | #include 15 | #include 16 | 17 | #include "algorithm/math/math_utils.h" 18 | 19 | namespace planning { 20 | 21 | using math::Vec2d; 22 | 23 | DiscretizedTrajectory::DiscretizedTrajectory( 24 | const DiscretizedTrajectory& rhs, 25 | const size_t begin, const size_t end) { 26 | size_t end_new = end; 27 | if (end < 0) { 28 | end_new = rhs.trajectory_.size(); 29 | } 30 | trajectory_.resize(end_new - begin); 31 | std::copy_n(std::next(rhs.trajectory_.begin(), begin), trajectory_.size(), trajectory_.begin()); 32 | } 33 | 34 | DiscretizedTrajectory::DataType::const_iterator 35 | DiscretizedTrajectory::QueryLowerBoundStationPoint( 36 | const double station) const { 37 | if (station >= trajectory_.back().s) { 38 | return trajectory_.end() - 1; 39 | } else if (station < trajectory_.front().s) { 40 | return trajectory_.begin(); 41 | } 42 | 43 | return std::lower_bound( 44 | trajectory_.begin(), trajectory_.end(), station, 45 | [](const TrajectoryPoint& t, const double station) { 46 | return t.s < station; 47 | }); 48 | } 49 | 50 | DiscretizedTrajectory::DataType::const_iterator 51 | DiscretizedTrajectory::QueryLowerBoundTimePoint( 52 | const double time) const { 53 | if (time >= trajectory_.back().time) { 54 | return trajectory_.end() - 1; 55 | } else if (time < trajectory_.front().time) { 56 | return trajectory_.begin(); 57 | } 58 | 59 | return std::lower_bound( 60 | trajectory_.begin(), trajectory_.end(), time, 61 | [](const TrajectoryPoint& tp, const double time) { 62 | return tp.time < time; 63 | }); 64 | } 65 | 66 | TrajectoryPoint LinearInterpolateTrajectory( 67 | const TrajectoryPoint& p0, const TrajectoryPoint& p1, const double s) { 68 | double s0 = p0.s; 69 | double s1 = p1.s; 70 | if (std::abs(s1 - s0) < math::kMathEpsilon) { 71 | return p0; 72 | } 73 | 74 | TrajectoryPoint pt; 75 | double weight = (s - s0) / (s1 - s0); 76 | pt.time = (1 - weight) * p0.time + weight * p1.time; 77 | pt.s = s; 78 | pt.x = (1 - weight) * p0.x + weight * p1.x; 79 | pt.y = (1 - weight) * p0.y + weight * p1.y; 80 | pt.theta = math::slerp(p0.theta, p0.s, p1.theta, p1.s, s); 81 | pt.kappa = (1 - weight) * p0.kappa + weight * p1.kappa; 82 | pt.velocity = (1 - weight) * p0.velocity + weight * p1.velocity; 83 | pt.left_bound = (1 - weight) * p0.left_bound + weight * p1.left_bound; 84 | pt.right_bound = (1 - weight) * p0.right_bound + weight * p1.right_bound; 85 | 86 | return pt; 87 | } 88 | 89 | TrajectoryPoint LinearInterpolateTrajectoryWithTime( 90 | const TrajectoryPoint& p0, const TrajectoryPoint& p1, const double time) { 91 | double time0 = p0.time; 92 | double time1 = p1.time; 93 | if (std::fabs(time1 - time0) < math::kMathEpsilon) { 94 | return p0; 95 | } 96 | 97 | TrajectoryPoint pt; 98 | double weight = (time - time0) / (time1 - time0); 99 | pt.time = time; 100 | pt.s = (1 - weight) * p0.s + weight * p1.s; 101 | pt.x = (1 - weight) * p0.x + weight * p1.x; 102 | pt.y = (1 - weight) * p0.y + weight * p1.y; 103 | pt.theta = math::slerp(p0.theta, p0.time, p1.theta, p1.time, time); 104 | pt.kappa = (1 - weight) * p0.kappa + weight * p1.kappa; 105 | pt.velocity = (1 - weight) * p0.velocity + weight * p1.velocity; 106 | pt.left_bound = (1 - weight) * p0.left_bound + weight * p1.left_bound; 107 | pt.right_bound = (1 - weight) * p0.right_bound + weight * p1.right_bound; 108 | 109 | return pt; 110 | } 111 | 112 | TrajectoryPoint DiscretizedTrajectory::EvaluateStation( 113 | const double station) const { 114 | auto iter = QueryLowerBoundStationPoint(station); 115 | 116 | if (iter == trajectory_.begin()) { 117 | iter = std::next(iter); 118 | } 119 | 120 | auto prev = std::prev(iter, 1); 121 | 122 | return LinearInterpolateTrajectory(*prev, *iter, station); 123 | } 124 | 125 | TrajectoryPoint DiscretizedTrajectory::EvaluateTime( 126 | const double time) const { 127 | auto iter = QueryLowerBoundTimePoint(time); 128 | 129 | if (iter == trajectory_.begin()) { 130 | iter = std::next(iter); 131 | } 132 | 133 | auto prev = std::prev(iter, 1); 134 | 135 | return LinearInterpolateTrajectoryWithTime(*prev, *iter, time); 136 | } 137 | 138 | DiscretizedTrajectory::DataType::const_iterator 139 | DiscretizedTrajectory::QueryNearestPoint( 140 | const Vec2d& point, double* const out_distance) const { 141 | auto nearest_iter = trajectory_.begin(); 142 | double nearest_distance = std::numeric_limits::max(); 143 | 144 | for (auto iter = trajectory_.begin(); iter != trajectory_.end(); iter++) { 145 | double dx = iter->x - point.x(), dy = iter->y - point.y(); 146 | double distance = dx * dx + dy * dy; 147 | if (distance < nearest_distance) { 148 | nearest_iter = iter; 149 | nearest_distance = distance; 150 | } 151 | } 152 | 153 | if (out_distance != nullptr) { 154 | *out_distance = sqrt(nearest_distance); 155 | } 156 | return nearest_iter; 157 | } 158 | 159 | Vec2d DiscretizedTrajectory::GetProjection( 160 | const Vec2d& xy, 161 | TrajectoryPoint* const project_point_ptr) const { 162 | long point_idx = std::distance(trajectory_.begin(), QueryNearestPoint(xy)); 163 | auto project_point = trajectory_[point_idx]; 164 | auto index_start = std::max(0l, point_idx - 1); 165 | auto index_end = std::min(trajectory_.size() - 1, (ulong) point_idx + 1); 166 | 167 | if (index_start < index_end) { 168 | double v0x = xy.x() - trajectory_[index_start].x; 169 | double v0y = xy.y() - trajectory_[index_start].y; 170 | 171 | double v1x = trajectory_[index_end].x - trajectory_[index_start].x; 172 | double v1y = trajectory_[index_end].y - trajectory_[index_start].y; 173 | 174 | double v1_norm = std::sqrt(v1x * v1x + v1y * v1y); 175 | double dot = v0x * v1x + v0y * v1y; 176 | 177 | double delta_s = dot / v1_norm; 178 | project_point = LinearInterpolateTrajectory( 179 | trajectory_[index_start], trajectory_[index_end], trajectory_[index_start].s + delta_s); 180 | } 181 | 182 | double nr_x = xy.x() - project_point.x, nr_y = xy.y() - project_point.y; 183 | double lateral = copysign(hypot(nr_x, nr_y), nr_y * std::cos(project_point.theta) 184 | - nr_x * std::sin(project_point.theta)); 185 | 186 | if (project_point_ptr) { 187 | *project_point_ptr = project_point; 188 | } 189 | return {project_point.s, lateral}; 190 | } 191 | 192 | Vec2d DiscretizedTrajectory::GetCartesian( 193 | const double station, const double lateral) const { 194 | auto ref = EvaluateStation(station); 195 | return {ref.x - lateral * std::sin(ref.theta), ref.y + lateral * std::cos(ref.theta)}; 196 | } 197 | 198 | } // namespace planning 199 | -------------------------------------------------------------------------------- /algorithm/utils/discretized_trajectory.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "algorithm/math/vec2d.h" 19 | 20 | namespace planning { 21 | 22 | struct StartState { 23 | double x, y, theta, v, phi, a, omega; 24 | }; 25 | 26 | struct TrajectoryPoint { 27 | double time = 0.0; 28 | double s = 0.0; 29 | 30 | double x = 0.0; 31 | double y = 0.0; 32 | double theta = 0.0; 33 | double kappa = 0.0; 34 | double velocity = 0.0; 35 | 36 | double a = 0.0; 37 | double jerk = 0.0; 38 | double delta = 0.0; 39 | double delta_rate = 0.0; 40 | 41 | double left_bound = 0.0; 42 | double right_bound = 0.0; 43 | }; 44 | 45 | typedef std::vector Trajectory; 46 | 47 | /** 48 | * Discretized Trajectory 49 | */ 50 | class DiscretizedTrajectory { 51 | public: 52 | typedef std::vector DataType; 53 | 54 | DiscretizedTrajectory() = default; 55 | 56 | DiscretizedTrajectory( 57 | const DiscretizedTrajectory& rhs, 58 | const size_t begin, 59 | const size_t end = -1); 60 | 61 | explicit DiscretizedTrajectory( 62 | const std::vector& points) 63 | : trajectory_(std::move(points)) {} 64 | 65 | inline const DataType& trajectory() const { return trajectory_; } 66 | 67 | DataType::const_iterator QueryLowerBoundStationPoint( 68 | const double station) const; 69 | 70 | DataType::const_iterator QueryLowerBoundTimePoint( 71 | const double time) const; 72 | 73 | DataType::const_iterator QueryNearestPoint( 74 | const math::Vec2d& point, double* const out_distance = nullptr) const; 75 | 76 | TrajectoryPoint EvaluateStation(const double station) const; 77 | 78 | TrajectoryPoint EvaluateTime(const double time) const; 79 | 80 | math::Vec2d GetProjection( 81 | const math::Vec2d& xy, 82 | TrajectoryPoint* const project_point = nullptr) const; 83 | 84 | math::Vec2d GetCartesian(const double station, const double lateral) const; 85 | 86 | bool empty() const { 87 | return trajectory_.empty(); 88 | } 89 | 90 | 91 | protected: 92 | std::vector trajectory_; 93 | }; 94 | 95 | } // namespace planning 96 | -------------------------------------------------------------------------------- /algorithm/utils/environment.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #include "algorithm/utils/environment.h" 13 | 14 | #include "algorithm/visualization/plot.h" 15 | 16 | namespace planning { 17 | 18 | constexpr double kSampleStep = 0.1; 19 | 20 | void Environment::set_reference(const DiscretizedTrajectory& reference) { 21 | reference_ = reference; 22 | 23 | road_barrier_.clear(); 24 | left_road_barrier_.clear(); 25 | right_road_barrier_.clear(); 26 | 27 | double start_s = reference_.trajectory().front().s; 28 | double back_s = reference_.trajectory().back().s; 29 | int sample_points = int((back_s - start_s) / kSampleStep); 30 | for (int i = 0; i <= sample_points; i++) { 31 | double s = start_s + i * kSampleStep; 32 | auto ref = reference_.EvaluateStation(s); 33 | 34 | road_barrier_.push_back(reference_.GetCartesian(s, ref.left_bound)); 35 | road_barrier_.push_back(reference_.GetCartesian(s, -ref.right_bound)); 36 | 37 | left_road_barrier_.push_back(reference_.GetCartesian(s, ref.left_bound)); 38 | right_road_barrier_.push_back(reference_.GetCartesian(s, -ref.right_bound)); 39 | } 40 | 41 | std::sort(road_barrier_.begin(), road_barrier_.end(), [](const math::Vec2d& a, const math::Vec2d& b) { 42 | return a.x() < b.x(); 43 | }); 44 | } 45 | 46 | bool Environment::CheckStaticCollision(const math::Box2d& rect) { 47 | for (auto& obstacle: obstacles_) { 48 | if (obstacle.HasOverlap(rect)) { 49 | return true; 50 | } 51 | } 52 | 53 | if (road_barrier_.empty()) { 54 | return false; 55 | } 56 | 57 | if (rect.max_x() < road_barrier_.front().x() || 58 | rect.min_x() > road_barrier_.back().x()) { 59 | return false; 60 | } 61 | 62 | auto comp = [](const double val, const math::Vec2d& a) { 63 | return val < a.x(); 64 | }; 65 | 66 | // binary search 67 | auto check_start = std::upper_bound(road_barrier_.begin(), road_barrier_.end(), rect.min_x(), comp); 68 | auto check_end = std::upper_bound(road_barrier_.begin(), road_barrier_.end(), rect.max_x(), comp); 69 | 70 | if (check_start > road_barrier_.begin()) { 71 | std::advance(check_start, -1); 72 | } 73 | 74 | for (auto iter = check_start; iter != check_end; iter++) { 75 | if (rect.IsPointIn(*iter)) { 76 | return true; 77 | } 78 | } 79 | 80 | return false; 81 | } 82 | 83 | bool Environment::CheckCollision( 84 | const double time, const math::Box2d& rect) { 85 | if (CheckDynamicCollision(time, rect)) { 86 | return true; 87 | } 88 | 89 | return CheckStaticCollision(rect); 90 | } 91 | 92 | bool Environment::CheckOptimizationCollision( 93 | const double time, const math::Pose& pose, const double collision_buffer) { 94 | math::AABox2d initial_box({-config_.vehicle.radius - collision_buffer, 95 | -config_.vehicle.radius - collision_buffer}, 96 | {config_.vehicle.radius + collision_buffer, 97 | config_.vehicle.radius + collision_buffer}); 98 | 99 | double xr, yr, xf, yf; 100 | std::tie(xr, yr, xf, yf) = config_.vehicle.GetDiscPositions(pose.x(), pose.y(), pose.theta()); 101 | 102 | auto f_box = initial_box, r_box = initial_box; 103 | f_box.Shift({xf, yf}); 104 | r_box.Shift({xr, yr}); 105 | if (CheckStaticCollision(math::Box2d(f_box)) || 106 | CheckStaticCollision(math::Box2d(r_box)) || 107 | CheckDynamicCollision(time, math::Box2d(f_box)) || 108 | CheckDynamicCollision(time, math::Box2d(r_box))) { 109 | return true; 110 | } 111 | return false; 112 | } 113 | 114 | bool Environment::CheckDynamicCollision( 115 | const double time, const math::Box2d& rect) { 116 | for (auto& obstacle: dynamic_obstacles_) { 117 | if (obstacle.front().first > time || obstacle.back().first < time) { 118 | continue; 119 | } 120 | auto result = std::upper_bound(obstacle.begin(), obstacle.end(), time, 121 | [](const double val, const std::pair& ob) { 122 | return val < ob.first; 123 | }); 124 | 125 | if (result->second.HasOverlap(rect)) { 126 | return true; 127 | } 128 | } 129 | 130 | return false; 131 | } 132 | 133 | std::unordered_map 134 | Environment::QueryDynamicObstacles(const double time) { 135 | std::unordered_map filtered; 136 | int idx = 0; 137 | for (auto& obstacle: dynamic_obstacles_) { 138 | idx++; 139 | if (obstacle.front().first > time + math::kMathEpsilon || 140 | obstacle.back().first < time - math::kMathEpsilon) { 141 | continue; 142 | } 143 | auto result = std::upper_bound(obstacle.begin(), obstacle.end(), time, 144 | [](const double val, const std::pair& ob) { 145 | return val < ob.first + math::kMathEpsilon; 146 | }); 147 | 148 | filtered.insert({idx, result->second}); 149 | } 150 | return filtered; 151 | } 152 | 153 | bool Environment::QueryStaticObstaclesPoints( 154 | std::vector* const points, 155 | const bool is_multiple_sample) { 156 | if (points == nullptr) { 157 | return false; 158 | } 159 | 160 | for (const auto& obstacle : obstacles_) { 161 | std::vector corners = is_multiple_sample ? obstacle.sample_points() : obstacle.points(); 162 | points->insert(points->end(), corners.begin(), corners.end()); 163 | } 164 | return true; 165 | } 166 | 167 | bool Environment::QueryDynamicObstaclesPoints( 168 | const double time, 169 | std::vector* const points, 170 | const bool is_multiple_sample) { 171 | 172 | if (points == nullptr) { 173 | return false; 174 | } 175 | 176 | std::unordered_map obstacles = QueryDynamicObstacles(time); 177 | for (const auto& obstacle : obstacles) { 178 | std::vector corners = is_multiple_sample ? obstacle.second.sample_points() : obstacle.second.points(); 179 | points->insert(points->end(), corners.begin(), corners.end()); 180 | } 181 | return true; 182 | } 183 | 184 | void Environment::Visualize() { 185 | std::vector lb_x, lb_y, ub_x, ub_y; 186 | 187 | for (const auto& point: reference_.trajectory()) { 188 | auto lb = reference_.GetCartesian(point.s, point.left_bound); 189 | auto ub = reference_.GetCartesian(point.s, -point.right_bound); 190 | 191 | lb_x.push_back(lb.x()); 192 | lb_y.push_back(lb.y()); 193 | ub_x.push_back(ub.x()); 194 | ub_y.push_back(ub.y()); 195 | } 196 | 197 | visualization::Plot(lb_x, lb_y, 0.1, visualization::Color::Grey, 1, "Road Left"); 198 | visualization::Plot(ub_x, ub_y, 0.1, visualization::Color::Grey, 1, "Road Right"); 199 | 200 | int idx = 0; 201 | for (auto& obstacle: obstacles_) { 202 | visualization::PlotPolygon(obstacle, 0.1, visualization::Color::Magenta, idx++, "Obstacles"); 203 | } 204 | 205 | // plot first frame of dynamic obstacles 206 | idx = 1; 207 | for (auto& obstacle: dynamic_obstacles_) { 208 | auto color = visualization::Color::fromHSV(int((double) idx / dynamic_obstacles_.size() * 320), 1.0, 1.0); 209 | color.set_alpha(0.5); 210 | visualization::PlotPolygon(obstacle[0].second, 0.1, color, idx, "Online Obstacle"); 211 | idx++; 212 | } 213 | 214 | visualization::Trigger(); 215 | } 216 | 217 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/utils/environment.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | 17 | #include "algorithm/math/polygon2d.h" 18 | #include "algorithm/utils/discretized_trajectory.h" 19 | #include "algorithm/params/vehicle_param.h" 20 | #include "algorithm/params/planner_config.h" 21 | 22 | namespace planning { 23 | 24 | class Environment { 25 | public: 26 | using DynamicObstacle = std::vector>; 27 | 28 | explicit Environment(const PlannerConfig& config) : config_(config) {} 29 | 30 | std::vector& obstacles() { 31 | return obstacles_; 32 | } 33 | 34 | bool QueryDynamicObstaclesPoints( 35 | const double time, 36 | std::vector* const points, 37 | const bool is_multiple_sample = false); 38 | 39 | bool QueryStaticObstaclesPoints( 40 | std::vector* const points, 41 | const bool is_multiple_sample = false); 42 | 43 | std::vector& dynamic_obstacles() { 44 | return dynamic_obstacles_; 45 | } 46 | 47 | const std::vector& left_road_barrier() { 48 | return left_road_barrier_; 49 | } 50 | 51 | const std::vector& right_road_barrier() { 52 | return right_road_barrier_; 53 | } 54 | 55 | const DiscretizedTrajectory& reference() const { 56 | return reference_; 57 | } 58 | 59 | void set_reference(const DiscretizedTrajectory& reference); 60 | 61 | bool CheckCollision(const double time, const math::Box2d& rect); 62 | 63 | bool CheckOptimizationCollision( 64 | const double time, const math::Pose& pose, 65 | const double collision_buffer = 0.0); 66 | 67 | std::unordered_map QueryDynamicObstacles( 68 | const double time); 69 | 70 | void Visualize(); 71 | 72 | private: 73 | bool CheckStaticCollision(const math::Box2d& rect); 74 | 75 | bool CheckDynamicCollision(const double time, const math::Box2d& rect); 76 | 77 | private: 78 | PlannerConfig config_; 79 | std::vector dynamic_obstacles_; 80 | std::vector obstacles_; 81 | DiscretizedTrajectory reference_; 82 | std::vector road_barrier_; 83 | 84 | std::vector left_road_barrier_; 85 | std::vector right_road_barrier_; 86 | }; 87 | 88 | using Env = std::shared_ptr; 89 | 90 | } // namespace planning 91 | -------------------------------------------------------------------------------- /algorithm/utils/timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace planning { 6 | namespace utils { 7 | 8 | // 毫秒 1 s = 1000 ms 9 | typedef std::chrono::time_point time; 11 | static inline time Time() { 12 | return std::chrono::time_point_cast( 13 | std::chrono::steady_clock::now()); 14 | } 15 | 16 | static inline double Duration(const time& start, const time& end) { 17 | return static_cast(std::chrono::duration_cast(end - start).count()) / 1e6; 18 | } 19 | 20 | } // namespace utils 21 | } // namespace planning -------------------------------------------------------------------------------- /algorithm/visualization/color.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #include "algorithm/visualization/color.h" 13 | #include 14 | 15 | namespace planning { 16 | namespace visualization { 17 | 18 | Color Color::Black(0.0, 0.0, 0.0); 19 | Color Color::Grey(0.7, 0.7, 0.7); 20 | Color Color::White(1.0, 1.0, 1.0); 21 | Color Color::Red(1.0, 0.0, 0.0); 22 | Color Color::Green(0.0, 1.0, 0.0); 23 | Color Color::Blue(0.0, 0.0, 1.0); 24 | Color Color::Cyan(0.0, 1.0, 1.0); 25 | Color Color::Yellow(1.0, 1.0, 0.0); 26 | Color Color::Magenta(1.0, 0.0, 1.0); 27 | 28 | Color Color::fromHSV(int H, double S, double V) { 29 | double C = S * V; 30 | double X = C * (1 - fabs(fmod(H / 60.0, 2) - 1)); 31 | double m = V - C; 32 | double Rs, Gs, Bs; 33 | 34 | if (H >= 0 && H < 60) { 35 | Rs = C; 36 | Gs = X; 37 | Bs = 0; 38 | } else if (H >= 60 && H < 120) { 39 | Rs = X; 40 | Gs = C; 41 | Bs = 0; 42 | } else if (H >= 120 && H < 180) { 43 | Rs = 0; 44 | Gs = C; 45 | Bs = X; 46 | } else if (H >= 180 && H < 240) { 47 | Rs = 0; 48 | Gs = X; 49 | Bs = C; 50 | } else if (H >= 240 && H < 300) { 51 | Rs = X; 52 | Gs = 0; 53 | Bs = C; 54 | } else { 55 | Rs = C; 56 | Gs = 0; 57 | Bs = X; 58 | } 59 | 60 | return Color(Rs + m, Gs + m, Bs + m); 61 | } 62 | 63 | void Color::toHSV(float &fH, float &fS, float &fV) const { 64 | float fCMax = std::max(std::max(r_, g_), b_); 65 | float fCMin = std::min(std::min(r_, g_), b_); 66 | float fDelta = fCMax - fCMin; 67 | 68 | if (fDelta > 0) { 69 | if (fCMax == r_) { 70 | fH = 60 * (std::fmod(((g_ - b_) / fDelta), 6)); 71 | } else if (fCMax == g_) { 72 | fH = 60 * (((b_ - r_) / fDelta) + 2); 73 | } else if (fCMax == b_) { 74 | fH = 60 * (((r_ - g_) / fDelta) + 4); 75 | } 76 | 77 | if (fCMax > 0) { 78 | fS = fDelta / fCMax; 79 | } else { 80 | fS = 0; 81 | } 82 | 83 | fV = fCMax; 84 | } else { 85 | fH = 0; 86 | fS = 0; 87 | fV = fCMax; 88 | } 89 | 90 | if (fH < 0) { 91 | fH = 360 + fH; 92 | } 93 | } 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /algorithm/visualization/color.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | using std_msgs::ColorRGBA; 23 | 24 | namespace planning { 25 | namespace visualization { 26 | 27 | class Color { 28 | public: 29 | Color() = default; 30 | 31 | Color(float r, float g, float b) : r_(r), g_(g), b_(b), a_(1.0) {} 32 | 33 | std::string toPlotColor() const { 34 | char buf[10]; 35 | snprintf(buf, 10, "#%02x%02x%02x", uint8_t(r_ * 255), uint8_t(g_ * 255), uint8_t(b_ * 255)); 36 | return buf; 37 | } 38 | 39 | ColorRGBA toColorRGBA() const { 40 | ColorRGBA color; 41 | color.r = r_; 42 | color.g = g_; 43 | color.b = b_; 44 | color.a = a_; 45 | return color; 46 | } 47 | 48 | static Color Black; 49 | static Color Grey; 50 | static Color White; 51 | static Color Red; 52 | static Color Green; 53 | static Color Blue; 54 | static Color Cyan; 55 | static Color Yellow; 56 | static Color Magenta; 57 | 58 | static Color fromRGB(float r, float g, float b) { 59 | return Color(std::min(r, 1.0f), std::min(g, 1.0f), std::min(b, 1.0f)); 60 | } 61 | 62 | /* 63 | * H(Hue): 0 - 360 degree (integer) 64 | * S(Saturation): 0 - 1.00 (double) 65 | * V(Value): 0 - 1.00 (double) 66 | */ 67 | static Color fromHSV(int H, double S, double V); 68 | 69 | /*! \brief Convert RGB to HSV color space 70 | 71 | Converts a given set of RGB values `r', `g', `b' into HSV 72 | coordinates. The input RGB values are in the range [0, 1], and the 73 | output HSV values are in the ranges h = [0, 360], and s, v = [0, 74 | 1], respectively. 75 | 76 | \param fH Hue component, used as output, range: [0, 360] 77 | \param fS Hue component, used as output, range: [0, 1] 78 | \param fV Hue component, used as output, range: [0, 1] 79 | 80 | */ 81 | void toHSV(float &fH, float &fS, float &fV) const; 82 | 83 | inline double r() { return r_; } 84 | 85 | inline double g() { return g_; } 86 | 87 | inline double b() { return b_; } 88 | 89 | inline void set_alpha(double alpha) { 90 | a_ = alpha; 91 | } 92 | 93 | 94 | private: 95 | double r_, g_, b_, a_; 96 | 97 | }; 98 | 99 | } 100 | } -------------------------------------------------------------------------------- /algorithm/visualization/plot.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #include "algorithm/visualization/plot.h" 13 | 14 | namespace planning { 15 | namespace visualization { 16 | namespace { 17 | std::string frame_ = "map"; 18 | std::mutex mutex_; 19 | 20 | ros::Publisher publisher_; 21 | visualization_msgs::MarkerArray arr_; 22 | } 23 | 24 | void Init(ros::NodeHandle &node, const std::string &frame, const std::string &topic) { 25 | frame_ = frame; 26 | publisher_ = node.advertise(topic, 10, true); 27 | } 28 | 29 | void 30 | Plot(const Vector &xs, const Vector &ys, double width, Color color, int id, const std::string &ns) { 31 | visualization_msgs::Marker msg; 32 | msg.header.frame_id = frame_; 33 | msg.header.stamp = ros::Time(); 34 | msg.ns = ns; 35 | msg.id = id >= 0 ? id : arr_.markers.size(); 36 | 37 | msg.action = visualization_msgs::Marker::ADD; 38 | msg.type = visualization_msgs::Marker::LINE_STRIP; 39 | msg.pose.orientation.w = 1.0; 40 | msg.scale.x = width; 41 | msg.color = color.toColorRGBA(); 42 | 43 | for (size_t i = 0; i < xs.size(); i++) { 44 | geometry_msgs::Point pt; 45 | pt.x = xs[i]; 46 | pt.y = ys[i]; 47 | pt.z = 0.1 * id; 48 | msg.points.push_back(pt); 49 | } 50 | 51 | mutex_.lock(); 52 | arr_.markers.push_back(msg); 53 | mutex_.unlock(); 54 | } 55 | 56 | void Plot(const Vector &xs, const Vector &ys, double width, 57 | const std::vector &color, int id, const std::string &ns) { 58 | assert(xs.size() == color.size()); 59 | 60 | visualization_msgs::Marker msg; 61 | msg.header.frame_id = frame_; 62 | msg.header.stamp = ros::Time(); 63 | msg.ns = ns; 64 | msg.id = id >= 0 ? id : arr_.markers.size(); 65 | 66 | msg.action = visualization_msgs::Marker::ADD; 67 | msg.type = visualization_msgs::Marker::LINE_STRIP; 68 | msg.pose.orientation.w = 1.0; 69 | msg.scale.x = width; 70 | 71 | for (size_t i = 0; i < xs.size(); i++) { 72 | geometry_msgs::Point pt; 73 | pt.x = xs[i]; 74 | pt.y = ys[i]; 75 | msg.points.push_back(pt); 76 | msg.colors.push_back(color[i].toColorRGBA()); 77 | } 78 | 79 | mutex_.lock(); 80 | arr_.markers.push_back(msg); 81 | mutex_.unlock(); 82 | } 83 | 84 | 85 | void PlotPolygon(const Vector &xs, const Vector &ys, double width, Color color, int id, 86 | const std::string &ns) { 87 | auto xxs = xs; 88 | auto yys = ys; 89 | xxs.push_back(xxs[0]); 90 | yys.push_back(yys[0]); 91 | Plot(xxs, yys, width, color, id, ns); 92 | } 93 | 94 | void PlotPolygon(const Polygon2d &polygon, double width, Color color, int id, 95 | const std::string &ns) { 96 | std::vector xs, ys; 97 | for (auto &pt: polygon.points()) { 98 | xs.push_back(pt.x()); 99 | ys.push_back(pt.y()); 100 | } 101 | PlotPolygon(xs, ys, width, color, id, ns); 102 | } 103 | 104 | void PlotConvexPolygon( 105 | const std::vector& polygon, 106 | double width, 107 | Color color, 108 | int id, const std::string &ns) { 109 | std::vector xs, ys; 110 | for (const auto& pt: polygon) { 111 | xs.push_back(pt[0]); 112 | ys.push_back(pt[1]); 113 | } 114 | PlotPolygon(xs, ys, width, color, id, ns); 115 | } 116 | 117 | void PlotConvexPolygons( 118 | const std::vector>& polygons, 119 | double width, 120 | Color color, 121 | int id, const std::string &ns) { 122 | int i = 0; 123 | for (const auto& p : polygons) { 124 | ++i; 125 | std::string name_space = ns + std::to_string(i); 126 | PlotConvexPolygon(p, width, color, id, name_space); 127 | } 128 | } 129 | 130 | void PlotPoint( 131 | const math::Vec2d& pt, 132 | double width, 133 | Color color, 134 | int id, const std::string &ns) { 135 | visualization_msgs::Marker msg; 136 | msg.header.frame_id = frame_; 137 | msg.header.stamp = ros::Time(); 138 | msg.ns = ns; 139 | msg.id = id >= 0 ? id : arr_.markers.size(); 140 | 141 | msg.action = visualization_msgs::Marker::ADD; 142 | msg.type = visualization_msgs::Marker::SPHERE; 143 | msg.pose.position.x = pt.x(); 144 | msg.pose.position.y = pt.y(); 145 | msg.pose.position.z = 0; 146 | msg.pose.position.y = pt.y(); 147 | msg.pose.position.z = 0; 148 | msg.pose.orientation.w = 1.0; 149 | msg.scale.x = width * 2.0; 150 | msg.scale.y = width * 2.0; 151 | msg.scale.z = 0.01; 152 | msg.color = color.toColorRGBA(); 153 | 154 | mutex_.lock(); 155 | arr_.markers.push_back(msg); 156 | mutex_.unlock(); 157 | } 158 | 159 | void PlotPoints( 160 | const std::vector& pts, 161 | double width, 162 | Color color, 163 | int id, const std::string &ns) { 164 | int i = 0; 165 | for (const auto& pt : pts) { 166 | ++i; 167 | std::string name_space = ns + std::to_string(i); 168 | PlotPoint(pt, width, color, id, name_space); 169 | } 170 | } 171 | 172 | void PlotLineSegment( 173 | const math::LineSegment2d& line, 174 | double width, 175 | Color color, 176 | int id, const std::string &ns) { 177 | std::vector xs, ys; 178 | xs.push_back(line.start().x()); 179 | xs.push_back(line.end().x()); 180 | ys.push_back(line.start().y()); 181 | ys.push_back(line.end().y()); 182 | Plot(xs, ys, width, color, id, ns); 183 | // PlotPoint(line.start(), width, color, id, ns + "1"); 184 | // PlotPoint(line.end(), width, Color::Red, id, ns + "2"); 185 | } 186 | 187 | void PlotLineSegments( 188 | const std::vector& lines, 189 | double width, 190 | Color color, 191 | int id, const std::string &ns) { 192 | int i = 0; 193 | for (const auto& line : lines) { 194 | ++i; 195 | std::string name_space = ns + std::to_string(i); 196 | PlotLineSegment(line, width, color, id, name_space); 197 | } 198 | } 199 | 200 | void PlotTrajectory(const Vector &xs, const Vector &ys, const Vector &vs, double max_velocity, double width, 201 | const Color &color, int id, const std::string &ns) { 202 | std::vector colors(xs.size()); 203 | float h, tmp; 204 | color.toHSV(h, tmp, tmp); 205 | 206 | for (size_t i = 0; i < xs.size(); i++) { 207 | double percent = (vs[i] / max_velocity); 208 | colors[i] = Color::fromHSV(h, percent, 1.0); 209 | } 210 | 211 | Plot(xs, ys, width, colors, id, ns); 212 | } 213 | 214 | void PlotPoints(const Vector &xs, const Vector &ys, double width, const Color &color, int id, 215 | const std::string &ns) { 216 | assert(xs.size() == ys.size()); 217 | 218 | visualization_msgs::Marker msg; 219 | msg.header.frame_id = frame_; 220 | msg.header.stamp = ros::Time(); 221 | msg.ns = ns.empty() ? "Points" : ns; 222 | msg.id = id >= 0 ? id : arr_.markers.size(); 223 | 224 | msg.action = visualization_msgs::Marker::ADD; 225 | msg.type = visualization_msgs::Marker::POINTS; 226 | msg.pose.orientation.w = 1.0; 227 | msg.scale.x = msg.scale.y = width; 228 | msg.color = color.toColorRGBA(); 229 | 230 | for (size_t i = 0; i < xs.size(); i++) { 231 | geometry_msgs::Point pt; 232 | pt.x = xs[i]; 233 | pt.y = ys[i]; 234 | msg.points.push_back(pt); 235 | } 236 | 237 | mutex_.lock(); 238 | arr_.markers.push_back(msg); 239 | mutex_.unlock(); 240 | } 241 | 242 | void Trigger() { 243 | mutex_.lock(); 244 | publisher_.publish(arr_); 245 | arr_.markers.clear(); 246 | mutex_.unlock(); 247 | } 248 | 249 | void Clear() { 250 | mutex_.lock(); 251 | arr_.markers.clear(); 252 | 253 | visualization_msgs::MarkerArray arr; 254 | visualization_msgs::Marker msg; 255 | msg.header.frame_id = frame_; 256 | msg.ns = "Markers"; 257 | 258 | msg.action = visualization_msgs::Marker::DELETEALL; 259 | arr.markers.push_back(msg); 260 | publisher_.publish(arr); 261 | mutex_.unlock(); 262 | } 263 | } 264 | } -------------------------------------------------------------------------------- /algorithm/visualization/plot.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 3 | * Frenet Frame: A Cartesian-based Trajectory Planning Method". 4 | *********************************************************************************** 5 | * Copyright (C) 2022 Bai Li 6 | * Users are suggested to cite the following article when they use the source codes. 7 | * Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 8 | * Frenet Frame: A Cartesian-based Trajectory Planning Method", 9 | * IEEE Transactions on Intelligent Transportation Systems, 2022. 10 | ***********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "algorithm/params/vehicle_param.h" 15 | 16 | #include "algorithm/math/vec2d.h" 17 | #include "algorithm/math/polygon2d.h" 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "algorithm/visualization/color.h" 26 | 27 | namespace planning { 28 | namespace visualization { 29 | 30 | using math::Vec2d; 31 | using math::Polygon2d; 32 | 33 | using Vector = std::vector; 34 | 35 | void Init(ros::NodeHandle &node, const std::string &frame, const std::string &topic); 36 | 37 | void Plot(const Vector &xs, const Vector &ys, double width = 0.1, Color color = Color(1, 1, 1), 38 | int id = -1, const std::string &ns = ""); 39 | 40 | void Plot(const Vector &xs, const Vector &ys, double width = 0.1, const std::vector &color = {}, 41 | int id = -1, const std::string &ns = ""); 42 | 43 | void PlotPolygon(const Vector &xs, const Vector &ys, double width = 0.1, Color color = Color::White, 44 | int id = -1, const std::string &ns = ""); 45 | 46 | void PlotPolygon(const Polygon2d &polygon, double width = 0.1, Color color = Color::White, 47 | int id = -1, const std::string &ns = ""); 48 | 49 | void PlotConvexPolygon( 50 | const std::vector& polygon, 51 | double width = 0.1, 52 | Color color = Color::White, 53 | int id = -1, const std::string &ns = ""); 54 | 55 | void PlotConvexPolygons( 56 | const std::vector>& polygon, 57 | double width = 0.1, 58 | Color color = Color::White, 59 | int id = -1, const std::string &ns = ""); 60 | 61 | void PlotPoint( 62 | const math::Vec2d& pt, 63 | double width = 0.1, 64 | Color color = Color::White, 65 | int id = -1, const std::string &ns = ""); 66 | 67 | void PlotPoints( 68 | const std::vector& pt, 69 | double width = 0.1, 70 | Color color = Color::White, 71 | int id = -1, const std::string &ns = ""); 72 | 73 | void PlotLineSegment( 74 | const math::LineSegment2d& line, 75 | double width = 0.1, 76 | Color color = Color::White, 77 | int id = -1, const std::string &ns = ""); 78 | 79 | void PlotLineSegments( 80 | const std::vector& lines, 81 | double width = 0.1, 82 | Color color = Color::White, 83 | int id = -1, const std::string &ns = ""); 84 | 85 | void PlotTrajectory(const Vector &xs, const Vector &ys, const Vector &vs, double max_velocity = 10.0, 86 | double width = 0.1, const Color &color = Color::Blue, 87 | int id = -1, const std::string &ns = ""); 88 | 89 | void PlotPoints(const Vector &xs, const Vector &ys, double width = 0.1, const Color &color = Color::White, 90 | int id = -1, const std::string &ns = ""); 91 | 92 | void Trigger(); 93 | 94 | void Clear(); 95 | } // namespace visualization 96 | 97 | } // namespace planning -------------------------------------------------------------------------------- /cmake/FindHSL.cmake: -------------------------------------------------------------------------------- 1 | # TRY TO FIND THE HSL LIBRARY 2 | 3 | find_library(HSL_LIB 4 | names hsl coinhsl 5 | HINTS $ENV{HSL}) 6 | 7 | if(HSL_LIB) 8 | message(STATUS "Found HSL: ${HSL_LIB}") 9 | set(HSL_LIBRARIES ${HSL_LIB}) 10 | set(HSL_FOUND TRUE) 11 | else(HSL_LIB) 12 | set(HSL_FOUND FALSE) 13 | message(STATUS "Could not find HSL; looking in environmental variable HSL ($ENV{HSL})") 14 | endif(HSL_LIB) 15 | -------------------------------------------------------------------------------- /config/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /MarkerArray1 11 | - /MarkerArray1/Namespaces1 12 | Splitter Ratio: 0.5726495981216431 13 | Tree Height: 989 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 2 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 100 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Buffer Length: 1 60 | Class: rviz/Path 61 | Color: 114; 159; 207 62 | Enabled: true 63 | Head Diameter: 0.30000001192092896 64 | Head Length: 0.20000000298023224 65 | Length: 0.30000001192092896 66 | Line Style: Lines 67 | Line Width: 0.029999999329447746 68 | Name: CenterLine 69 | Offset: 70 | X: 0 71 | Y: 0 72 | Z: 0 73 | Pose Color: 255; 85; 255 74 | Pose Style: None 75 | Radius: 0.029999999329447746 76 | Shaft Diameter: 0.10000000149011612 77 | Shaft Length: 0.10000000149011612 78 | Topic: /center_line_path 79 | Unreliable: false 80 | Value: true 81 | - Class: rviz/MarkerArray 82 | Enabled: true 83 | Marker Topic: /trajectory_planner_markers 84 | Name: MarkerArray 85 | Namespaces: 86 | Coarse Trajectory: true 87 | Footprint: true 88 | Front Corridor: false 89 | Intermediate Trajectory: false 90 | Obstacles: true 91 | Online Obstacle: true 92 | Optimized Trajectory: true 93 | Rear Corridor: false 94 | Road Left: true 95 | Road Right: true 96 | Tires: true 97 | Queue Size: 100 98 | Value: true 99 | Enabled: true 100 | Global Options: 101 | Background Color: 48; 48; 48 102 | Default Light: true 103 | Fixed Frame: map 104 | Frame Rate: 30 105 | Name: root 106 | Tools: 107 | - Class: rviz/Interact 108 | Hide Inactive Objects: true 109 | - Class: rviz/MoveCamera 110 | - Class: rviz/Select 111 | - Class: rviz/FocusCamera 112 | - Class: rviz/Measure 113 | - Class: rviz/SetInitialPose 114 | Theta std deviation: 0.2617993950843811 115 | Topic: /initialpose 116 | X std deviation: 0.5 117 | Y std deviation: 0.5 118 | - Class: rviz/SetGoal 119 | Topic: /move_base_simple/goal 120 | - Class: rviz/PublishPoint 121 | Single click: true 122 | Topic: /clicked_point 123 | Value: true 124 | Views: 125 | Current: 126 | Angle: 0 127 | Class: rviz/TopDownOrtho 128 | Enable Stereo Rendering: 129 | Stereo Eye Separation: 0.05999999865889549 130 | Stereo Focal Distance: 1 131 | Swap Stereo Eyes: false 132 | Value: false 133 | Invert Z Axis: false 134 | Name: Current View 135 | Near Clip Distance: 0.009999999776482582 136 | Scale: 17.24561309814453 137 | Target Frame: 138 | Value: TopDownOrtho (rviz) 139 | X: 42.037025451660156 140 | Y: -1.8880302906036377 141 | Saved: ~ 142 | Window Geometry: 143 | Displays: 144 | collapsed: true 145 | Height: 1321 146 | Hide Left Dock: true 147 | Hide Right Dock: true 148 | QMainWindow State: 000000ff00000000fd00000004000000000000016100000479fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004700000479000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000452fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004700000452000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074c00000041fc0100000002fb0000000800540069006d006501000000000000074c0000037100fffffffb0000000800540069006d006501000000000000045000000000000000000000074c0000047900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 149 | Selection: 150 | collapsed: false 151 | Time: 152 | collapsed: false 153 | Tool Properties: 154 | collapsed: false 155 | Views: 156 | collapsed: true 157 | Width: 1868 158 | X: 638 159 | Y: 34 160 | -------------------------------------------------------------------------------- /launch/pedestrian_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/random_pedestrian_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /main.cc: -------------------------------------------------------------------------------- 1 | #include "algorithm/planning_node.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "trajectory_planner_node"); 5 | 6 | ros::NodeHandle nh; 7 | planning::visualization::Init(nh, "map", "trajectory_planner_markers"); 8 | 9 | planning::PlanningNode node(nh); 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /msg/CenterLine.msg: -------------------------------------------------------------------------------- 1 | CenterLinePoint[] points 2 | -------------------------------------------------------------------------------- /msg/CenterLinePoint.msg: -------------------------------------------------------------------------------- 1 | float64 s 2 | float64 x 3 | float64 y 4 | float64 theta 5 | float64 kappa 6 | float64 left_bound 7 | float64 right_bound 8 | -------------------------------------------------------------------------------- /msg/DynamicObstacle.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Polygon polygon 2 | DynamicTrajectoryPoint[] trajectory 3 | -------------------------------------------------------------------------------- /msg/DynamicObstacles.msg: -------------------------------------------------------------------------------- 1 | DynamicObstacle[] obstacles 2 | -------------------------------------------------------------------------------- /msg/DynamicTrajectoryPoint.msg: -------------------------------------------------------------------------------- 1 | float64 time 2 | float64 x 3 | float64 y 4 | float64 theta 5 | -------------------------------------------------------------------------------- /msg/Obstacles.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Polygon[] obstacles 2 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | planning 4 | 0.0.1 5 | The planning package 6 | hh 7 | GPL 8 | 9 | catkin 10 | roscpp 11 | roscpp 12 | roscpp 13 | eigen 14 | message_generation 15 | message_runtime 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /resources/cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mpt0816/Cilqr/f4e10ba7c69ef6450c1f5d633cb8f9f48e83df98/resources/cost.png -------------------------------------------------------------------------------- /resources/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mpt0816/Cilqr/f4e10ba7c69ef6450c1f5d633cb8f9f48e83df98/resources/demo.png -------------------------------------------------------------------------------- /resources/demo.webm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mpt0816/Cilqr/f4e10ba7c69ef6450c1f5d633cb8f9f48e83df98/resources/demo.webm -------------------------------------------------------------------------------- /resources/iter_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mpt0816/Cilqr/f4e10ba7c69ef6450c1f5d633cb8f9f48e83df98/resources/iter_results.png -------------------------------------------------------------------------------- /resources/results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mpt0816/Cilqr/f4e10ba7c69ef6450c1f5d633cb8f9f48e83df98/resources/results.png -------------------------------------------------------------------------------- /script/pickle_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """*********************************************************************************** 3 | C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 4 | Frenet Frame: A Cartesian-based Trajectory Planning Method". 5 | *********************************************************************************** 6 | Copyright (C) 2022 Bai Li 7 | Users are suggested to cite the following article when they use the source codes. 8 | Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 9 | Frenet Frame: A Cartesian-based Trajectory Planning Method", 10 | IEEE Transactions on Intelligent Transportation Systems, 2022. 11 | ***********************************************************************************""" 12 | import pickle 13 | import sys 14 | import rospy 15 | from os import path 16 | from planning.msg import CenterLine, Obstacles, DynamicObstacles 17 | from nav_msgs.msg import Path 18 | from geometry_msgs.msg import PoseStamped 19 | 20 | 21 | def main(): 22 | rospy.init_node('pickle_publisher') 23 | 24 | pickle_path = path.join(path.dirname(__file__), 'reference.pickle') 25 | if sys.argv[0].endswith('.pickle'): 26 | pickle_path = sys.argv[0] 27 | 28 | with open(pickle_path) as f: 29 | reference = pickle.load(f) 30 | 31 | center_pub = rospy.Publisher('/center_line', CenterLine, queue_size=1, latch=True) 32 | center_pub.publish(reference['center']) 33 | 34 | if 'static' in reference: 35 | obstacles_pub = rospy.Publisher('/obstacles', Obstacles, queue_size=1, latch=True) 36 | obstacles_pub.publish(reference['static']) 37 | 38 | if 'dynamic' in reference: 39 | obstacles_pub = rospy.Publisher('/dynamic_obstacles', DynamicObstacles, queue_size=1, latch=True) 40 | obstacles_pub.publish(reference['dynamic']) 41 | 42 | if 'path' in sys.argv: 43 | ref_pub = rospy.Publisher('/center_line_path', Path, queue_size=1, latch=True) 44 | msg = Path() 45 | msg.header.stamp = rospy.Time.now() 46 | msg.header.frame_id = 'map' 47 | for pt in reference['center'].points: 48 | pose = PoseStamped() 49 | pose.header = msg.header 50 | pose.pose.position.x = pt.x 51 | pose.pose.position.y = pt.y 52 | msg.poses.append(pose) 53 | 54 | ref_pub.publish(msg) 55 | 56 | rospy.spin() 57 | 58 | 59 | if __name__ == '__main__': 60 | main() 61 | -------------------------------------------------------------------------------- /script/reference_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """*********************************************************************************** 3 | C++ Source Codes for "Autonomous Driving on Curvy Roads without Reliance on 4 | Frenet Frame: A Cartesian-based Trajectory Planning Method". 5 | *********************************************************************************** 6 | Copyright (C) 2022 Bai Li 7 | Users are suggested to cite the following article when they use the source codes. 8 | Bai Li et al., "Autonomous Driving on Curvy Roads without Reliance on 9 | Frenet Frame: A Cartesian-based Trajectory Planning Method", 10 | IEEE Transactions on Intelligent Transportation Systems, 2022. 11 | ***********************************************************************************""" 12 | import bisect 13 | import sys 14 | import rospy 15 | import numpy as np 16 | import pickle 17 | import os 18 | from geometry_msgs.msg import PoseStamped 19 | from nav_msgs.msg import Path 20 | from planning.msg import CenterLine, CenterLinePoint, Obstacles, DynamicObstacles, DynamicObstacle, \ 21 | DynamicTrajectoryPoint 22 | from geometry_msgs.msg import Polygon, Point32 23 | 24 | 25 | def generate_center_line(config, start_x=0.0, start_y=0.0, start_yaw=0.0, resolution=0.1, left_bound=2.5, 26 | right_bound=6.0): 27 | x = start_x 28 | y = start_y 29 | yaw = start_yaw 30 | 31 | incremental_s = 0.0 32 | center_line = CenterLine() 33 | center_line.points = [ 34 | CenterLinePoint(s=0.0, x=x, y=y, theta=yaw, kappa=0.0, left_bound=left_bound, right_bound=right_bound)] 35 | 36 | for seg in config: 37 | if isinstance(seg, list): 38 | [degree, radius] = seg 39 | angle = np.deg2rad(degree) 40 | arc_direction = -1 if angle < 0 else 1 41 | arc_length = angle * radius 42 | kappa = 1 / radius * arc_direction 43 | start_angle = yaw - np.pi / 2 * arc_direction 44 | end_angle = start_angle + angle 45 | 46 | center_yaw = yaw + np.pi / 2 * arc_direction 47 | xc = x + radius * np.cos(center_yaw) 48 | yc = y + radius * np.sin(center_yaw) 49 | 50 | point_count = np.floor(np.abs(arc_length) / resolution) 51 | angles = np.linspace(start_angle, end_angle, point_count) 52 | 53 | yaw_inc = angle / point_count 54 | 55 | for ang in angles: 56 | x = xc + radius * np.cos(ang) 57 | y = yc + radius * np.sin(ang) 58 | incremental_s += resolution 59 | yaw += yaw_inc 60 | 61 | center_line.points.append( 62 | CenterLinePoint(s=incremental_s, x=x, y=y, theta=yaw, kappa=kappa, left_bound=left_bound, 63 | right_bound=right_bound)) 64 | else: 65 | for i in range(int(seg / resolution)): 66 | x += resolution * np.cos(yaw) 67 | y += resolution * np.sin(yaw) 68 | incremental_s += resolution 69 | center_line.points.append( 70 | CenterLinePoint(s=incremental_s, x=x, y=y, theta=yaw, kappa=0, left_bound=left_bound, 71 | right_bound=right_bound)) 72 | 73 | if len(center_line.points) > 1: 74 | center_line.points[0].kappa = center_line.points[1].kappa 75 | return center_line 76 | 77 | 78 | def convert_frenet_to_cartesian(ref, laterals): 79 | xs = ref[:, 1] - laterals * np.sin(ref[:, 3]) 80 | ys = ref[:, 2] + laterals * np.cos(ref[:, 3]) 81 | return xs, ys 82 | 83 | 84 | def transform_footprint(x, y, theta, footprint): 85 | points = [ 86 | [-footprint[0] / 2, -footprint[1] / 2, 1], 87 | [-footprint[0] / 2, footprint[1] / 2, 1], 88 | [footprint[0] / 2, footprint[1] / 2, 1], 89 | [footprint[0] / 2, -footprint[1] / 2, 1], 90 | ] 91 | 92 | rotation = np.array([ 93 | [np.cos(theta), -np.sin(theta), x], 94 | [np.sin(theta), np.cos(theta), y], 95 | [0, 0, 1] 96 | ]) 97 | 98 | return np.dot(points, rotation.T)[:, :2] 99 | 100 | 101 | def get_random_reference_points(center_line, count, start_idx=100, back_idx=500): 102 | random_stations = np.random.randint(start_idx, len(center_line.points) - back_idx, count) 103 | 104 | ref_s = [] 105 | ref_x = [] 106 | ref_y = [] 107 | ref_theta = [] 108 | for i in random_stations: 109 | ref_s.append(center_line.points[i].s) 110 | ref_x.append(center_line.points[i].x) 111 | ref_y.append(center_line.points[i].y) 112 | ref_theta.append(center_line.points[i].theta) 113 | return np.vstack([ref_s, ref_x, ref_y, ref_theta]).transpose() 114 | 115 | 116 | def generate_random_vehicles(center_line, count, length=4.0, width=2.0): 117 | lateral_samples = np.array([1.0, 0.0, -4.0]) 118 | random_laterals = lateral_samples[np.random.randint(0, len(lateral_samples), count)] 119 | 120 | ref = get_random_reference_points(center_line, count) 121 | ref_theta = ref[:, 3] 122 | ob_x, ob_y = convert_frenet_to_cartesian(ref, random_laterals) 123 | 124 | msg = Obstacles() 125 | for i in range(count): 126 | footprint = transform_footprint(ob_x[i], ob_y[i], ref_theta[i], [length, width]) 127 | polygon = Polygon(points=[Point32(x=row[0], y=row[1]) for row in footprint]) 128 | msg.obstacles.append(polygon) 129 | 130 | return msg 131 | 132 | 133 | def generate_random_dynamic_vehicles(center_line, count, horizon=16.0, dt=0.1): 134 | cls = [pt.s for pt in center_line.points] 135 | max_s = cls[-1] 136 | ref = get_random_reference_points(center_line, count, back_idx=1000) 137 | ref_center_line = np.array([[pt.s, pt.x, pt.y, pt.theta] for pt in center_line.points]) 138 | 139 | velocities = 4 + 2 * np.random.rand(count) 140 | footprint = transform_footprint(0, 0, 0, [4.0, 2.0]) 141 | vehicle = Polygon(points=[Point32(x=row[0], y=row[1]) for row in footprint]) 142 | 143 | msg = DynamicObstacles() 144 | for i in range(count): 145 | start_s = ref[i, 0] 146 | start_s_ind = bisect.bisect_left(cls, start_s) 147 | 148 | traj_len = int(horizon / dt) + 1 149 | s_ind = np.linspace(start_s_ind, bisect.bisect_left(cls, min(max_s, start_s + velocities[i] * horizon)), 150 | traj_len, dtype=np.int) 151 | 152 | rand_lateral = 0.0 if np.random.rand() > 0.5 else -4.0 153 | traj_x, traj_y = convert_frenet_to_cartesian(ref_center_line[s_ind, :], np.repeat(rand_lateral, traj_len)) 154 | traj_theta = ref_center_line[s_ind, 3] 155 | 156 | tps = [DynamicTrajectoryPoint(time=j * dt, x=traj_x[j], y=traj_y[j], theta=traj_theta[j]) for j in 157 | range(traj_len)] 158 | msg.obstacles.append(DynamicObstacle(polygon=vehicle, trajectory=tps)) 159 | 160 | return msg 161 | 162 | 163 | def generate_random_pedestrian(center_line, count, dt=0.1, ego_velocity=20.0): 164 | ref = get_random_reference_points(center_line, count) 165 | 166 | velocities = 0.4 + np.random.rand(count) 167 | road_lb = -center_line.points[0].right_bound - 1.0 168 | road_ub = center_line.points[0].left_bound + 1.0 169 | distance = road_ub - road_lb 170 | 171 | pedestrian = Polygon(points=[ 172 | Point32(x=-0.5, y=-0.5, z=0.0), 173 | Point32(x=-0.5, y=0.5, z=0.0), 174 | Point32(x=0.5, y=0.5, z=0.0), 175 | Point32(x=0.5, y=-0.5, z=0.0), 176 | ]) 177 | 178 | msg = DynamicObstacles() 179 | for i in range(count): 180 | s = ref[i, 0] 181 | 182 | traj_len = int(distance / velocities[i] / dt) 183 | laterals = np.linspace(road_ub, road_lb, traj_len) if np.random.rand(1) > 0.5 else np.linspace(road_lb, road_ub, 184 | traj_len) 185 | 186 | traj_x, traj_y = convert_frenet_to_cartesian(ref[np.repeat(i, traj_len), :], laterals) 187 | 188 | time_offset = s / ego_velocity 189 | 190 | tps = [DynamicTrajectoryPoint(time=time_offset + j * dt, x=traj_x[j], y=traj_y[j], theta=0) for j in 191 | range(traj_len)] 192 | msg.obstacles.append(DynamicObstacle(polygon=pedestrian, trajectory=tps)) 193 | 194 | return msg 195 | 196 | 197 | def main(): 198 | rospy.init_node('reference_publisher') 199 | 200 | config = [ 201 | 30, 202 | # degree, radius 203 | [-90, 10], 204 | 10, 205 | [180, 5], 206 | 36, 207 | [-180, 12], 208 | 50, 209 | ] 210 | center_line = generate_center_line(config) 211 | center_pub = rospy.Publisher('/center_line', CenterLine, queue_size=1, latch=True) 212 | center_pub.publish(center_line) 213 | 214 | static_obstacles = None 215 | if 'static' in sys.argv: 216 | static_obstacles = generate_random_vehicles(center_line, 2) 217 | obstacles_pub = rospy.Publisher('/obstacles', Obstacles, queue_size=1, latch=True) 218 | obstacles_pub.publish(static_obstacles) 219 | 220 | dynamic_obstacles = None 221 | if 'dynamic' in sys.argv or 'pedestrian' in sys.argv: 222 | dynamic_obstacles = DynamicObstacles() 223 | if 'pedestrian' in sys.argv: 224 | dynamic_obstacles.obstacles.extend(generate_random_pedestrian(center_line, 6).obstacles) 225 | 226 | if 'dynamic' in sys.argv: 227 | dynamic_obstacles.obstacles.extend(generate_random_dynamic_vehicles(center_line, 3).obstacles) 228 | 229 | dynamic_obstacles_pub = rospy.Publisher('/dynamic_obstacles', DynamicObstacles, queue_size=1, latch=True) 230 | dynamic_obstacles_pub.publish(dynamic_obstacles) 231 | 232 | if 'serialize' in sys.argv: 233 | pickle_path = os.path.join(os.path.dirname(__file__), 'reference.pickle') 234 | with open(pickle_path, 'w') as f: 235 | pickle.dump({"center": center_line, "static": static_obstacles, "dynamic": dynamic_obstacles}, f) 236 | print('pickle saved to %s' % pickle_path) 237 | 238 | if 'path' in sys.argv: 239 | ref_pub = rospy.Publisher('/center_line_path', Path, queue_size=1, latch=True) 240 | msg = Path() 241 | msg.header.stamp = rospy.Time.now() 242 | msg.header.frame_id = 'map' 243 | for pt in center_line.points: 244 | pose = PoseStamped() 245 | pose.header = msg.header 246 | pose.pose.position.x = pt.x 247 | pose.pose.position.y = pt.y 248 | msg.poses.append(pose) 249 | 250 | ref_pub.publish(msg) 251 | 252 | rospy.spin() 253 | 254 | 255 | if __name__ == '__main__': 256 | main() 257 | -------------------------------------------------------------------------------- /script/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rm -rf ./core 4 | 5 | echo "" > log.txt 6 | 7 | sudo sysctl -p 8 | core_dir=/home/mpt/catkin_ws/core 9 | [ ! -d ${core_dir} ] && mkdir -p ${core_dir} 10 | ulimit -c unlimited 11 | sudo sysctl -w kernel.core_pattern=${core}/core.%e.%p 12 | sudo sysctl -p 13 | 14 | source devel/setup.bash 15 | roslaunch planning pedestrian_test.launch --------------------------------------------------------------------------------