@@ -248,16 +248,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
248
248
base_vector.insert (base_vector.end (), additional_vector.begin (), additional_vector.end ());
249
249
}
250
250
251
+ bool isAvoidShift (const double start_shift_length, const double end_shift_length)
252
+ {
253
+ constexpr double THRESHOLD = 0.1 ;
254
+ return std::abs (start_shift_length) < THRESHOLD && std::abs (end_shift_length) > THRESHOLD;
255
+ }
256
+
257
+ bool isReturnShift (const double start_shift_length, const double end_shift_length)
258
+ {
259
+ constexpr double THRESHOLD = 0.1 ;
260
+ return std::abs (start_shift_length) > THRESHOLD && std::abs (end_shift_length) < THRESHOLD;
261
+ }
262
+
263
+ bool isLeftMiddleShift (const double start_shift_length, const double end_shift_length)
264
+ {
265
+ constexpr double THRESHOLD = 0.1 ;
266
+ return start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
267
+ }
268
+
269
+ bool isRightMiddleShift (const double start_shift_length, const double end_shift_length)
270
+ {
271
+ constexpr double THRESHOLD = 0.1 ;
272
+ return start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
273
+ }
274
+
251
275
bool existShiftSideLane (
252
276
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
253
277
const bool no_right_lanes)
254
278
{
255
- constexpr double THRESHOLD = 0.1 ;
256
279
const auto relative_shift_length = end_shift_length - start_shift_length;
257
280
258
- const auto avoid_shift =
259
- std::abs (start_shift_length) < THRESHOLD && std::abs (end_shift_length) > THRESHOLD;
260
- if (avoid_shift) {
281
+ if (isAvoidShift (start_shift_length, end_shift_length)) {
261
282
// Left avoid. But there is no adjacent lane. No need blinker.
262
283
if (relative_shift_length > 0.0 && no_left_lanes) {
263
284
return false ;
@@ -269,9 +290,7 @@ bool existShiftSideLane(
269
290
}
270
291
}
271
292
272
- const auto return_shift =
273
- std::abs (start_shift_length) > THRESHOLD && std::abs (end_shift_length) < THRESHOLD;
274
- if (return_shift) {
293
+ if (isReturnShift (start_shift_length, end_shift_length)) {
275
294
// Right return. But there is no adjacent lane. No need blinker.
276
295
if (relative_shift_length > 0.0 && no_right_lanes) {
277
296
return false ;
@@ -283,8 +302,7 @@ bool existShiftSideLane(
283
302
}
284
303
}
285
304
286
- const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
287
- if (left_middle_shift) {
305
+ if (isLeftMiddleShift (start_shift_length, end_shift_length)) {
288
306
// Left avoid. But there is no adjacent lane. No need blinker.
289
307
if (relative_shift_length > 0.0 && no_left_lanes) {
290
308
return false ;
@@ -296,8 +314,7 @@ bool existShiftSideLane(
296
314
}
297
315
}
298
316
299
- const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
300
- if (right_middle_shift) {
317
+ if (isRightMiddleShift (start_shift_length, end_shift_length)) {
301
318
// Right avoid. But there is no adjacent lane. No need blinker.
302
319
if (relative_shift_length < 0.0 && no_right_lanes) {
303
320
return false ;
@@ -312,6 +329,27 @@ bool existShiftSideLane(
312
329
return true ;
313
330
}
314
331
332
+ bool isNearEndOfShift (
333
+ const double start_shift_length, const double end_shift_length, const Point & ego_pos,
334
+ const lanelet::ConstLanelets & original_lanes)
335
+ {
336
+ using boost::geometry::within;
337
+ using lanelet::utils::to2D;
338
+ using lanelet::utils::conversion::toLaneletPoint;
339
+
340
+ if (!isReturnShift (start_shift_length, end_shift_length)) {
341
+ return false ;
342
+ }
343
+
344
+ for (const auto & lane : original_lanes) {
345
+ if (within (to2D (toLaneletPoint (ego_pos)), lane.polygon2d ().basicPolygon ())) {
346
+ return true ;
347
+ }
348
+ }
349
+
350
+ return false ;
351
+ }
352
+
315
353
bool straddleRoadBound (
316
354
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
317
355
const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -1260,11 +1298,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
1260
1298
throw std::logic_error (" empty path." );
1261
1299
}
1262
1300
1263
- if (path.points .front ().lane_ids .empty ()) {
1301
+ const auto idx = planner_data->findEgoIndex (path.points );
1302
+
1303
+ if (path.points .at (idx).lane_ids .empty ()) {
1264
1304
throw std::logic_error (" empty lane ids." );
1265
1305
}
1266
1306
1267
- const auto start_id = path.points .front ( ).lane_ids .front ();
1307
+ const auto start_id = path.points .at (idx ).lane_ids .front ();
1268
1308
const auto start_lane = planner_data->route_handler ->getLaneletsFromId (start_id);
1269
1309
const auto & p = planner_data->parameters ;
1270
1310
@@ -2224,7 +2264,7 @@ double calcDistanceToReturnDeadLine(
2224
2264
return distance_to_return_dead_line;
2225
2265
}
2226
2266
2227
- TurnSignalInfo calcTurnSignalInfo (
2267
+ std::pair< TurnSignalInfo, bool > calcTurnSignalInfo (
2228
2268
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
2229
2269
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
2230
2270
{
@@ -2236,22 +2276,22 @@ TurnSignalInfo calcTurnSignalInfo(
2236
2276
2237
2277
if (shift_line.start_idx + 1 > path.shift_length .size ()) {
2238
2278
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2239
- return {} ;
2279
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2240
2280
}
2241
2281
2242
2282
if (shift_line.start_idx + 1 > path.path .points .size ()) {
2243
2283
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2244
- return {} ;
2284
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2245
2285
}
2246
2286
2247
2287
if (shift_line.end_idx + 1 > path.shift_length .size ()) {
2248
2288
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2249
- return {} ;
2289
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2250
2290
}
2251
2291
2252
2292
if (shift_line.end_idx + 1 > path.path .points .size ()) {
2253
2293
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2254
- return {} ;
2294
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2255
2295
}
2256
2296
2257
2297
const auto start_shift_length = path.shift_length .at (shift_line.start_idx );
@@ -2260,12 +2300,12 @@ TurnSignalInfo calcTurnSignalInfo(
2260
2300
2261
2301
// If shift length is shorter than the threshold, it does not need to turn on blinkers
2262
2302
if (std::fabs (relative_shift_length) < p.turn_signal_shift_length_threshold ) {
2263
- return {} ;
2303
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2264
2304
}
2265
2305
2266
2306
// If the vehicle does not shift anymore, we turn off the blinker
2267
2307
if (std::fabs (path.shift_length .at (shift_line.end_idx ) - current_shift_length) < THRESHOLD) {
2268
- return {} ;
2308
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2269
2309
}
2270
2310
2271
2311
const auto get_command = [](const auto & shift_length) {
@@ -2280,7 +2320,7 @@ TurnSignalInfo calcTurnSignalInfo(
2280
2320
p.vehicle_info .max_longitudinal_offset_m ;
2281
2321
2282
2322
if (signal_prepare_distance < ego_front_to_shift_start) {
2283
- return {} ;
2323
+ return std::make_pair (TurnSignalInfo{}, false ) ;
2284
2324
}
2285
2325
2286
2326
const auto blinker_start_pose = path.path .points .at (shift_line.start_idx ).point .pose ;
@@ -2297,12 +2337,12 @@ TurnSignalInfo calcTurnSignalInfo(
2297
2337
turn_signal_info.turn_signal .command = get_command (relative_shift_length);
2298
2338
2299
2339
if (!p.turn_signal_on_swerving ) {
2300
- return turn_signal_info;
2340
+ return std::make_pair ( turn_signal_info, false ) ;
2301
2341
}
2302
2342
2303
2343
lanelet::ConstLanelet lanelet;
2304
2344
if (!rh->getClosestLaneletWithinRoute (shift_line.end , &lanelet)) {
2305
- return {} ;
2345
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2306
2346
}
2307
2347
2308
2348
const auto left_same_direction_lane = rh->getLeftLanelet (lanelet, true , true );
@@ -2314,13 +2354,21 @@ TurnSignalInfo calcTurnSignalInfo(
2314
2354
right_same_direction_lane.has_value () || !right_opposite_lanes.empty ();
2315
2355
2316
2356
if (!existShiftSideLane (start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
2317
- return {} ;
2357
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2318
2358
}
2319
2359
2320
2360
if (!straddleRoadBound (path, shift_line, data.current_lanelets , p.vehicle_info )) {
2321
- return {};
2361
+ return std::make_pair (TurnSignalInfo{}, true );
2362
+ }
2363
+
2364
+ constexpr double STOPPED_THRESHOLD = 0.1 ; // [m/s]
2365
+ if (ego_speed < STOPPED_THRESHOLD) {
2366
+ if (isNearEndOfShift (
2367
+ start_shift_length, end_shift_length, ego_pose.position , data.current_lanelets )) {
2368
+ return std::make_pair (TurnSignalInfo{}, true );
2369
+ }
2322
2370
}
2323
2371
2324
- return turn_signal_info;
2372
+ return std::make_pair ( turn_signal_info, false ) ;
2325
2373
}
2326
2374
} // namespace behavior_path_planner::utils::avoidance
0 commit comments