diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 608e8c45b2c04..4fb36a413bba4 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -141,17 +141,17 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; + if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); - const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; - utils::generateDrivableArea( *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data); + *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); @@ -163,7 +163,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - data->parameters.vehicle_length, data); + data->parameters.vehicle_length, data, is_driving_forward); } // extract obstacles from drivable area