From 872afccc1d983714bfb44beedf5da34be14a044c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 11 Aug 2023 23:58:29 +0900 Subject: [PATCH] feat(dynamic_avoidance): object polygon based drivable area generation Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 5 +- .../dynamic_avoidance_module.cpp | 61 ++++++++++++++++++- .../dynamic_avoidance/manager.cpp | 4 ++ 3 files changed, 67 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 1f54786df83d7..dd0e06fffb255 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -74,6 +74,7 @@ struct DynamicAvoidanceParameters double max_oncoming_crossing_object_angle{0.0}; // drivable area generation + std::string polygon_generation_method{}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; double max_time_for_lat_shift{0.0}; @@ -335,7 +336,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const; + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; void printIgnoreReason(const std::string & obj_uuid, const std::string & reason) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 7f69397cb3cf5..b69e95875c5fb 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -274,7 +274,15 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { - const auto obstacle_poly = calcDynamicObstaclePolygon(object); + const auto obstacle_poly = [&]() { + if (parameters_->polygon_generation_method == "ego_path_base") { + return calcEgoPathBasedDynamicObstaclePolygon(object); + } + if (parameters_->polygon_generation_method == "object_path_base") { + return calcObjectPathBasedDynamicObstaclePolygon(object); + } + throw std::logic_error("The polygon_generation_method's string is invalid."); + }(); if (obstacle_poly) { obstacles_for_drivable_area.push_back( {object.pose, obstacle_poly.value(), object.is_collision_left}); @@ -836,7 +844,8 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( } // NOTE: object does not have const only to update min_bound_lat_offset. -std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( +std::optional +DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { @@ -896,4 +905,52 @@ std::optional DynamicAvoidanceModule::calcDynam boost::geometry::correct(obj_poly); return obj_poly; } + +std::optional +DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + const auto obj_path = *std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // calculate left and right bound + std::vector obj_left_bound_points; + std::vector obj_right_bound_points; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + const double lon_offset = [&]() { + if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle; + if (i == obj_path.path.size() - 1) + return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle; + return 0.0; + }(); + + const auto & obj_pose = obj_path.path.at(i); + obj_left_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + obj_pose, lon_offset, + object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) + .position); + obj_right_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + obj_pose, lon_offset, + -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) + .position); + } + + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + for (const auto & bound_point : obj_right_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); + for (const auto & bound_point : obj_left_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + + boost::geometry::correct(obj_poly); + return obj_poly; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index e378d43f6497c..3b2e2caf92e6f 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -81,6 +81,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.polygon_generation_method = + node->declare_parameter(ns + "polygon_generation_method"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); p.max_time_for_lat_shift = @@ -178,6 +180,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + updateParam( + parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); updateParam(