@@ -250,132 +250,6 @@ 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 (
254
- const double start_shift_length, const double end_shift_length, const double threshold)
255
- {
256
- return std::abs (start_shift_length) < threshold && std::abs (end_shift_length) > threshold;
257
- }
258
-
259
- bool isReturnShift (
260
- const double start_shift_length, const double end_shift_length, const double threshold)
261
- {
262
- return std::abs (start_shift_length) > threshold && std::abs (end_shift_length) < threshold;
263
- }
264
-
265
- bool isLeftMiddleShift (
266
- const double start_shift_length, const double end_shift_length, const double threshold)
267
- {
268
- return start_shift_length > threshold && end_shift_length > threshold;
269
- }
270
-
271
- bool isRightMiddleShift (
272
- const double start_shift_length, const double end_shift_length, const double threshold)
273
- {
274
- return start_shift_length < threshold && end_shift_length < threshold;
275
- }
276
-
277
- bool existShiftSideLane (
278
- const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
279
- const bool no_right_lanes, const double threshold)
280
- {
281
- const auto relative_shift_length = end_shift_length - start_shift_length;
282
-
283
- if (isAvoidShift (start_shift_length, end_shift_length, threshold)) {
284
- // Left avoid. But there is no adjacent lane. No need blinker.
285
- if (relative_shift_length > 0.0 && no_left_lanes) {
286
- return false ;
287
- }
288
-
289
- // Right avoid. But there is no adjacent lane. No need blinker.
290
- if (relative_shift_length < 0.0 && no_right_lanes) {
291
- return false ;
292
- }
293
- }
294
-
295
- if (isReturnShift (start_shift_length, end_shift_length, threshold)) {
296
- // Right return. But there is no adjacent lane. No need blinker.
297
- if (relative_shift_length > 0.0 && no_right_lanes) {
298
- return false ;
299
- }
300
-
301
- // Left return. But there is no adjacent lane. No need blinker.
302
- if (relative_shift_length < 0.0 && no_left_lanes) {
303
- return false ;
304
- }
305
- }
306
-
307
- if (isLeftMiddleShift (start_shift_length, end_shift_length, threshold)) {
308
- // Left avoid. But there is no adjacent lane. No need blinker.
309
- if (relative_shift_length > 0.0 && no_left_lanes) {
310
- return false ;
311
- }
312
-
313
- // Left return. But there is no adjacent lane. No need blinker.
314
- if (relative_shift_length < 0.0 && no_left_lanes) {
315
- return false ;
316
- }
317
- }
318
-
319
- if (isRightMiddleShift (start_shift_length, end_shift_length, threshold)) {
320
- // Right avoid. But there is no adjacent lane. No need blinker.
321
- if (relative_shift_length < 0.0 && no_right_lanes) {
322
- return false ;
323
- }
324
-
325
- // Left avoid. But there is no adjacent lane. No need blinker.
326
- if (relative_shift_length > 0.0 && no_right_lanes) {
327
- return false ;
328
- }
329
- }
330
-
331
- return true ;
332
- }
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, const double threshold)
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, threshold)) {
343
- return false ;
344
- }
345
-
346
- return std::any_of (original_lanes.begin (), original_lanes.end (), [&ego_pos](const auto & lane) {
347
- return within (to2D (toLaneletPoint (ego_pos)), lane.polygon2d ().basicPolygon ());
348
- });
349
- }
350
-
351
- bool straddleRoadBound (
352
- const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
353
- const vehicle_info_util::VehicleInfo & vehicle_info)
354
- {
355
- using boost::geometry::intersects;
356
- using tier4_autoware_utils::pose2transform;
357
- using tier4_autoware_utils::transformVector;
358
-
359
- const auto footprint = vehicle_info.createFootprint ();
360
-
361
- for (const auto & lane : lanes) {
362
- for (size_t i = shift_line.start_idx ; i < shift_line.end_idx ; ++i) {
363
- const auto transform = pose2transform (path.path .points .at (i).point .pose );
364
- const auto shifted_vehicle_footprint = transformVector (footprint, transform);
365
-
366
- if (intersects (lane.leftBound2d ().basicLineString (), shifted_vehicle_footprint)) {
367
- return true ;
368
- }
369
-
370
- if (intersects (lane.rightBound2d ().basicLineString (), shifted_vehicle_footprint)) {
371
- return true ;
372
- }
373
- }
374
- }
375
-
376
- return false ;
377
- }
378
-
379
253
} // namespace
380
254
381
255
namespace filtering_utils
@@ -2367,115 +2241,4 @@ double calcDistanceToReturnDeadLine(
2367
2241
2368
2242
return distance_to_return_dead_line;
2369
2243
}
2370
-
2371
- std::pair<TurnSignalInfo, bool > calcTurnSignalInfo (
2372
- const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
2373
- const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
2374
- {
2375
- constexpr double THRESHOLD = 0.1 ;
2376
- const auto & p = planner_data->parameters ;
2377
- const auto & rh = planner_data->route_handler ;
2378
- const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
2379
- const auto & ego_speed = planner_data->self_odometry ->twist .twist .linear .x ;
2380
-
2381
- if (shift_line.start_idx + 1 > path.shift_length .size ()) {
2382
- RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2383
- return std::make_pair (TurnSignalInfo{}, true );
2384
- }
2385
-
2386
- if (shift_line.start_idx + 1 > path.path .points .size ()) {
2387
- RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2388
- return std::make_pair (TurnSignalInfo{}, true );
2389
- }
2390
-
2391
- if (shift_line.end_idx + 1 > path.shift_length .size ()) {
2392
- RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2393
- return std::make_pair (TurnSignalInfo{}, true );
2394
- }
2395
-
2396
- if (shift_line.end_idx + 1 > path.path .points .size ()) {
2397
- RCLCPP_WARN (rclcpp::get_logger (__func__), " index inconsistency." );
2398
- return std::make_pair (TurnSignalInfo{}, true );
2399
- }
2400
-
2401
- const auto start_shift_length = path.shift_length .at (shift_line.start_idx );
2402
- const auto end_shift_length = path.shift_length .at (shift_line.end_idx );
2403
- const auto relative_shift_length = end_shift_length - start_shift_length;
2404
-
2405
- // If shift length is shorter than the threshold, it does not need to turn on blinkers
2406
- if (std::fabs (relative_shift_length) < p.turn_signal_shift_length_threshold ) {
2407
- return std::make_pair (TurnSignalInfo{}, true );
2408
- }
2409
-
2410
- // If the vehicle does not shift anymore, we turn off the blinker
2411
- if (std::fabs (path.shift_length .at (shift_line.end_idx ) - current_shift_length) < THRESHOLD) {
2412
- return std::make_pair (TurnSignalInfo{}, true );
2413
- }
2414
-
2415
- const auto get_command = [](const auto & shift_length) {
2416
- return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
2417
- : TurnIndicatorsCommand::ENABLE_RIGHT;
2418
- };
2419
-
2420
- const auto signal_prepare_distance =
2421
- std::max (ego_speed * p.turn_signal_search_time , p.turn_signal_minimum_search_distance );
2422
- const auto ego_front_to_shift_start =
2423
- calcSignedArcLength (path.path .points , ego_pose.position , shift_line.start_idx ) -
2424
- p.vehicle_info .max_longitudinal_offset_m ;
2425
-
2426
- if (signal_prepare_distance < ego_front_to_shift_start) {
2427
- return std::make_pair (TurnSignalInfo{}, false );
2428
- }
2429
-
2430
- const auto blinker_start_pose = path.path .points .at (shift_line.start_idx ).point .pose ;
2431
- const auto blinker_end_pose = path.path .points .at (shift_line.end_idx ).point .pose ;
2432
- const auto get_start_pose = [&](const auto & ego_to_shift_start) {
2433
- return ego_to_shift_start ? ego_pose : blinker_start_pose;
2434
- };
2435
-
2436
- TurnSignalInfo turn_signal_info{};
2437
- turn_signal_info.desired_start_point = get_start_pose (ego_front_to_shift_start);
2438
- turn_signal_info.desired_end_point = blinker_end_pose;
2439
- turn_signal_info.required_start_point = blinker_start_pose;
2440
- turn_signal_info.required_end_point = blinker_end_pose;
2441
- turn_signal_info.turn_signal .command = get_command (relative_shift_length);
2442
-
2443
- if (!p.turn_signal_on_swerving ) {
2444
- return std::make_pair (turn_signal_info, false );
2445
- }
2446
-
2447
- lanelet::ConstLanelet lanelet;
2448
- if (!rh->getClosestLaneletWithinRoute (shift_line.end , &lanelet)) {
2449
- return std::make_pair (TurnSignalInfo{}, true );
2450
- }
2451
-
2452
- const auto left_same_direction_lane = rh->getLeftLanelet (lanelet, true , true );
2453
- const auto left_opposite_lanes = rh->getLeftOppositeLanelets (lanelet);
2454
- const auto right_same_direction_lane = rh->getRightLanelet (lanelet, true , true );
2455
- const auto right_opposite_lanes = rh->getRightOppositeLanelets (lanelet);
2456
- const auto has_left_lane = left_same_direction_lane.has_value () || !left_opposite_lanes.empty ();
2457
- const auto has_right_lane =
2458
- right_same_direction_lane.has_value () || !right_opposite_lanes.empty ();
2459
-
2460
- if (!existShiftSideLane (
2461
- start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
2462
- p.turn_signal_shift_length_threshold )) {
2463
- return std::make_pair (TurnSignalInfo{}, true );
2464
- }
2465
-
2466
- if (!straddleRoadBound (path, shift_line, data.current_lanelets , p.vehicle_info )) {
2467
- return std::make_pair (TurnSignalInfo{}, true );
2468
- }
2469
-
2470
- constexpr double STOPPED_THRESHOLD = 0.1 ; // [m/s]
2471
- if (ego_speed < STOPPED_THRESHOLD) {
2472
- if (isNearEndOfShift (
2473
- start_shift_length, end_shift_length, ego_pose.position , data.current_lanelets ,
2474
- p.turn_signal_shift_length_threshold )) {
2475
- return std::make_pair (TurnSignalInfo{}, true );
2476
- }
2477
- }
2478
-
2479
- return std::make_pair (turn_signal_info, false );
2480
- }
2481
2244
} // namespace behavior_path_planner::utils::avoidance
0 commit comments