@@ -182,18 +182,14 @@ void StartPlannerModule::updateData()
182182 status_.first_engaged_time = clock_->now ();
183183 }
184184
185- if (hasFinishedBackwardDriving ()) {
185+ status_.backward_driving_complete = hasFinishedBackwardDriving ();
186+ if (status_.backward_driving_complete ) {
186187 updateStatusAfterBackwardDriving ();
187188 DEBUG_PRINT (" StartPlannerModule::updateData() completed backward driving"
188- } else {
189- status_.backward_driving_complete = false ;
190189 }
191190
192- if (requiresDynamicObjectsCollisionDetection ()) {
193- status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects ();
194- } else {
195- status_.is_safe_dynamic_objects = true ;
196- }
191+ status_.is_safe_dynamic_objects =
192+ (!requiresDynamicObjectsCollisionDetection ()) ? true : !hasCollisionWithDynamicObjects ();
197193}
198194
199195bool StartPlannerModule::hasFinishedBackwardDriving () const
@@ -364,20 +360,17 @@ bool StartPlannerModule::isStopped()
364360
365361bool StartPlannerModule::isExecutionReady () const
366362{
367- bool is_safe = true ;
368363 // Evaluate safety. The situation is not safe if any of the following conditions are met:
369364 // 1. pull out path has not been found
370365 // 2. there is a moving objects around ego
371366 // 3. waiting for approval and there is a collision with dynamic objects
372- if (!status_.found_pull_out_path ) {
373- is_safe = false ;
374- } else if (isWaitingApproval ()) {
375- if (!noMovingObjectsAround ()) {
376- is_safe = false ;
377- } else if (requiresDynamicObjectsCollisionDetection () && hasCollisionWithDynamicObjects ()) {
378- is_safe = false ;
379- }
380- }
367+
368+ const bool is_safe = [&]() -> bool {
369+ if (!status_.found_pull_out_path ) return false ;
370+ if (!isWaitingApproval ()) return true ;
371+ if (!noMovingObjectsAround ()) return false ;
372+ return !(requiresDynamicObjectsCollisionDetection () && hasCollisionWithDynamicObjects ());
373+ }();
381374
382375 if (!is_safe) {
383376 stop_pose_ = planner_data_->self_odometry ->pose .pose ;
@@ -459,14 +452,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
459452
460453 setDrivableAreaInfo (output);
461454
462- const uint16_t steering_factor_direction = std::invoke ([&output]() {
463- if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_LEFT) {
464- return SteeringFactor::LEFT;
465- } else if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_RIGHT) {
466- return SteeringFactor::RIGHT;
467- }
468- return SteeringFactor::STRAIGHT;
469- });
455+ const auto steering_factor_direction = getSteeringFactorDirection (output);
470456
471457 if (status_.driving_forward ) {
472458 const double start_distance = motion_utils::calcSignedArcLength (
@@ -480,15 +466,16 @@ BehaviorModuleOutput StartPlannerModule::plan()
480466 {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose },
481467 {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
482468 SteeringFactor::TURNING, " "
483- } else {
484- const double distance = motion_utils::calcSignedArcLength (
485- path.points , planner_data_->self_odometry ->pose .pose .position ,
486- status_.pull_out_path .start_pose .position );
487- updateRTCStatus (0.0 , distance);
488- steering_factor_interface_ptr_->updateSteeringFactor (
489- {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
490- PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, " "
469+ setDebugData ();
470+ return output;
491471 }
472+ const double distance = motion_utils::calcSignedArcLength (
473+ path.points , planner_data_->self_odometry ->pose .pose .position ,
474+ status_.pull_out_path .start_pose .position );
475+ updateRTCStatus (0.0 , distance);
476+ steering_factor_interface_ptr_->updateSteeringFactor (
477+ {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
478+ PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, " "
492479
493480 setDebugData ();
494481
@@ -569,14 +556,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
569556
570557 setDrivableAreaInfo (output);
571558
572- const uint16_t steering_factor_direction = std::invoke ([&output]() {
573- if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_LEFT) {
574- return SteeringFactor::LEFT;
575- } else if (output.turn_signal_info .turn_signal .command == TurnIndicatorsCommand::ENABLE_RIGHT) {
576- return SteeringFactor::RIGHT;
577- }
578- return SteeringFactor::STRAIGHT;
579- });
559+ const auto steering_factor_direction = getSteeringFactorDirection (output);
580560
581561 if (status_.driving_forward ) {
582562 const double start_distance = motion_utils::calcSignedArcLength (
@@ -590,15 +570,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
590570 {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose },
591571 {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
592572 SteeringFactor::APPROACHING, " "
593- } else {
594- const double distance = motion_utils::calcSignedArcLength (
595- stop_path.points , planner_data_->self_odometry ->pose .pose .position ,
596- status_.pull_out_path .start_pose .position );
597- updateRTCStatus (0.0 , distance);
598- steering_factor_interface_ptr_->updateSteeringFactor (
599- {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
600- PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, " "
573+ setDebugData ();
574+
575+ return output;
601576 }
577+ const double distance = motion_utils::calcSignedArcLength (
578+ stop_path.points , planner_data_->self_odometry ->pose .pose .position ,
579+ status_.pull_out_path .start_pose .position );
580+ updateRTCStatus (0.0 , distance);
581+ steering_factor_interface_ptr_->updateSteeringFactor (
582+ {status_.pull_out_path .start_pose , status_.pull_out_path .end_pose }, {0.0 , distance},
583+ PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, " "
602584
603585 setDebugData ();
604586
@@ -658,16 +640,20 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
658640 order_priority.emplace_back (i, planner);
659641 }
660642 }
661- } else if (search_priority == " short_back_distance"
643+ return order_priority;
644+ }
645+
646+ if (search_priority == " short_back_distance"
662647 for (size_t i = 0 ; i < start_pose_candidates_num; i++) {
663648 for (const auto & planner : start_planners_) {
664649 order_priority.emplace_back (i, planner);
665650 }
666651 }
667- } else {
668- RCLCPP_ERROR (getLogger (), " Invalid search_priority: %s" c_str ());
669- throw std::domain_error (" [start_planner] invalid search_priority"
652+ return order_priority;
670653 }
654+
655+ RCLCPP_ERROR (getLogger (), " Invalid search_priority: %s" c_str ());
656+ throw std::domain_error (" [start_planner] invalid search_priority"
671657 return order_priority;
672658}
673659
@@ -1099,12 +1085,7 @@ bool StartPlannerModule::isStuck()
10991085 return false ;
11001086 }
11011087
1102- if (status_.planner_type == PlannerType::STOP) {
1103- return true ;
1104- }
1105-
1106- // not found safe path
1107- if (!status_.found_pull_out_path ) {
1088+ if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path ) {
11081089 return true ;
11091090 }
11101091
@@ -1197,30 +1178,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
11971178 return false ;
11981179 });
11991180
1200- if (is_near_intersection) {
1201- // offset required end pose with ration to activate turn signal for intersection
1202- turn_signal.required_end_point = std::invoke ([&]() {
1203- const double length_start_to_end =
1204- motion_utils::calcSignedArcLength (path.points , start_pose.position , end_pose.position );
1205- const auto ratio = std::clamp (
1206- parameters_->length_ratio_for_turn_signal_deactivation_near_intersection , 0.0 , 1.0 );
1207-
1208- const double required_end_length = length_start_to_end * ratio;
1209- double accumulated_length = 0.0 ;
1210- const size_t start_idx = motion_utils::findNearestIndex (path.points , start_pose.position );
1211- for (size_t i = start_idx; i < path.points .size () - 1 ; ++i) {
1212- accumulated_length +=
1213- tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1214- if (accumulated_length > required_end_length) {
1215- return path.points .at (i).point .pose ;
1216- }
1181+ turn_signal.required_end_point = std::invoke ([&]() {
1182+ if (is_near_intersection) return end_pose;
1183+ const double length_start_to_end =
1184+ motion_utils::calcSignedArcLength (path.points , start_pose.position , end_pose.position );
1185+ const auto ratio = std::clamp (
1186+ parameters_->length_ratio_for_turn_signal_deactivation_near_intersection , 0.0 , 1.0 );
1187+
1188+ const double required_end_length = length_start_to_end * ratio;
1189+ double accumulated_length = 0.0 ;
1190+ const size_t start_idx = motion_utils::findNearestIndex (path.points , start_pose.position );
1191+ for (size_t i = start_idx; i < path.points .size () - 1 ; ++i) {
1192+ accumulated_length +=
1193+ tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1194+ if (accumulated_length > required_end_length) {
1195+ return path.points .at (i).point .pose ;
12171196 }
1218- // not found required end point
1219- return end_pose;
1220- });
1221- } else {
1222- turn_signal.required_end_point = end_pose;
1223- }
1197+ }
1198+ // not found required end point
1199+ return end_pose;
1200+ });
12241201
12251202 return turn_signal;
12261203}
@@ -1389,22 +1366,27 @@ bool StartPlannerModule::planFreespacePath()
13891366
13901367void StartPlannerModule::setDrivableAreaInfo (BehaviorModuleOutput & output) const
13911368{
1392- if (status_.planner_type == PlannerType::FREESPACE) {
1393- const double drivable_area_margin = planner_data_->parameters .vehicle_width ;
1394- output.drivable_area_info .drivable_margin =
1395- planner_data_->parameters .vehicle_width / 2.0 + drivable_area_margin;
1396- } else {
1397- const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes (
1398- output.path , generateDrivableLanes (output.path ),
1399- planner_data_->drivable_area_expansion_parameters );
1400-
1401- DrivableAreaInfo current_drivable_area_info;
1402- current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1403- output.drivable_area_info =
1404- status_.driving_forward
1405- ? utils::combineDrivableAreaInfo (
1406- current_drivable_area_info, getPreviousModuleOutput ().drivable_area_info )
1407- : current_drivable_area_info;
1369+ switch (status_.planner_type ) {
1370+ case PlannerType::FREESPACE: {
1371+ const double drivable_area_margin = planner_data_->parameters .vehicle_width ;
1372+ output.drivable_area_info .drivable_margin =
1373+ planner_data_->parameters .vehicle_width / 2.0 + drivable_area_margin;
1374+ break ;
1375+ }
1376+ default : {
1377+ const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes (
1378+ output.path , generateDrivableLanes (output.path ),
1379+ planner_data_->drivable_area_expansion_parameters );
1380+
1381+ DrivableAreaInfo current_drivable_area_info;
1382+ current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1383+ output.drivable_area_info =
1384+ status_.driving_forward
1385+ ? utils::combineDrivableAreaInfo (
1386+ current_drivable_area_info, getPreviousModuleOutput ().drivable_area_info )
1387+ : current_drivable_area_info;
1388+ break ;
1389+ }
14081390 }
14091391}
14101392
0 commit comments