@@ -317,38 +317,42 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
317
317
constexpr double color_b = 205.0 / 256.0 ;
318
318
constexpr double color_a = 0.999 ;
319
319
const auto current_time = get_clock ()->now ();
320
- generateEgoPath (current_v, current_w, ego_path, ego_polys);
321
-
322
- std::vector<ObjectData> objects;
323
- createObjectData (ego_path, ego_polys, current_time, objects);
324
- has_collision_ego = hasCollision (current_v, ego_path, objects);
325
-
326
- std::string ns = " ego" ;
327
- addMarker (
328
- current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns,
329
- debug_markers);
320
+ const auto ego_path_opt = generateEgoPath (current_v, current_w, ego_polys);
321
+ if (ego_path_opt) {
322
+ const auto & ego_path = ego_path_opt.value ();
323
+ std::vector<ObjectData> objects;
324
+ createObjectData (ego_path, ego_polys, current_time, objects);
325
+ has_collision_ego = hasCollision (current_v, ego_path, objects);
326
+
327
+ std::string ns = " ego" ;
328
+ addMarker (
329
+ current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns,
330
+ debug_markers);
331
+ }
330
332
}
331
333
332
334
// step4. transform predicted trajectory from control module
333
335
bool has_collision_predicted = false ;
334
336
if (use_predicted_trajectory_) {
335
- Path predicted_path;
336
337
std::vector<Polygon2d> predicted_polys;
337
338
const auto predicted_traj_ptr = predicted_traj_ptr_;
338
339
constexpr double color_r = 0.0 ;
339
340
constexpr double color_g = 100.0 / 256.0 ;
340
341
constexpr double color_b = 0.0 ;
341
342
constexpr double color_a = 0.999 ;
342
343
const auto current_time = predicted_traj_ptr->header .stamp ;
343
- generateEgoPath (*predicted_traj_ptr, predicted_path, predicted_polys);
344
- std::vector<ObjectData> objects;
345
- createObjectData (predicted_path, predicted_polys, current_time, objects);
346
- has_collision_predicted = hasCollision (current_v, predicted_path, objects);
347
-
348
- std::string ns = " predicted" ;
349
- addMarker (
350
- current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
351
- ns, debug_markers);
344
+ const auto predicted_path_opt = generateEgoPath (*predicted_traj_ptr, predicted_polys);
345
+ if (predicted_path_opt) {
346
+ const auto & predicted_path = predicted_path_opt.value ();
347
+ std::vector<ObjectData> objects;
348
+ createObjectData (predicted_path, predicted_polys, current_time, objects);
349
+ has_collision_predicted = hasCollision (current_v, predicted_path, objects);
350
+
351
+ std::string ns = " predicted" ;
352
+ addMarker (
353
+ current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
354
+ ns, debug_markers);
355
+ }
352
356
}
353
357
354
358
return has_collision_ego || has_collision_predicted;
@@ -384,9 +388,10 @@ bool AEB::hasCollision(
384
388
return false ;
385
389
}
386
390
387
- void AEB::generateEgoPath (
388
- const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
391
+ std::optional<Path> AEB::generateEgoPath (
392
+ const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
389
393
{
394
+ Path path;
390
395
double curr_x = 0.0 ;
391
396
double curr_y = 0.0 ;
392
397
double curr_yaw = 0.0 ;
@@ -397,7 +402,7 @@ void AEB::generateEgoPath(
397
402
398
403
if (curr_v < 0.1 ) {
399
404
// if current velocity is too small, assume it stops at the same point
400
- return ;
405
+ return std::nullopt ;
401
406
}
402
407
403
408
constexpr double epsilon = 1e-6 ;
@@ -435,24 +440,25 @@ void AEB::generateEgoPath(
435
440
for (size_t i = 0 ; i < path.size () - 1 ; ++i) {
436
441
polygons.at (i) = createPolygon (path.at (i), path.at (i + 1 ), vehicle_info_, expand_width_);
437
442
}
443
+ return path;
438
444
}
439
445
440
- void AEB::generateEgoPath (
441
- const Trajectory & predicted_traj, Path & path,
442
- std::vector<tier4_autoware_utils::Polygon2d> & polygons)
446
+ std::optional<Path> AEB::generateEgoPath (
447
+ const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
443
448
{
444
449
if (predicted_traj.points .empty ()) {
445
- return ;
450
+ return std::nullopt ;
446
451
}
447
452
453
+ Path path;
448
454
geometry_msgs::msg::TransformStamped transform_stamped{};
449
455
try {
450
456
transform_stamped = tf_buffer_.lookupTransform (
451
457
" base_link" , predicted_traj.header .frame_id , predicted_traj.header .stamp ,
452
458
rclcpp::Duration::from_seconds (0.5 ));
453
459
} catch (tf2::TransformException & ex) {
454
460
RCLCPP_ERROR_STREAM (get_logger (), " [AEB] Failed to look up transform from base_link to map" );
455
- return ;
461
+ return std::nullopt ;
456
462
}
457
463
458
464
// create path
@@ -471,6 +477,7 @@ void AEB::generateEgoPath(
471
477
for (size_t i = 0 ; i < path.size () - 1 ; ++i) {
472
478
polygons.at (i) = createPolygon (path.at (i), path.at (i + 1 ), vehicle_info_, expand_width_);
473
479
}
480
+ return path;
474
481
}
475
482
476
483
void AEB::createObjectData (
0 commit comments