From 0a66438a842a4c9d1204bae0cc7d408a347c45d9 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Wed, 7 Feb 2024 17:31:35 +0900
Subject: [PATCH] fix(avoidance): limit drivable lane only when the ego in on
original lane
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
.../src/scene.cpp | 4 +-
.../behavior_path_avoidance_module/utils.hpp | 9 ++-
.../src/scene.cpp | 48 ++++++++++++---
.../src/utils.cpp | 61 +++++++++++++++----
4 files changed, 97 insertions(+), 25 deletions(-)
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
index 501d1c157d4df..9f3966e055d9b 100644
--- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
// expand drivable lanes
std::for_each(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
- data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
- lanelet, planner_data_, avoidance_parameters_, false));
+ data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
+ lanelet, planner_data_, avoidance_parameters_));
});
// calc drivable bound
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
index da3a224689149..02d8d68cf7a56 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
@@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj);
double calcShiftLength(
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
+bool isWithinLanes(
+ const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data);
+
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
@@ -157,9 +160,11 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, DebugData & debug);
-DrivableLanes generateExpandDrivableLanes(
+DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet);
+
+DrivableLanes generateExpandedDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
- const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver);
+ const std::shared_ptr<AvoidanceParameters> & parameters);
double calcDistanceToReturnDeadLine(
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index b8253d9aabc0e..75529c35c2af5 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -21,6 +21,7 @@
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
#include "behavior_path_planner_common/utils/path_utils.hpp"
+#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"
#include <lanelet2_extension/utility/message_conversion.hpp>
@@ -32,8 +33,6 @@
#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
-#include <boost/geometry/algorithms/centroid.hpp>
-
#include <algorithm>
#include <limits>
#include <memory>
@@ -223,24 +222,51 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
// expand drivable lanes
- const auto has_shift_point = !path_shifter_.getShiftLines().empty();
- const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted();
+ const auto is_within_current_lane =
+ utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_);
+ const auto red_signal_lane_itr = std::find_if(
+ data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
+ const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet);
+ return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_);
+ });
+ const auto not_use_adjacent_lane =
+ is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end();
+
std::for_each(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
- data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
- lanelet, planner_data_, parameters_, in_avoidance_maneuver));
+ if (!not_use_adjacent_lane) {
+ data.drivable_lanes.push_back(
+ utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
+ } else if (red_signal_lane_itr->id() != lanelet.id()) {
+ data.drivable_lanes.push_back(
+ utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
+ } else {
+ data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet));
+ }
});
// calc drivable bound
auto tmp_path = getPreviousModuleOutput().path;
const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
+ const auto use_left_side_hatched_road_marking_area = [&]() {
+ if (!not_use_adjacent_lane) {
+ return true;
+ }
+ return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr);
+ }();
+ const auto use_right_side_hatched_road_marking_area = [&]() {
+ if (!not_use_adjacent_lane) {
+ return true;
+ }
+ return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr);
+ }();
data.left_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
- parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
+ use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas,
parameters_->use_freespace_areas, true);
data.right_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
- parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
+ use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas,
parameters_->use_freespace_areas, false);
// reference path
@@ -939,7 +965,11 @@ BehaviorModuleOutput AvoidanceModule::plan()
{
DrivableAreaInfo current_drivable_area_info;
// generate drivable lanes
- current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
+ std::for_each(
+ data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
+ current_drivable_area_info.drivable_lanes.push_back(
+ utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
+ });
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index e8b7600684509..6a44350dd48c6 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -957,6 +957,44 @@ double calcShiftLength(
return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
}
+bool isWithinLanes(
+ const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
+{
+ const auto & rh = planner_data->route_handler;
+ const auto & ego_pose = planner_data->self_odometry->pose.pose;
+ const auto transform = tier4_autoware_utils::pose2transform(ego_pose);
+ const auto footprint = tier4_autoware_utils::transformVector(
+ planner_data->parameters.vehicle_info.createFootprint(), transform);
+
+ lanelet::ConstLanelet closest_lanelet{};
+ if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
+ return true;
+ }
+
+ lanelet::ConstLanelets concat_lanelets{};
+
+ // push previous lanelet
+ lanelet::ConstLanelets prev_lanelet;
+ if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
+ concat_lanelets.push_back(prev_lanelet.front());
+ }
+
+ // push nearest lanelet
+ {
+ concat_lanelets.push_back(closest_lanelet);
+ }
+
+ // push next lanelet
+ lanelet::ConstLanelet next_lanelet;
+ if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
+ concat_lanelets.push_back(next_lanelet);
+ }
+
+ const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets);
+
+ return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon());
+}
+
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
{
/**
@@ -2076,9 +2114,18 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
return std::make_pair(target_objects, other_objects);
}
-DrivableLanes generateExpandDrivableLanes(
+DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet)
+{
+ DrivableLanes current_drivable_lanes;
+ current_drivable_lanes.left_lane = lanelet;
+ current_drivable_lanes.right_lane = lanelet;
+
+ return current_drivable_lanes;
+}
+
+DrivableLanes generateExpandedDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
- const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver)
+ const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto & route_handler = planner_data->route_handler;
@@ -2092,11 +2139,6 @@ DrivableLanes generateExpandDrivableLanes(
// 1. get left/right side lanes
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
- const auto next_lanes = route_handler->getNextLanelets(target_lane);
- const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
- if (is_stop_signal && !in_avoidance_maneuver) {
- return;
- }
const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_left_lanelets.empty()) {
@@ -2107,11 +2149,6 @@ DrivableLanes generateExpandDrivableLanes(
}
};
const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
- const auto next_lanes = route_handler->getNextLanelets(target_lane);
- const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
- if (is_stop_signal && !in_avoidance_maneuver) {
- return;
- }
const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_right_lanelets.empty()) {