Skip to content

Commit 2323c6f

Browse files
committed
fix(avoidance): limit drivable lane only when the ego in on original lane (autowarefoundation#6349)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent a4ba10a commit 2323c6f

File tree

4 files changed

+97
-25
lines changed

4 files changed

+97
-25
lines changed

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
155155
// expand drivable lanes
156156
std::for_each(
157157
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
158-
data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
159-
lanelet, planner_data_, avoidance_parameters_, false));
158+
data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
159+
lanelet, planner_data_, avoidance_parameters_));
160160
});
161161

162162
// calc drivable bound

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj);
3636
double calcShiftLength(
3737
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
3838

39+
bool isWithinLanes(
40+
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data);
41+
3942
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
4043

4144
bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
@@ -157,9 +160,11 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
157160
const std::shared_ptr<AvoidanceParameters> & parameters,
158161
const double object_check_forward_distance, DebugData & debug);
159162

160-
DrivableLanes generateExpandDrivableLanes(
163+
DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet);
164+
165+
DrivableLanes generateExpandedDrivableLanes(
161166
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
162-
const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver);
167+
const std::shared_ptr<AvoidanceParameters> & parameters);
163168

164169
double calcDistanceToReturnDeadLine(
165170
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,

planning/behavior_path_avoidance_module/src/scene.cpp

+39-9
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
2222
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
2323
#include "behavior_path_planner_common/utils/path_utils.hpp"
24+
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
2425
#include "behavior_path_planner_common/utils/utils.hpp"
2526

2627
#include <lanelet2_extension/utility/message_conversion.hpp>
@@ -32,8 +33,6 @@
3233
#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
3334
#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
3435

35-
#include <boost/geometry/algorithms/centroid.hpp>
36-
3736
#include <algorithm>
3837
#include <limits>
3938
#include <memory>
@@ -223,24 +222,51 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
223222
utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
224223

225224
// 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+
228235
std::for_each(
229236
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+
}
232246
});
233247

234248
// calc drivable bound
235249
auto tmp_path = getPreviousModuleOutput().path;
236250
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+
}();
237263
data.left_bound = utils::calcBound(
238264
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,
240266
parameters_->use_freespace_areas, true);
241267
data.right_bound = utils::calcBound(
242268
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,
244270
parameters_->use_freespace_areas, false);
245271

246272
// reference path
@@ -939,7 +965,11 @@ BehaviorModuleOutput AvoidanceModule::plan()
939965
{
940966
DrivableAreaInfo current_drivable_area_info;
941967
// 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+
});
943973
// expand hatched road markings
944974
current_drivable_area_info.enable_expanding_hatched_road_markings =
945975
parameters_->use_hatched_road_markings;

planning/behavior_path_avoidance_module/src/utils.cpp

+49-12
Original file line numberDiff line numberDiff line change
@@ -957,6 +957,44 @@ double calcShiftLength(
957957
return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
958958
}
959959

960+
bool isWithinLanes(
961+
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
962+
{
963+
const auto & rh = planner_data->route_handler;
964+
const auto & ego_pose = planner_data->self_odometry->pose.pose;
965+
const auto transform = tier4_autoware_utils::pose2transform(ego_pose);
966+
const auto footprint = tier4_autoware_utils::transformVector(
967+
planner_data->parameters.vehicle_info.createFootprint(), transform);
968+
969+
lanelet::ConstLanelet closest_lanelet{};
970+
if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
971+
return true;
972+
}
973+
974+
lanelet::ConstLanelets concat_lanelets{};
975+
976+
// push previous lanelet
977+
lanelet::ConstLanelets prev_lanelet;
978+
if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
979+
concat_lanelets.push_back(prev_lanelet.front());
980+
}
981+
982+
// push nearest lanelet
983+
{
984+
concat_lanelets.push_back(closest_lanelet);
985+
}
986+
987+
// push next lanelet
988+
lanelet::ConstLanelet next_lanelet;
989+
if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
990+
concat_lanelets.push_back(next_lanelet);
991+
}
992+
993+
const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets);
994+
995+
return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon());
996+
}
997+
960998
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
961999
{
9621000
/**
@@ -2076,9 +2114,18 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
20762114
return std::make_pair(target_objects, other_objects);
20772115
}
20782116

2079-
DrivableLanes generateExpandDrivableLanes(
2117+
DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet)
2118+
{
2119+
DrivableLanes current_drivable_lanes;
2120+
current_drivable_lanes.left_lane = lanelet;
2121+
current_drivable_lanes.right_lane = lanelet;
2122+
2123+
return current_drivable_lanes;
2124+
}
2125+
2126+
DrivableLanes generateExpandedDrivableLanes(
20802127
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
2081-
const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver)
2128+
const std::shared_ptr<AvoidanceParameters> & parameters)
20822129
{
20832130
const auto & route_handler = planner_data->route_handler;
20842131

@@ -2092,11 +2139,6 @@ DrivableLanes generateExpandDrivableLanes(
20922139

20932140
// 1. get left/right side lanes
20942141
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
2095-
const auto next_lanes = route_handler->getNextLanelets(target_lane);
2096-
const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
2097-
if (is_stop_signal && !in_avoidance_maneuver) {
2098-
return;
2099-
}
21002142
const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
21012143
target_lane, parameters->use_opposite_lane, true);
21022144
if (!all_left_lanelets.empty()) {
@@ -2107,11 +2149,6 @@ DrivableLanes generateExpandDrivableLanes(
21072149
}
21082150
};
21092151
const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
2110-
const auto next_lanes = route_handler->getNextLanelets(target_lane);
2111-
const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
2112-
if (is_stop_signal && !in_avoidance_maneuver) {
2113-
return;
2114-
}
21152152
const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
21162153
target_lane, parameters->use_opposite_lane, true);
21172154
if (!all_right_lanelets.empty()) {

0 commit comments

Comments
 (0)