49 {
50 CHECK_NOTNULL(points);
51 points->clear();
52 points->insert(points->begin(), std::vector<common::SLPoint>{init_sl_point_});
53
54 const double kMinSampleDistance = 40.0;
55 const double total_length = std::fmin(
56 init_sl_point_.
s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
58 const auto &vehicle_config =
59 common::VehicleConfigHelper::Instance()->GetConfig();
60 const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
61 const double num_sample_per_level =
62 FLAGS_use_navigation_mode ?
config_.navigator_sample_num_each_level()
63 :
config_.sample_points_num_each_level();
64
65 static constexpr double kSamplePointLookForwardTime = 4.0;
66 const double level_distance =
69
71 double prev_s = accumulated_s;
72
73 static constexpr size_t kNumLevel = 3;
74 for (size_t i = 0; i < kNumLevel && accumulated_s < total_length; ++i) {
75 accumulated_s += level_distance;
76 if (accumulated_s + level_distance / 2.0 > total_length) {
77 accumulated_s = total_length;
78 }
79 const double s = std::fmin(accumulated_s, total_length);
80 static constexpr double kMinAllowedSampleStep = 1.0;
81 if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
82 continue;
83 }
84 prev_s = s;
85
86 double left_width = 0.0;
87 double right_width = 0.0;
89 &right_width);
90
91 static constexpr double kBoundaryBuff = 0.20;
92 const double eff_right_width = right_width - half_adc_width - kBoundaryBuff;
93 const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;
94
95
96 const double delta_dl = 1.2 / 20.0;
99 1.2, 3.5);
100
101 double kDefaultUnitL = kChangeLaneDeltaL / (num_sample_per_level - 1);
104 kDefaultUnitL = 1.0;
105 }
106 const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
107 double sample_right_boundary = -eff_right_width;
108 double sample_left_boundary = eff_left_width;
109
110 static constexpr double kLargeDeviationL = 1.75;
111 static constexpr double kTwentyMilesPerHour = 8.94;
114 if (injector_->ego_info()->start_point().v() > kTwentyMilesPerHour) {
115 sample_right_boundary = std::fmin(-eff_right_width,
init_sl_point_.
l());
117
119 sample_right_boundary = std::fmax(
121 }
123 sample_left_boundary = std::fmin(sample_left_boundary,
125 }
126 }
127 }
128
129 std::vector<double> sample_l;
133 } else {
135 sample_right_boundary, sample_left_boundary,
136 static_cast<uint32_t>(num_sample_per_level - 1), &sample_l);
137 }
138 std::vector<common::SLPoint> level_points;
139 planning_internal::SampleLayerDebug sample_layer_debug;
140 for (size_t j = 0; j < sample_l.size(); ++j) {
141 common::SLPoint sl =
143 sample_layer_debug.add_sl_point()->CopyFrom(sl);
144 level_points.push_back(std::move(sl));
145 }
146 if (!level_points.empty()) {
148 ->mutable_dp_poly_graph()
149 ->add_sample_layer()
150 ->CopyFrom(sample_layer_debug);
151 points->emplace_back(level_points);
152 }
153 }
154 return true;
155}
static SLPoint ToSLPoint(const double s, const double l)
const ReferenceLine & reference_line() const
double OffsetToOtherReferenceLine() const
bool IsChangeLanePath() const
Check if the current reference line is a change lane reference line, i.e., ADC's current position is ...
bool GetLaneWidth(const double s, double *const lane_left_width, double *const lane_right_width) const
apollo::planning_internal::Debug * planning_debug_
T Clamp(const T value, T bound1, T bound2)
Clamp a value between two bounds.
void uniform_slice(const T start, const T end, uint32_t num, std::vector< T > *sliced)
uniformly slice a segment [start, end] to num + 1 pieces the result sliced will contain the n + 1 poi...
bool IsClearToChangeLane(ReferenceLineInfo *reference_line_info)
A static function to check if the ChangeLanePath type of reference line is safe or if current referen...