|
33 | 33 |
|
34 | 34 | namespace autoware::frenet_planner
|
35 | 35 | {
|
36 |
| -std::vector<Trajectory> generateTrajectories( |
37 |
| - const autoware::sampler_common::transform::Spline2D & reference_spline, |
38 |
| - const FrenetState & initial_state, const SamplingParameters & sampling_parameters) |
39 |
| -{ |
40 |
| - std::vector<Trajectory> trajectories; |
41 |
| - trajectories.reserve(sampling_parameters.parameters.size()); |
42 |
| - for (const auto & parameter : sampling_parameters.parameters) { |
43 |
| - auto trajectory = generateCandidate( |
44 |
| - initial_state, parameter.target_state, parameter.target_duration, |
45 |
| - sampling_parameters.resolution); |
46 |
| - trajectory.sampling_parameter = parameter; |
47 |
| - calculateCartesian(reference_spline, trajectory); |
48 |
| - std::stringstream ss; |
49 |
| - ss << parameter; |
50 |
| - trajectory.tag = ss.str(); |
51 |
| - trajectories.push_back(trajectory); |
52 |
| - } |
53 |
| - return trajectories; |
54 |
| -} |
55 |
| - |
56 |
| -std::vector<Trajectory> generateLowVelocityTrajectories( |
57 |
| - const autoware::sampler_common::transform::Spline2D & reference_spline, |
58 |
| - const FrenetState & initial_state, const SamplingParameters & sampling_parameters) |
59 |
| -{ |
60 |
| - std::vector<Trajectory> trajectories; |
61 |
| - trajectories.reserve(sampling_parameters.parameters.size()); |
62 |
| - for (const auto & parameter : sampling_parameters.parameters) { |
63 |
| - auto trajectory = generateLowVelocityCandidate( |
64 |
| - initial_state, parameter.target_state, parameter.target_duration, |
65 |
| - sampling_parameters.resolution); |
66 |
| - calculateCartesian(reference_spline, trajectory); |
67 |
| - std::stringstream ss; |
68 |
| - ss << parameter; |
69 |
| - trajectory.tag = ss.str(); |
70 |
| - trajectories.push_back(trajectory); |
71 |
| - } |
72 |
| - return trajectories; |
73 |
| -} |
74 |
| - |
75 | 36 | std::vector<Path> generatePaths(
|
76 | 37 | const autoware::sampler_common::transform::Spline2D & reference_spline,
|
77 | 38 | const FrenetState & initial_state, const SamplingParameters & sampling_parameters)
|
@@ -108,30 +69,6 @@ Trajectory generateCandidate(
|
108 | 69 | return trajectory;
|
109 | 70 | }
|
110 | 71 |
|
111 |
| -Trajectory generateLowVelocityCandidate( |
112 |
| - const FrenetState & initial_state, const FrenetState & target_state, const double duration, |
113 |
| - const double time_resolution) |
114 |
| -{ |
115 |
| - Trajectory trajectory; |
116 |
| - trajectory.longitudinal_polynomial = Polynomial( |
117 |
| - initial_state.position.s, initial_state.longitudinal_velocity, |
118 |
| - initial_state.longitudinal_acceleration, target_state.position.s, |
119 |
| - target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration); |
120 |
| - const auto delta_s = target_state.position.s - initial_state.position.s; |
121 |
| - trajectory.lateral_polynomial = Polynomial( |
122 |
| - initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration, |
123 |
| - target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration, |
124 |
| - delta_s); |
125 |
| - for (double t = 0.0; t <= duration; t += time_resolution) { |
126 |
| - trajectory.times.push_back(t); |
127 |
| - const auto s = trajectory.longitudinal_polynomial->position(t); |
128 |
| - const auto ds = s - initial_state.position.s; |
129 |
| - const auto d = trajectory.lateral_polynomial->position(ds); |
130 |
| - trajectory.frenet_points.emplace_back(s, d); |
131 |
| - } |
132 |
| - return trajectory; |
133 |
| -} |
134 |
| - |
135 | 72 | Path generateCandidate(
|
136 | 73 | const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution)
|
137 | 74 | {
|
|
0 commit comments