@@ -309,16 +309,14 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
309
309
// step3. create ego path based on sensor data
310
310
bool has_collision_ego = false ;
311
311
if (use_imu_path_) {
312
- Path ego_path;
313
312
std::vector<Polygon2d> ego_polys;
314
313
const double current_w = angular_velocity_ptr_->z ;
315
314
constexpr double color_r = 0.0 / 256.0 ;
316
315
constexpr double color_g = 148.0 / 256.0 ;
317
316
constexpr double color_b = 205.0 / 256.0 ;
318
317
constexpr double color_a = 0.999 ;
319
318
const auto current_time = get_clock ()->now ();
320
- generateEgoPath (current_v, current_w, ego_path, ego_polys);
321
-
319
+ const auto ego_path = generateEgoPath (current_v, current_w, ego_polys);
322
320
std::vector<ObjectData> objects;
323
321
createObjectData (ego_path, ego_polys, current_time, objects);
324
322
has_collision_ego = hasCollision (current_v, ego_path, objects);
@@ -332,23 +330,25 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
332
330
// step4. transform predicted trajectory from control module
333
331
bool has_collision_predicted = false ;
334
332
if (use_predicted_trajectory_) {
335
- Path predicted_path;
336
333
std::vector<Polygon2d> predicted_polys;
337
334
const auto predicted_traj_ptr = predicted_traj_ptr_;
338
335
constexpr double color_r = 0.0 ;
339
336
constexpr double color_g = 100.0 / 256.0 ;
340
337
constexpr double color_b = 0.0 ;
341
338
constexpr double color_a = 0.999 ;
342
339
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);
340
+ const auto predicted_path_opt = generateEgoPath (*predicted_traj_ptr, predicted_polys);
341
+ if (predicted_path_opt) {
342
+ const auto & predicted_path = predicted_path_opt.value ();
343
+ std::vector<ObjectData> objects;
344
+ createObjectData (predicted_path, predicted_polys, current_time, objects);
345
+ has_collision_predicted = hasCollision (current_v, predicted_path, objects);
346
+
347
+ std::string ns = " predicted" ;
348
+ addMarker (
349
+ current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
350
+ ns, debug_markers);
351
+ }
352
352
}
353
353
354
354
return has_collision_ego || has_collision_predicted;
@@ -384,9 +384,10 @@ bool AEB::hasCollision(
384
384
return false ;
385
385
}
386
386
387
- void AEB::generateEgoPath (
388
- const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
387
+ Path AEB::generateEgoPath (
388
+ const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
389
389
{
390
+ Path path;
390
391
double curr_x = 0.0 ;
391
392
double curr_y = 0.0 ;
392
393
double curr_yaw = 0.0 ;
@@ -397,7 +398,7 @@ void AEB::generateEgoPath(
397
398
398
399
if (curr_v < 0.1 ) {
399
400
// if current velocity is too small, assume it stops at the same point
400
- return ;
401
+ return path ;
401
402
}
402
403
403
404
constexpr double epsilon = 1e-6 ;
@@ -435,14 +436,14 @@ void AEB::generateEgoPath(
435
436
for (size_t i = 0 ; i < path.size () - 1 ; ++i) {
436
437
polygons.at (i) = createPolygon (path.at (i), path.at (i + 1 ), vehicle_info_, expand_width_);
437
438
}
439
+ return path;
438
440
}
439
441
440
- void AEB::generateEgoPath (
441
- const Trajectory & predicted_traj, Path & path,
442
- std::vector<tier4_autoware_utils::Polygon2d> & polygons)
442
+ std::optional<Path> AEB::generateEgoPath (
443
+ const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
443
444
{
444
445
if (predicted_traj.points .empty ()) {
445
- return ;
446
+ return std::nullopt ;
446
447
}
447
448
448
449
geometry_msgs::msg::TransformStamped transform_stamped{};
@@ -452,10 +453,11 @@ void AEB::generateEgoPath(
452
453
rclcpp::Duration::from_seconds (0.5 ));
453
454
} catch (tf2::TransformException & ex) {
454
455
RCLCPP_ERROR_STREAM (get_logger (), " [AEB] Failed to look up transform from base_link to map" );
455
- return ;
456
+ return std::nullopt ;
456
457
}
457
458
458
459
// create path
460
+ Path path;
459
461
path.resize (predicted_traj.points .size ());
460
462
for (size_t i = 0 ; i < predicted_traj.points .size (); ++i) {
461
463
geometry_msgs::msg::Pose map_pose;
@@ -471,6 +473,7 @@ void AEB::generateEgoPath(
471
473
for (size_t i = 0 ; i < path.size () - 1 ; ++i) {
472
474
polygons.at (i) = createPolygon (path.at (i), path.at (i + 1 ), vehicle_info_, expand_width_);
473
475
}
476
+ return path;
474
477
}
475
478
476
479
void AEB::createObjectData (
0 commit comments