driving-planning/path_plan_test.cc at v1.0 · hs867785578/driving-planning · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include "EMPlanner/EMPlanner.h"
#include "localization/localization_estimate.h"
#include "perception/perception_obstacle.h"
#include "plot/plot.h"
#include "reference_line/reference_line_provider.h"
#include "routing/routing_path.h"
#include <memory>
#include <unistd.h>

using namespace std;
namespace plt = matplotlibcpp;

int main(int argc, char const *argv[]) {
  std::vector<double> x = {0};
  std::vector<double> y = {0};

  plt::plot(x, y);

  //构造路由模块指针
  std::unique_ptr<RoutingPath> routing_path =
      std::make_unique<RoutingPath>(); //注意是make_unique

  //定位信息指针
  std::unique_ptr<LocalizationEstimate> localization =
      std::make_unique<LocalizationEstimate>();
  //障碍物信息
  std::unique_ptr<PerceptionObstacle> perception =
      std::make_unique<PerceptionObstacle>();
  //构造全局路径

  routing_path->CreatePath();
  //创建一个静态障碍物
  perception->AddStaticObstacle(0, 30, -0.5, 0,
                                0); //此代码执行错误，待查找原因

  perception->AddDynamicObstacle(1, 40, -10, M_PI / 2,
                                 2); //此代码执行错误，待查找原因

  auto static_obstacle_info = perception->static_obstacles();
  auto dynamic_obstacle_info = perception->dynamic_obstacles();

  auto routing_path_points = routing_path->routing_path_points();

  LocalizationInfo localization_info;

  ReferenceLine reference_line; //当前参考线
  std::vector<ReferencePoint> rp;
  rp.clear();
  reference_line.set_reference_points(rp);

  ReferenceLine pre_reference_line; //上一时刻参考线

  Trajectory trajectory;
  Trajectory pre_trajectory;
  uint64_t time = 0;

  localization->UpdateLocalizationInfo(0, trajectory);
  localization_info = localization->localization_info();

  // 2.参考线生成,参考新默认-30m~150m
  std::unique_ptr<ReferenceLineProvider> reference_line_provider =
      std::make_unique<ReferenceLineProvider>();
  pre_reference_line = reference_line;
  //传参应该是数据类型，而不是类的对象
  reference_line_provider->Provide(routing_path_points, localization_info,
                                   pre_reference_line, reference_line);
  auto raw_reference_line = reference_line_provider->raw_reference_line();

  EMPlannerConfig emplanner_config;
  std::unique_ptr<EMPlanner> em_planner =
      std::make_unique<EMPlanner>(emplanner_config);

  TrajectoryPoint plan_start_point;
  Trajectory stitch_trajectory;

  std::vector<ObstacleInfo> virtual_obs;

  em_planner->CalPlaningStartPoint(pre_trajectory, localization_info,
                                   &plan_start_point, &stitch_trajectory);

  em_planner->Plan(0, plan_start_point, raw_reference_line, localization_info,
                   static_obstacle_info, dynamic_obstacle_info, &trajectory,
                   virtual_obs);

  std::unique_ptr<Plot> plot = std::make_unique<Plot>();

  plt::figure(1); // xy
  plot->PlotRoutingPath(routing_path_points, "k");
  plot->PlotReferenceLine(reference_line, "y");

  plt::figure(2); // sl
  plot->PlotSLPath(em_planner->sl_graph_->dp_path_points(), "r");
  plot->PlotSLPath(em_planner->sl_graph_->dp_path_points_dense(), "g");

  for (const auto obs : static_obstacle_info) {
    plot->PlotObs(obs, "k");
  }

  plot->PlotSLPath(em_planner->sl_graph_->qp_path_points(), "r");

  plot->PlotSLPath(em_planner->sl_graph_->qp_path_points_dense(), "p");

  plot->PlotPlanningPath(
      em_planner->sl_graph_->planning_path().reference_points(), "b");

  plt::figure(3); // st
  plot->PlotSTObs(em_planner->st_graph_->st_obstacles(), "k");
  plot->PlotSTPath(em_planner->st_graph_->dp_speed_points(), "r");
  plt::figure(3);
  plot->PlotSTPath(em_planner->st_graph_->qp_speed_points(), "b");

  plt::figure(3);
  plot->PlotSTPath(em_planner->st_graph_->qp_speed_points_dense(), "g");

  plt::show();
  return 0;
}