35 {
36 DiscretizedTrajectory combined_trajectory;
37
38 double s0 = lon_trajectory.Evaluate(0, 0.0);
39 double s_ref_max = reference_line.back().s();
40 double accumulated_trajectory_s = 0.0;
41 PathPoint prev_trajectory_point;
42
43 double last_s = -FLAGS_numerical_epsilon;
44 double t_param = 0.0;
45 while (t_param < FLAGS_trajectory_time_length) {
46
47
48 double s = lon_trajectory.Evaluate(0, t_param);
49 if (last_s > 0.0) {
50 s = std::max(last_s, s);
51 }
52 last_s = s;
53
54 double s_dot =
55 std::max(FLAGS_numerical_epsilon, lon_trajectory.Evaluate(1, t_param));
56 double s_ddot = lon_trajectory.Evaluate(2, t_param);
57 if (s > s_ref_max) {
58 break;
59 }
60
61 double relative_s = s - s0;
62
63
64 double d = lat_trajectory.Evaluate(0, relative_s);
65 double d_prime = lat_trajectory.Evaluate(1, relative_s);
66 double d_pprime = lat_trajectory.Evaluate(2, relative_s);
67
69
70 double x = 0.0;
71 double y = 0.0;
72 double theta = 0.0;
73 double kappa = 0.0;
74 double v = 0.0;
75 double a = 0.0;
76
77 const double rs = matched_ref_point.s();
78 const double rx = matched_ref_point.x();
79 const double ry = matched_ref_point.y();
80 const double rtheta = matched_ref_point.theta();
81 const double rkappa = matched_ref_point.kappa();
82 const double rdkappa = matched_ref_point.dkappa();
83
84 std::array<double, 3> s_conditions = {rs, s_dot, s_ddot};
85 std::array<double, 3> d_conditions = {d, d_prime, d_pprime};
87 rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x, &y,
88 &theta, &kappa, &v, &a);
89
90 if (prev_trajectory_point.has_x() && prev_trajectory_point.has_y()) {
91 double delta_x = x - prev_trajectory_point.x();
92 double delta_y = y - prev_trajectory_point.y();
93 double delta_s = std::hypot(delta_x, delta_y);
94 accumulated_trajectory_s += delta_s;
95 }
96
97 TrajectoryPoint trajectory_point;
98 trajectory_point.mutable_path_point()->set_x(x);
99 trajectory_point.mutable_path_point()->set_y(y);
100 trajectory_point.mutable_path_point()->set_s(accumulated_trajectory_s);
101 trajectory_point.mutable_path_point()->set_theta(theta);
102 trajectory_point.mutable_path_point()->set_kappa(kappa);
103 trajectory_point.set_v(v);
104 trajectory_point.set_a(a);
105 trajectory_point.set_relative_time(t_param + init_relative_time);
106
107 combined_trajectory.AppendTrajectoryPoint(trajectory_point);
108
109 t_param = t_param + FLAGS_trajectory_time_resolution;
110
111 prev_trajectory_point = trajectory_point.path_point();
112 }
113 return combined_trajectory;
114}
static void frenet_to_cartesian(const double rs, const double rx, const double ry, const double rtheta, const double rkappa, const double rdkappa, const std::array< double, 3 > &s_condition, const std::array< double, 3 > &d_condition, double *const ptr_x, double *const ptr_y, double *const ptr_theta, double *const ptr_kappa, double *const ptr_v, double *const ptr_a)
Convert a vehicle state in Frenet frame to Cartesian frame.
static PathPoint MatchToPath(const std::vector< PathPoint > &reference_line, const double x, const double y)