|
21 | 21 | #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
|
22 | 22 | #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
|
23 | 23 | #include "behavior_path_planner_common/utils/path_utils.hpp"
|
| 24 | +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" |
24 | 25 | #include "behavior_path_planner_common/utils/utils.hpp"
|
25 | 26 |
|
26 | 27 | #include <lanelet2_extension/utility/message_conversion.hpp>
|
|
32 | 33 | #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
|
33 | 34 | #include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
|
34 | 35 |
|
35 |
| -#include <boost/geometry/algorithms/centroid.hpp> |
36 |
| - |
37 | 36 | #include <algorithm>
|
38 | 37 | #include <limits>
|
39 | 38 | #include <memory>
|
@@ -223,24 +222,51 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
|
223 | 222 | utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
|
224 | 223 |
|
225 | 224 | // expand drivable lanes
|
226 |
| - const auto has_shift_point = !path_shifter_.getShiftLines().empty(); |
227 |
| - const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); |
| 225 | + const auto is_within_current_lane = |
| 226 | + utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); |
| 227 | + const auto red_signal_lane_itr = std::find_if( |
| 228 | + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { |
| 229 | + const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); |
| 230 | + return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_); |
| 231 | + }); |
| 232 | + const auto not_use_adjacent_lane = |
| 233 | + is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end(); |
| 234 | + |
228 | 235 | std::for_each(
|
229 | 236 | data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
|
230 |
| - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( |
231 |
| - lanelet, planner_data_, parameters_, in_avoidance_maneuver)); |
| 237 | + if (!not_use_adjacent_lane) { |
| 238 | + data.drivable_lanes.push_back( |
| 239 | + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); |
| 240 | + } else if (red_signal_lane_itr->id() != lanelet.id()) { |
| 241 | + data.drivable_lanes.push_back( |
| 242 | + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); |
| 243 | + } else { |
| 244 | + data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); |
| 245 | + } |
232 | 246 | });
|
233 | 247 |
|
234 | 248 | // calc drivable bound
|
235 | 249 | auto tmp_path = getPreviousModuleOutput().path;
|
236 | 250 | const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
|
| 251 | + const auto use_left_side_hatched_road_marking_area = [&]() { |
| 252 | + if (!not_use_adjacent_lane) { |
| 253 | + return true; |
| 254 | + } |
| 255 | + return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr); |
| 256 | + }(); |
| 257 | + const auto use_right_side_hatched_road_marking_area = [&]() { |
| 258 | + if (!not_use_adjacent_lane) { |
| 259 | + return true; |
| 260 | + } |
| 261 | + return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr); |
| 262 | + }(); |
237 | 263 | data.left_bound = utils::calcBound(
|
238 | 264 | getPreviousModuleOutput().path, planner_data_, shorten_lanes,
|
239 |
| - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, |
| 265 | + use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas, |
240 | 266 | parameters_->use_freespace_areas, true);
|
241 | 267 | data.right_bound = utils::calcBound(
|
242 | 268 | getPreviousModuleOutput().path, planner_data_, shorten_lanes,
|
243 |
| - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, |
| 269 | + use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas, |
244 | 270 | parameters_->use_freespace_areas, false);
|
245 | 271 |
|
246 | 272 | // reference path
|
@@ -939,7 +965,11 @@ BehaviorModuleOutput AvoidanceModule::plan()
|
939 | 965 | {
|
940 | 966 | DrivableAreaInfo current_drivable_area_info;
|
941 | 967 | // generate drivable lanes
|
942 |
| - current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; |
| 968 | + std::for_each( |
| 969 | + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { |
| 970 | + current_drivable_area_info.drivable_lanes.push_back( |
| 971 | + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); |
| 972 | + }); |
943 | 973 | // expand hatched road markings
|
944 | 974 | current_drivable_area_info.enable_expanding_hatched_road_markings =
|
945 | 975 | parameters_->use_hatched_road_markings;
|
|
0 commit comments