@@ -250,16 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
250
250
base_vector.insert (base_vector.end (), additional_vector.begin (), additional_vector.end ());
251
251
}
252
252
253
+ bool isAvoidShift (const double start_shift_length, const double end_shift_length)
254
+ {
255
+ constexpr double THRESHOLD = 0.1 ;
256
+ return std::abs (start_shift_length) < THRESHOLD && std::abs (end_shift_length) > THRESHOLD;
257
+ }
258
+
259
+ bool isReturnShift (const double start_shift_length, const double end_shift_length)
260
+ {
261
+ constexpr double THRESHOLD = 0.1 ;
262
+ return std::abs (start_shift_length) > THRESHOLD && std::abs (end_shift_length) < THRESHOLD;
263
+ }
264
+
265
+ bool isLeftMiddleShift (const double start_shift_length, const double end_shift_length)
266
+ {
267
+ constexpr double THRESHOLD = 0.1 ;
268
+ return start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
269
+ }
270
+
271
+ bool isRightMiddleShift (const double start_shift_length, const double end_shift_length)
272
+ {
273
+ constexpr double THRESHOLD = 0.1 ;
274
+ return start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
275
+ }
276
+
253
277
bool existShiftSideLane (
254
278
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
255
279
const bool no_right_lanes)
256
280
{
257
- constexpr double THRESHOLD = 0.1 ;
258
281
const auto relative_shift_length = end_shift_length - start_shift_length;
259
282
260
- const auto avoid_shift =
261
- std::abs (start_shift_length) < THRESHOLD && std::abs (end_shift_length) > THRESHOLD;
262
- if (avoid_shift) {
283
+ if (isAvoidShift (start_shift_length, end_shift_length)) {
263
284
// Left avoid. But there is no adjacent lane. No need blinker.
264
285
if (relative_shift_length > 0.0 && no_left_lanes) {
265
286
return false ;
@@ -271,9 +292,7 @@ bool existShiftSideLane(
271
292
}
272
293
}
273
294
274
- const auto return_shift =
275
- std::abs (start_shift_length) > THRESHOLD && std::abs (end_shift_length) < THRESHOLD;
276
- if (return_shift) {
295
+ if (isReturnShift (start_shift_length, end_shift_length)) {
277
296
// Right return. But there is no adjacent lane. No need blinker.
278
297
if (relative_shift_length > 0.0 && no_right_lanes) {
279
298
return false ;
@@ -285,8 +304,7 @@ bool existShiftSideLane(
285
304
}
286
305
}
287
306
288
- const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
289
- if (left_middle_shift) {
307
+ if (isLeftMiddleShift (start_shift_length, end_shift_length)) {
290
308
// Left avoid. But there is no adjacent lane. No need blinker.
291
309
if (relative_shift_length > 0.0 && no_left_lanes) {
292
310
return false ;
@@ -298,8 +316,7 @@ bool existShiftSideLane(
298
316
}
299
317
}
300
318
301
- const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
302
- if (right_middle_shift) {
319
+ if (isRightMiddleShift (start_shift_length, end_shift_length)) {
303
320
// Right avoid. But there is no adjacent lane. No need blinker.
304
321
if (relative_shift_length < 0.0 && no_right_lanes) {
305
322
return false ;
@@ -314,6 +331,27 @@ bool existShiftSideLane(
314
331
return true ;
315
332
}
316
333
334
+ bool isNearEndOfShift (
335
+ const double start_shift_length, const double end_shift_length, const Point & ego_pos,
336
+ const lanelet::ConstLanelets & original_lanes)
337
+ {
338
+ using boost::geometry::within;
339
+ using lanelet::utils::to2D;
340
+ using lanelet::utils::conversion::toLaneletPoint;
341
+
342
+ if (!isReturnShift (start_shift_length, end_shift_length)) {
343
+ return false ;
344
+ }
345
+
346
+ for (const auto & lane : original_lanes) {
347
+ if (within (to2D (toLaneletPoint (ego_pos)), lane.polygon2d ().basicPolygon ())) {
348
+ return true ;
349
+ }
350
+ }
351
+
352
+ return false ;
353
+ }
354
+
317
355
bool straddleRoadBound (
318
356
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
319
357
const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -2286,7 +2324,7 @@ double calcDistanceToReturnDeadLine(
2286
2324
return distance_to_return_dead_line;
2287
2325
}
2288
2326
2289
- TurnSignalInfo calcTurnSignalInfo (
2327
+ std::pair< TurnSignalInfo, bool > calcTurnSignalInfo (
2290
2328
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
2291
2329
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
2292
2330
{
@@ -2298,22 +2336,22 @@ TurnSignalInfo calcTurnSignalInfo(
2298
2336
2299
2337
if (shift_line.start_idx + 1 > path.shift_length .size ()) {
2300
2338
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2301
- return {} ;
2339
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2302
2340
}
2303
2341
2304
2342
if (shift_line.start_idx + 1 > path.path .points .size ()) {
2305
2343
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2306
- return {} ;
2344
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2307
2345
}
2308
2346
2309
2347
if (shift_line.end_idx + 1 > path.shift_length .size ()) {
2310
2348
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2311
- return {} ;
2349
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2312
2350
}
2313
2351
2314
2352
if (shift_line.end_idx + 1 > path.path .points .size ()) {
2315
2353
RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2316
- return {} ;
2354
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2317
2355
}
2318
2356
2319
2357
const auto start_shift_length = path.shift_length .at (shift_line.start_idx );
@@ -2322,12 +2360,12 @@ TurnSignalInfo calcTurnSignalInfo(
2322
2360
2323
2361
// If shift length is shorter than the threshold, it does not need to turn on blinkers
2324
2362
if (std::fabs (relative_shift_length) < p.turn_signal_shift_length_threshold ) {
2325
- return {} ;
2363
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2326
2364
}
2327
2365
2328
2366
// If the vehicle does not shift anymore, we turn off the blinker
2329
2367
if (std::fabs (path.shift_length .at (shift_line.end_idx ) - current_shift_length) < THRESHOLD) {
2330
- return {} ;
2368
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2331
2369
}
2332
2370
2333
2371
const auto get_command = [](const auto & shift_length) {
@@ -2342,7 +2380,7 @@ TurnSignalInfo calcTurnSignalInfo(
2342
2380
p.vehicle_info .max_longitudinal_offset_m ;
2343
2381
2344
2382
if (signal_prepare_distance < ego_front_to_shift_start) {
2345
- return {} ;
2383
+ return std::make_pair (TurnSignalInfo{}, false ) ;
2346
2384
}
2347
2385
2348
2386
const auto blinker_start_pose = path.path .points .at (shift_line.start_idx ).point .pose ;
@@ -2359,12 +2397,12 @@ TurnSignalInfo calcTurnSignalInfo(
2359
2397
turn_signal_info.turn_signal .command = get_command (relative_shift_length);
2360
2398
2361
2399
if (!p.turn_signal_on_swerving ) {
2362
- return turn_signal_info;
2400
+ return std::make_pair ( turn_signal_info, false ) ;
2363
2401
}
2364
2402
2365
2403
lanelet::ConstLanelet lanelet;
2366
2404
if (!rh->getClosestLaneletWithinRoute (shift_line.end , &lanelet)) {
2367
- return {} ;
2405
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2368
2406
}
2369
2407
2370
2408
const auto left_same_direction_lane = rh->getLeftLanelet (lanelet, true , true );
@@ -2376,13 +2414,21 @@ TurnSignalInfo calcTurnSignalInfo(
2376
2414
right_same_direction_lane.has_value () || !right_opposite_lanes.empty ();
2377
2415
2378
2416
if (!existShiftSideLane (start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
2379
- return {} ;
2417
+ return std::make_pair (TurnSignalInfo{}, true ) ;
2380
2418
}
2381
2419
2382
2420
if (!straddleRoadBound (path, shift_line, data.current_lanelets , p.vehicle_info )) {
2383
- return {};
2421
+ return std::make_pair (TurnSignalInfo{}, true );
2422
+ }
2423
+
2424
+ constexpr double STOPPED_THRESHOLD = 0.1 ; // [m/s]
2425
+ if (ego_speed < STOPPED_THRESHOLD) {
2426
+ if (isNearEndOfShift (
2427
+ start_shift_length, end_shift_length, ego_pose.position , data.current_lanelets )) {
2428
+ return std::make_pair (TurnSignalInfo{}, true );
2429
+ }
2384
2430
}
2385
2431
2386
- return turn_signal_info;
2432
+ return std::make_pair ( turn_signal_info, false ) ;
2387
2433
}
2388
2434
} // namespace behavior_path_planner::utils::avoidance
0 commit comments