23
23
24
24
#include < lanelet2_extension/utility/query.hpp>
25
25
#include < lanelet2_extension/utility/utilities.hpp>
26
+ #include < lanelet2_extension/visualization/visualization.hpp>
26
27
#include < magic_enum.hpp>
27
28
#include < rclcpp/rclcpp.hpp>
28
29
@@ -136,7 +137,7 @@ void StartPlannerModule::initVariables()
136
137
debug_marker_.markers .clear ();
137
138
initializeSafetyCheckParameters ();
138
139
initializeCollisionCheckDebugMap (debug_data_.collision_check );
139
- updateDrivableLanes ();
140
+ updateDepartureCheckLanes ();
140
141
}
141
142
142
143
void StartPlannerModule::updateEgoPredictedPathParams (
@@ -564,7 +565,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
564
565
void StartPlannerModule::resetStatus ()
565
566
{
566
567
status_ = PullOutStatus{};
567
- updateDrivableLanes ();
568
+ updateDepartureCheckLanes ();
568
569
}
569
570
570
571
void StartPlannerModule::incrementPathIndex ()
@@ -1202,8 +1203,12 @@ bool StartPlannerModule::isSafePath() const
1202
1203
const double hysteresis_factor =
1203
1204
status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate ;
1204
1205
1205
- behavior_path_planner::updateSafetyCheckDebugData (
1206
- debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
1206
+ // debug
1207
+ {
1208
+ debug_data_.filtered_objects = filtered_objects;
1209
+ debug_data_.target_objects_on_lane = target_objects_on_lane;
1210
+ debug_data_.ego_predicted_path = ego_predicted_path;
1211
+ }
1207
1212
1208
1213
return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS (
1209
1214
pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane ,
@@ -1331,19 +1336,23 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
1331
1336
}
1332
1337
}
1333
1338
1334
- void StartPlannerModule::updateDrivableLanes ()
1339
+ void StartPlannerModule::updateDepartureCheckLanes ()
1335
1340
{
1336
- const auto drivable_lanes = createDrivableLanes ();
1341
+ const auto departure_check_lanes = createDepartureCheckLanes ();
1337
1342
for (auto & planner : start_planners_) {
1338
1343
auto shift_pull_out = std::dynamic_pointer_cast<ShiftPullOut>(planner);
1339
1344
1340
1345
if (shift_pull_out) {
1341
- shift_pull_out->setDrivableLanes (drivable_lanes );
1346
+ shift_pull_out->setDepartureCheckLanes (departure_check_lanes );
1342
1347
}
1343
1348
}
1349
+ // debug
1350
+ {
1351
+ debug_data_.departure_check_lanes = departure_check_lanes;
1352
+ }
1344
1353
}
1345
1354
1346
- lanelet::ConstLanelets StartPlannerModule::createDrivableLanes () const
1355
+ lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes () const
1347
1356
{
1348
1357
const double backward_path_length =
1349
1358
planner_data_->parameters .backward_path_length + parameters_->max_back_distance ;
@@ -1362,13 +1371,14 @@ lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const
1362
1371
[this ](const auto & pull_out_lane) {
1363
1372
return planner_data_->route_handler ->isShoulderLanelet (pull_out_lane);
1364
1373
});
1365
- const auto drivable_lanes = utils::transformToLanelets (
1374
+ const auto departure_check_lanes = utils::transformToLanelets (
1366
1375
utils::generateDrivableLanesWithShoulderLanes (road_lanes, shoulder_lanes));
1367
- return drivable_lanes ;
1376
+ return departure_check_lanes ;
1368
1377
}
1369
1378
1370
1379
void StartPlannerModule::setDebugData ()
1371
1380
{
1381
+ using lanelet::visualization::laneletsAsTriangleMarkerArray;
1372
1382
using marker_utils::addFootprintMarker;
1373
1383
using marker_utils::createFootprintMarkerArray;
1374
1384
using marker_utils::createObjectsMarkerArray;
@@ -1383,6 +1393,12 @@ void StartPlannerModule::setDebugData()
1383
1393
using tier4_autoware_utils::createMarkerScale;
1384
1394
using visualization_msgs::msg::Marker;
1385
1395
1396
+ const auto red_color = createMarkerColor (1.0 , 0.0 , 0.0 , 0.999 );
1397
+ const auto cyan_color = createMarkerColor (0.0 , 1.0 , 1.0 , 0.2 );
1398
+ const auto pink_color = createMarkerColor (1.0 , 0.5 , 0.5 , 0.35 );
1399
+ const auto purple_color = createMarkerColor (1.0 , 0.0 , 1.0 , 0.99 );
1400
+ const auto white_color = createMarkerColor (1.0 , 1.0 , 1.0 , 0.99 );
1401
+
1386
1402
const auto life_time = rclcpp::Duration::from_seconds (1.5 );
1387
1403
auto add = [&](MarkerArray added) {
1388
1404
for (auto & marker : added.markers ) {
@@ -1409,10 +1425,9 @@ void StartPlannerModule::setDebugData()
1409
1425
if (collision_check_end_pose) {
1410
1426
add (createPoseMarkerArray (
1411
1427
*collision_check_end_pose, " static_collision_check_end_pose" , 0 , 1.0 , 0.0 , 0.0 ));
1412
- auto marker = tier4_autoware_utils:: createDefaultMarker (
1428
+ auto marker = createDefaultMarker (
1413
1429
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " static_collision_check_end_polygon" , 0 ,
1414
- Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale (0.1 , 0.1 , 0.1 ),
1415
- tier4_autoware_utils::createMarkerColor (1.0 , 0.0 , 0.0 , 0.999 ));
1430
+ Marker::LINE_LIST, createMarkerScale (0.1 , 0.1 , 0.1 ), red_color);
1416
1431
const auto footprint = transformVector (
1417
1432
local_footprint, tier4_autoware_utils::pose2transform (*collision_check_end_pose));
1418
1433
const double ego_z = planner_data_->self_odometry ->pose .pose .position .z ;
@@ -1432,13 +1447,13 @@ void StartPlannerModule::setDebugData()
1432
1447
{
1433
1448
MarkerArray start_pose_footprint_marker_array{};
1434
1449
MarkerArray start_pose_text_marker_array{};
1435
- const auto purple = createMarkerColor (1.0 , 0.0 , 1.0 , 0.99 );
1436
1450
Marker footprint_marker = createDefaultMarker (
1437
1451
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " start_pose_candidates" , 0 , Marker::LINE_STRIP,
1438
- createMarkerScale (0.2 , 0.2 , 0.2 ), purple );
1452
+ createMarkerScale (0.2 , 0.2 , 0.2 ), purple_color );
1439
1453
Marker text_marker = createDefaultMarker (
1440
1454
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " start_pose_candidates_idx" , 0 ,
1441
- visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale (0.3 , 0.3 , 0.3 ), purple);
1455
+ visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale (0.3 , 0.3 , 0.3 ),
1456
+ purple_color);
1442
1457
footprint_marker.lifetime = rclcpp::Duration::from_seconds (1.5 );
1443
1458
text_marker.lifetime = rclcpp::Duration::from_seconds (1.5 );
1444
1459
for (size_t i = 0 ; i < debug_data_.start_pose_candidates .size (); ++i) {
@@ -1459,10 +1474,9 @@ void StartPlannerModule::setDebugData()
1459
1474
// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
1460
1475
{
1461
1476
MarkerArray pull_out_path_footprint_marker_array{};
1462
- const auto pink = createMarkerColor (1.0 , 0.0 , 1.0 , 0.99 );
1463
1477
Marker pull_out_path_footprint_marker = createDefaultMarker (
1464
1478
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " shift_path_footprint" , 0 , Marker::LINE_STRIP,
1465
- createMarkerScale (0.2 , 0.2 , 0.2 ), pink );
1479
+ createMarkerScale (0.2 , 0.2 , 0.2 ), pink_color );
1466
1480
pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds (1.5 );
1467
1481
PathWithLaneId path_shift_start_to_end{};
1468
1482
const auto shift_path = status_.pull_out_path .partial_paths .front ();
@@ -1520,8 +1534,7 @@ void StartPlannerModule::setDebugData()
1520
1534
const auto header = planner_data_->route_handler ->getRouteHeader ();
1521
1535
{
1522
1536
visualization_msgs::msg::MarkerArray planner_type_marker_array{};
1523
- const auto color = status_.found_pull_out_path ? createMarkerColor (1.0 , 1.0 , 1.0 , 0.99 )
1524
- : createMarkerColor (1.0 , 0.0 , 0.0 , 0.99 );
1537
+ const auto color = status_.found_pull_out_path ? white_color : red_color;
1525
1538
auto marker = createDefaultMarker (
1526
1539
header.frame_id , header.stamp , " planner_type" , 0 ,
1527
1540
visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale (0.0 , 0.0 , 1.0 ), color);
@@ -1536,6 +1549,15 @@ void StartPlannerModule::setDebugData()
1536
1549
planner_type_marker_array.markers .push_back (marker);
1537
1550
add (planner_type_marker_array);
1538
1551
}
1552
+
1553
+ add (laneletsAsTriangleMarkerArray (
1554
+ " departure_check_lanes_for_shift_pull_out_path" , debug_data_.departure_check_lanes ,
1555
+ cyan_color));
1556
+
1557
+ const auto pull_out_lanes = start_planner_utils::getPullOutLanes (
1558
+ planner_data_, planner_data_->parameters .backward_path_length + parameters_->max_back_distance );
1559
+ add (laneletsAsTriangleMarkerArray (
1560
+ " pull_out_lanes_for_static_objects_collision_check" , pull_out_lanes, pink_color));
1539
1561
}
1540
1562
1541
1563
void StartPlannerModule::logPullOutStatus (rclcpp::Logger::Level log_level) const
0 commit comments