@@ -176,18 +176,14 @@ void StartPlannerModule::updateData()
176
176
status_.first_engaged_and_driving_forward_time = clock_->now ();
177
177
}
178
178
179
- if (hasFinishedBackwardDriving ()) {
179
+ status_.backward_driving_complete = hasFinishedBackwardDriving ();
180
+ if (status_.backward_driving_complete ) {
180
181
updateStatusAfterBackwardDriving ();
181
182
DEBUG_PRINT (" StartPlannerModule::updateData() completed backward driving" );
182
- } else {
183
- status_.backward_driving_complete = false ;
184
183
}
185
184
186
- if (requiresDynamicObjectsCollisionDetection ()) {
187
- status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects ();
188
- } else {
189
- status_.is_safe_dynamic_objects = true ;
190
- }
185
+ status_.is_safe_dynamic_objects =
186
+ (!requiresDynamicObjectsCollisionDetection ()) ? true : !hasCollisionWithDynamicObjects ();
191
187
}
192
188
193
189
bool StartPlannerModule::hasFinishedBackwardDriving () const
@@ -326,20 +322,17 @@ bool StartPlannerModule::isStopped()
326
322
327
323
bool StartPlannerModule::isExecutionReady () const
328
324
{
329
- bool is_safe = true ;
330
325
// Evaluate safety. The situation is not safe if any of the following conditions are met:
331
326
// 1. pull out path has not been found
332
327
// 2. there is a moving objects around ego
333
328
// 3. waiting for approval and there is a collision with dynamic objects
334
- if (!status_.found_pull_out_path ) {
335
- is_safe = false ;
336
- } else if (isWaitingApproval ()) {
337
- if (!noMovingObjectsAround ()) {
338
- is_safe = false ;
339
- } else if (requiresDynamicObjectsCollisionDetection () && hasCollisionWithDynamicObjects ()) {
340
- is_safe = false ;
341
- }
342
- }
329
+
330
+ const bool is_safe = [&]() -> bool {
331
+ if (!status_.found_pull_out_path ) return false ;
332
+ if (!isWaitingApproval ()) return true ;
333
+ if (!noMovingObjectsAround ()) return false ;
334
+ return !(requiresDynamicObjectsCollisionDetection () && hasCollisionWithDynamicObjects ());
335
+ }();
343
336
344
337
if (!is_safe) {
345
338
stop_pose_ = planner_data_->self_odometry ->pose .pose ;
@@ -420,14 +413,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
420
413
421
414
setDrivableAreaInfo (output);
422
415
423
- const uint16_t steering_factor_direction = std::invoke ([&output]() {
424
- if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_LEFT) {
425
- return SteeringFactor::LEFT;
426
- } else if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_RIGHT) {
427
- return SteeringFactor::RIGHT;
428
- }
429
- return SteeringFactor::STRAIGHT;
430
- });
416
+ const auto steering_factor_direction = getSteeringFactorDirection (output);
431
417
432
418
if (status_.driving_forward ) {
433
419
const double start_distance = motion_utils::calcSignedArcLength (
@@ -441,15 +427,16 @@ BehaviorModuleOutput StartPlannerModule::plan()
441
427
{status_.pull_out_path .start_pose , status_.pull_out_path .end_pose },
442
428
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
443
429
SteeringFactor::TURNING, " " );
444
- } else {
445
- const double distance = motion_utils::calcSignedArcLength (
446
- path.points , planner_data_->self_odometry ->pose .pose .position ,
447
- status_.pull_out_path .start_pose .position );
448
- updateRTCStatus (0.0 , distance);
449
- steering_factor_interface_ptr_->updateSteeringFactor (
450
- {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
451
- PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, " " );
430
+ setDebugData ();
431
+ return output;
452
432
}
433
+ const double distance = motion_utils::calcSignedArcLength (
434
+ path.points , planner_data_->self_odometry ->pose .pose .position ,
435
+ status_.pull_out_path .start_pose .position );
436
+ updateRTCStatus (0.0 , distance);
437
+ steering_factor_interface_ptr_->updateSteeringFactor (
438
+ {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
439
+ PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, " " );
453
440
454
441
setDebugData ();
455
442
@@ -530,14 +517,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
530
517
531
518
setDrivableAreaInfo (output);
532
519
533
- const uint16_t steering_factor_direction = std::invoke ([&output]() {
534
- if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_LEFT) {
535
- return SteeringFactor::LEFT;
536
- } else if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_RIGHT) {
537
- return SteeringFactor::RIGHT;
538
- }
539
- return SteeringFactor::STRAIGHT;
540
- });
520
+ const auto steering_factor_direction = getSteeringFactorDirection (output);
541
521
542
522
if (status_.driving_forward ) {
543
523
const double start_distance = motion_utils::calcSignedArcLength (
@@ -551,15 +531,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
551
531
{status_.pull_out_path .start_pose , status_.pull_out_path .end_pose },
552
532
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
553
533
SteeringFactor::APPROACHING, " " );
554
- } else {
555
- const double distance = motion_utils::calcSignedArcLength (
556
- stop_path.points , planner_data_->self_odometry ->pose .pose .position ,
557
- status_.pull_out_path .start_pose .position );
558
- updateRTCStatus (0.0 , distance);
559
- steering_factor_interface_ptr_->updateSteeringFactor (
560
- {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
561
- PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, " " );
534
+ setDebugData ();
535
+
536
+ return output;
562
537
}
538
+ const double distance = motion_utils::calcSignedArcLength (
539
+ stop_path.points , planner_data_->self_odometry ->pose .pose .position ,
540
+ status_.pull_out_path .start_pose .position );
541
+ updateRTCStatus (0.0 , distance);
542
+ steering_factor_interface_ptr_->updateSteeringFactor (
543
+ {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
544
+ PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, " " );
563
545
564
546
setDebugData ();
565
547
@@ -618,10 +600,11 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
618
600
order_priority.emplace_back (i, planner);
619
601
}
620
602
}
621
- } else {
622
- RCLCPP_ERROR (getLogger (), " Invalid search_priority: %s" , search_priority.c_str ());
623
- throw std::domain_error (" [start_planner] invalid search_priority" );
603
+ return order_priority;
624
604
}
605
+
606
+ RCLCPP_ERROR (getLogger (), " Invalid search_priority: %s" , search_priority.c_str ());
607
+ throw std::domain_error (" [start_planner] invalid search_priority" );
625
608
return order_priority;
626
609
}
627
610
@@ -990,12 +973,7 @@ bool StartPlannerModule::isStuck()
990
973
return false ;
991
974
}
992
975
993
- if (status_.planner_type == PlannerType::STOP) {
994
- return true ;
995
- }
996
-
997
- // not found safe path
998
- if (!status_.found_pull_out_path ) {
976
+ if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path ) {
999
977
return true ;
1000
978
}
1001
979
@@ -1088,30 +1066,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
1088
1066
return false ;
1089
1067
});
1090
1068
1091
- if (is_near_intersection) {
1092
- // offset required end pose with ration to activate turn signal for intersection
1093
- turn_signal.required_end_point = std::invoke ([&]() {
1094
- const double length_start_to_end =
1095
- motion_utils::calcSignedArcLength (path.points , start_pose.position , end_pose.position );
1096
- const auto ratio = std::clamp (
1097
- parameters_->length_ratio_for_turn_signal_deactivation_near_intersection , 0.0 , 1.0 );
1098
-
1099
- const double required_end_length = length_start_to_end * ratio;
1100
- double accumulated_length = 0.0 ;
1101
- const size_t start_idx = motion_utils::findNearestIndex (path.points , start_pose.position );
1102
- for (size_t i = start_idx; i < path.points .size () - 1 ; ++i) {
1103
- accumulated_length +=
1104
- tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1105
- if (accumulated_length > required_end_length) {
1106
- return path.points .at (i).point .pose ;
1107
- }
1069
+ turn_signal.required_end_point = std::invoke ([&]() {
1070
+ if (is_near_intersection) return end_pose;
1071
+ const double length_start_to_end =
1072
+ motion_utils::calcSignedArcLength (path.points , start_pose.position , end_pose.position );
1073
+ const auto ratio = std::clamp (
1074
+ parameters_->length_ratio_for_turn_signal_deactivation_near_intersection , 0.0 , 1.0 );
1075
+
1076
+ const double required_end_length = length_start_to_end * ratio;
1077
+ double accumulated_length = 0.0 ;
1078
+ const size_t start_idx = motion_utils::findNearestIndex (path.points , start_pose.position );
1079
+ for (size_t i = start_idx; i < path.points .size () - 1 ; ++i) {
1080
+ accumulated_length +=
1081
+ tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1082
+ if (accumulated_length > required_end_length) {
1083
+ return path.points .at (i).point .pose ;
1108
1084
}
1109
- // not found required end point
1110
- return end_pose;
1111
- });
1112
- } else {
1113
- turn_signal.required_end_point = end_pose;
1114
- }
1085
+ }
1086
+ // not found required end point
1087
+ return end_pose;
1088
+ });
1115
1089
1116
1090
return turn_signal;
1117
1091
}
@@ -1279,23 +1253,27 @@ bool StartPlannerModule::planFreespacePath()
1279
1253
1280
1254
void StartPlannerModule::setDrivableAreaInfo (BehaviorModuleOutput & output) const
1281
1255
{
1282
- if (status_.planner_type == PlannerType::FREESPACE) {
1283
- std::cerr << " Freespace planner updated drivable area." << std::endl;
1284
- const double drivable_area_margin = planner_data_->parameters .vehicle_width ;
1285
- output.drivable_area_info .drivable_margin =
1286
- planner_data_->parameters .vehicle_width / 2.0 + drivable_area_margin;
1287
- } else {
1288
- const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes (
1289
- output.path , generateDrivableLanes (output.path ),
1290
- planner_data_->drivable_area_expansion_parameters );
1291
-
1292
- DrivableAreaInfo current_drivable_area_info;
1293
- current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1294
- output.drivable_area_info =
1295
- status_.driving_forward
1296
- ? utils::combineDrivableAreaInfo (
1297
- current_drivable_area_info, getPreviousModuleOutput ().drivable_area_info )
1298
- : current_drivable_area_info;
1256
+ switch (status_.planner_type ) {
1257
+ case PlannerType::FREESPACE: {
1258
+ const double drivable_area_margin = planner_data_->parameters .vehicle_width ;
1259
+ output.drivable_area_info .drivable_margin =
1260
+ planner_data_->parameters .vehicle_width / 2.0 + drivable_area_margin;
1261
+ break ;
1262
+ }
1263
+ default : {
1264
+ const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes (
1265
+ output.path , generateDrivableLanes (output.path ),
1266
+ planner_data_->drivable_area_expansion_parameters );
1267
+
1268
+ DrivableAreaInfo current_drivable_area_info;
1269
+ current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1270
+ output.drivable_area_info =
1271
+ status_.driving_forward
1272
+ ? utils::combineDrivableAreaInfo (
1273
+ current_drivable_area_info, getPreviousModuleOutput ().drivable_area_info )
1274
+ : current_drivable_area_info;
1275
+ break ;
1276
+ }
1299
1277
}
1300
1278
}
1301
1279
0 commit comments