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 32c92b4f4b88b..4e9750cd7a690 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 @@ -30,16 +30,6 @@ namespace behavior_path_planner { -namespace -{ -lanelet::BasicLineString3d toLineString3d(const std::vector & bound) -{ - lanelet::BasicLineString3d ret{}; - std::for_each( - bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); - return ret; -} -} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) @@ -172,10 +162,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( // calc drivable bound const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); - data.right_bound = toLineString3d(utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); + data.left_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true); + data.right_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 5e162a17a5064..051a3cee9e196 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -541,9 +541,9 @@ struct AvoidancePlanningData std::vector drivable_lanes{}; - lanelet::BasicLineString3d right_bound{}; + std::vector right_bound{}; - lanelet::BasicLineString3d left_bound{}; + std::vector left_bound{}; bool safe{false}; 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 cbf68ada44abb..4d8da502f3c55 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 @@ -127,6 +127,10 @@ void filterTargetObjects( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 7c55c62bd8cc1..e097dd5341d64 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -469,7 +469,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.right_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); @@ -482,7 +482,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.left_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 5172bcbeb626c..685739fee432e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -234,14 +234,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // calc drivable bound auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( + data.left_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, - parameters_->use_freespace_areas, true)); - data.right_bound = toLineString3d(utils::calcBound( + 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, - parameters_->use_freespace_areas, false)); + parameters_->use_freespace_areas, false); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -294,6 +294,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; + using utils::avoidance::updateRoadShoulderDistance; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; @@ -315,6 +316,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Filter out the objects to determine the ones to be avoided. filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); + updateRoadShoulderDistance(data, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -929,10 +931,6 @@ BehaviorModuleOutput AvoidanceModule::plan() DrivableAreaInfo current_drivable_area_info; // generate drivable lanes current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -941,6 +939,21 @@ BehaviorModuleOutput AvoidanceModule::plan() parameters_->use_intersection_areas; // expand freespace areas current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; + // generate obstacle polygons + if (parameters_->enable_bound_clipping) { + ObjectDataArray clip_objects; + // If avoidance is executed by both behavior and motion, only non-avoidable object will be + // extracted from the drivable area. + std::for_each( + data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) clip_objects.push_back(object); + }); + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + } else { + current_drivable_area_info.obstacles.clear(); + } output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); @@ -1150,8 +1163,8 @@ bool AvoidanceModule::isValidShiftLine( const size_t end_idx = shift_lines.back().end_idx; const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound); - const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); for (size_t i = start_idx; i <= end_idx; ++i) { const auto p = getPoint(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 998ea7c40781c..4ea9db5f74bc3 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,6 +16,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" @@ -893,13 +894,8 @@ double getRoadShoulderDistance( std::vector intersects; for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + tier4_autoware_utils::intersect(p1_object, p2_object, bound.at(i - 1), bound.at(i)); if (!opt_intersect) { continue; @@ -1184,24 +1180,21 @@ std::vector generateObstaclePolygonsForDrivableArea( { std::vector obstacles_for_drivable_area; - if (objects.empty() || !parameters->enable_bound_clipping) { + if (objects.empty()) { return obstacles_for_drivable_area; } for (const auto & object : objects) { - // If avoidance is executed by both behavior and motion, only non-avoidable object will be - // extracted from the drivable area. - if (!parameters->disable_path_update) { - if (object.is_avoidable) { - continue; - } - } - // check if avoid marin is calculated if (!object.avoid_margin.has_value()) { continue; } + // check original polygon + if (object.envelope_poly.outer().empty()) { + continue; + } + const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -1615,6 +1608,38 @@ void compensateDetectionLost( } } +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + ObjectDataArray clip_objects; + std::for_each(data.other_objects.begin(), data.other_objects.end(), [&](const auto & object) { + if (object.reason != AvoidanceDebugFactor::MOVING_OBJECT) clip_objects.push_back(object); + }); + for (auto & o : clip_objects) { + const auto & vehicle_width = planner_data->parameters.vehicle_width; + const auto object_type = utils::getHighestProbLabel(o.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + + o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + } + const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( + clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); + + auto tmp_path = data.reference_path; + tmp_path.left_bound = data.left_bound; + tmp_path.right_bound = data.right_bound; + utils::extractObstaclesFromDrivableArea(tmp_path, extract_obstacles); + + data.left_bound = tmp_path.left_bound; + data.right_bound = tmp_path.right_bound; + + for (auto & o : data.target_objects) { + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + } +} + void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data,