@@ -139,6 +139,7 @@ void StartPlannerModule::initVariables()
139
139
resetPathCandidate ();
140
140
resetPathReference ();
141
141
debug_marker_.markers .clear ();
142
+ info_marker_.markers .clear ();
142
143
initializeSafetyCheckParameters ();
143
144
initializeCollisionCheckDebugMap (debug_data_.collision_check );
144
145
updateDepartureCheckLanes ();
@@ -1447,25 +1448,35 @@ void StartPlannerModule::setDebugData()
1447
1448
const auto white_color = createMarkerColor (1.0 , 1.0 , 1.0 , 0.99 );
1448
1449
1449
1450
const auto life_time = rclcpp::Duration::from_seconds (1.5 );
1450
- auto add = [&](MarkerArray added) {
1451
+ auto add = [&](MarkerArray added, MarkerArray & target_marker_array ) {
1451
1452
for (auto & marker : added.markers ) {
1452
1453
marker.lifetime = life_time;
1453
1454
}
1454
- tier4_autoware_utils::appendMarkerArray (added, &debug_marker_ );
1455
+ tier4_autoware_utils::appendMarkerArray (added, &target_marker_array );
1455
1456
};
1456
1457
1457
1458
debug_marker_.markers .clear ();
1458
- add (createPoseMarkerArray (status_.pull_out_start_pose , " back_end_pose" , 0 , 0.9 , 0.3 , 0.3 ));
1459
- add (createPoseMarkerArray (status_.pull_out_path .start_pose , " start_pose" , 0 , 0.3 , 0.9 , 0.3 ));
1460
- add (createPoseMarkerArray (status_.pull_out_path .end_pose , " end_pose" , 0 , 0.9 , 0.9 , 0.3 ));
1461
- add (createFootprintMarkerArray (
1462
- debug_data_.refined_start_pose , vehicle_info_, " refined_start_pose" , 0 , 0.9 , 0.9 , 0.3 ));
1463
- add (createPathMarkerArray (getFullPath (), " full_path" , 0 , 0.0 , 0.5 , 0.9 ));
1464
- add (createPathMarkerArray (status_.backward_path , " backward_driving_path" , 0 , 0.0 , 0.9 , 0.0 ));
1459
+ info_marker_.markers .clear ();
1460
+ add (
1461
+ createPoseMarkerArray (status_.pull_out_start_pose , " back_end_pose" , 0 , 0.9 , 0.3 , 0.3 ),
1462
+ info_marker_);
1463
+ add (
1464
+ createPoseMarkerArray (status_.pull_out_path .start_pose , " start_pose" , 0 , 0.3 , 0.9 , 0.3 ),
1465
+ info_marker_);
1466
+ add (
1467
+ createPoseMarkerArray (status_.pull_out_path .end_pose , " end_pose" , 0 , 0.9 , 0.9 , 0.3 ),
1468
+ info_marker_);
1469
+ add (
1470
+ createFootprintMarkerArray (
1471
+ debug_data_.refined_start_pose , vehicle_info_, " refined_start_pose" , 0 , 0.9 , 0.9 , 0.3 ),
1472
+ debug_marker_);
1473
+ add (createPathMarkerArray (getFullPath (), " full_path" , 0 , 0.0 , 0.5 , 0.9 ), debug_marker_);
1474
+ add (
1475
+ createPathMarkerArray (status_.backward_path , " backward_driving_path" , 0 , 0.0 , 0.9 , 0.0 ),
1476
+ debug_marker_);
1465
1477
1466
1478
// visualize collision_check_end_pose and footprint
1467
1479
{
1468
- const auto local_footprint = vehicle_info_.createFootprint ();
1469
1480
std::map<PlannerType, double > collision_check_distances = {
1470
1481
{PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end },
1471
1482
{PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end }};
@@ -1475,24 +1486,16 @@ void StartPlannerModule::setDebugData()
1475
1486
getFullPath ().points , status_.pull_out_path .end_pose .position ,
1476
1487
collision_check_distance_from_end);
1477
1488
if (collision_check_end_pose) {
1478
- add (createPoseMarkerArray (
1479
- *collision_check_end_pose, " static_collision_check_end_pose" , 0 , 1.0 , 0.0 , 0.0 ));
1489
+ add (
1490
+ createPoseMarkerArray (
1491
+ *collision_check_end_pose, " static_collision_check_end_pose" , 0 , 1.0 , 0.0 , 0.0 ),
1492
+ info_marker_);
1480
1493
auto marker = createDefaultMarker (
1481
1494
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " static_collision_check_end_polygon" , 0 ,
1482
- Marker::LINE_LIST, createMarkerScale (0.1 , 0.1 , 0.1 ), red_color);
1483
- const auto footprint = transformVector (
1484
- local_footprint, tier4_autoware_utils::pose2transform (*collision_check_end_pose));
1485
- const double ego_z = planner_data_->self_odometry ->pose .pose .position .z ;
1486
- for (size_t i = 0 ; i < footprint.size (); i++) {
1487
- const auto & current_point = footprint.at (i);
1488
- const auto & next_point = footprint.at ((i + 1 ) % footprint.size ());
1489
- marker.points .push_back (
1490
- tier4_autoware_utils::createPoint (current_point.x (), current_point.y (), ego_z));
1491
- marker.points .push_back (
1492
- tier4_autoware_utils::createPoint (next_point.x (), next_point.y (), ego_z));
1493
- }
1495
+ Marker::LINE_STRIP, createMarkerScale (0.1 , 0.1 , 0.1 ), red_color);
1496
+ addFootprintMarker (marker, *collision_check_end_pose, vehicle_info_);
1494
1497
marker.lifetime = life_time;
1495
- debug_marker_ .markers .push_back (marker);
1498
+ info_marker_ .markers .push_back (marker);
1496
1499
}
1497
1500
}
1498
1501
// start pose candidates
@@ -1519,8 +1522,8 @@ void StartPlannerModule::setDebugData()
1519
1522
start_pose_text_marker_array.markers .push_back (text_marker);
1520
1523
}
1521
1524
1522
- add (start_pose_footprint_marker_array);
1523
- add (start_pose_text_marker_array);
1525
+ add (start_pose_footprint_marker_array, debug_marker_ );
1526
+ add (start_pose_text_marker_array, debug_marker_ );
1524
1527
}
1525
1528
1526
1529
// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
@@ -1552,26 +1555,32 @@ void StartPlannerModule::setDebugData()
1552
1555
pull_out_path_footprint_marker_array.markers .push_back (pull_out_path_footprint_marker);
1553
1556
}
1554
1557
1555
- add (pull_out_path_footprint_marker_array);
1558
+ add (pull_out_path_footprint_marker_array, debug_marker_ );
1556
1559
}
1557
1560
1558
1561
// safety check
1559
1562
if (parameters_->safety_check_params .enable_safety_check ) {
1560
1563
if (debug_data_.ego_predicted_path .size () > 0 ) {
1561
1564
const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath (
1562
1565
debug_data_.ego_predicted_path , ego_predicted_path_params_->time_resolution );
1563
- add (createPredictedPathMarkerArray (
1564
- ego_predicted_path, vehicle_info_, " ego_predicted_path_start_planner" , 0 , 0.0 , 0.5 , 0.9 ));
1566
+ add (
1567
+ createPredictedPathMarkerArray (
1568
+ ego_predicted_path, vehicle_info_, " ego_predicted_path_start_planner" , 0 , 0.0 , 0.5 , 0.9 ),
1569
+ debug_marker_);
1565
1570
}
1566
1571
1567
1572
if (debug_data_.filtered_objects .objects .size () > 0 ) {
1568
- add (createObjectsMarkerArray (
1569
- debug_data_.filtered_objects , " filtered_objects" , 0 , 0.0 , 0.5 , 0.9 ));
1573
+ add (
1574
+ createObjectsMarkerArray (
1575
+ debug_data_.filtered_objects , " filtered_objects" , 0 , 0.0 , 0.5 , 0.9 ),
1576
+ info_marker_);
1570
1577
}
1571
1578
1572
- add (showSafetyCheckInfo (debug_data_.collision_check , " object_debug_info" ));
1573
- add (showPredictedPath (debug_data_.collision_check , " ego_predicted_path" ));
1574
- add (showPolygon (debug_data_.collision_check , " ego_and_target_polygon_relation" ));
1579
+ add (showSafetyCheckInfo (debug_data_.collision_check , " object_debug_info" ), debug_marker_);
1580
+ add (
1581
+ showPredictedPath (debug_data_.collision_check , " predicted_path_for_safety_check" ),
1582
+ info_marker_);
1583
+ add (showPolygon (debug_data_.collision_check , " ego_and_target_polygon_relation" ), info_marker_);
1575
1584
1576
1585
// set objects of interest
1577
1586
for (const auto & [uuid, data] : debug_data_.collision_check ) {
@@ -1599,17 +1608,21 @@ void StartPlannerModule::setDebugData()
1599
1608
std::to_string (status_.pull_out_path .partial_paths .size () - 1 );
1600
1609
marker.lifetime = life_time;
1601
1610
planner_type_marker_array.markers .push_back (marker);
1602
- add (planner_type_marker_array);
1611
+ add (planner_type_marker_array, info_marker_ );
1603
1612
}
1604
1613
1605
- add (laneletsAsTriangleMarkerArray (
1606
- " departure_check_lanes_for_shift_pull_out_path" , debug_data_.departure_check_lanes ,
1607
- cyan_color));
1614
+ add (
1615
+ laneletsAsTriangleMarkerArray (
1616
+ " departure_check_lanes_for_shift_pull_out_path" , debug_data_.departure_check_lanes ,
1617
+ cyan_color),
1618
+ debug_marker_);
1608
1619
1609
1620
const auto pull_out_lanes = start_planner_utils::getPullOutLanes (
1610
1621
planner_data_, planner_data_->parameters .backward_path_length + parameters_->max_back_distance );
1611
- add (laneletsAsTriangleMarkerArray (
1612
- " pull_out_lanes_for_static_objects_collision_check" , pull_out_lanes, pink_color));
1622
+ add (
1623
+ laneletsAsTriangleMarkerArray (
1624
+ " pull_out_lanes_for_static_objects_collision_check" , pull_out_lanes, pink_color),
1625
+ debug_marker_);
1613
1626
}
1614
1627
1615
1628
void StartPlannerModule::logPullOutStatus (rclcpp::Logger::Level log_level) const
0 commit comments