From aeb7a88aa740f346c1c4c6c18b4f415cce7dad50 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Wed, 9 Feb 2022 17:35:38 +0900 Subject: [PATCH] feat(behavior_velocity): create triangle detection area polygon Signed-off-by: tanaka3 --- .../scene_module/occlusion_spot/geometry.hpp | 4 +- .../scene_module/occlusion_spot/geometry.cpp | 66 ++++++++--------- .../occlusion_spot/occlusion_spot_utils.cpp | 2 +- .../test/src/test_geometry.cpp | 72 ------------------- 4 files changed, 33 insertions(+), 111 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp index 1f7fde9acd103..2e3de8e04e766 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp @@ -49,9 +49,9 @@ struct Slice // using the given range and desired slice length and width void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width, const double resolution); + const double slice_length, const double resolution); //!< @brief build sidewalk slice from path -void buildSidewalkSlices( +void buildDetectionAreaPolygon( std::vector & slice, const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, const double lateral_offset, const double min_size, const double lateral_max_dist); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp index e88274458e229..ad3eeff40f3d0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp @@ -57,10 +57,8 @@ void createOffsetLineString( void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width, const double resolution) + const double slice_length, const double resolution) { - const int num_lateral_slice = - static_cast(std::abs(range.max_distance - range.min_distance) / slice_width); /** * @brief bounds * +---------- outer bounds @@ -72,60 +70,56 @@ void buildSlices( if (inner_bounds.size() < 2) return; createOffsetLineString(inner_bounds, range.max_distance, outer_bounds); const double ratio_dist_start = std::abs(range.min_distance / range.max_distance); - const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance)); + double next_ratio_dist = ratio_dist_start; lanelet::BasicPolygon2d poly; const int num_step = static_cast(slice_length / resolution); //! max index is the last index of path point const int max_index = static_cast(inner_bounds.size() - 1); - for (int s = 0; s < max_index; s += num_step) { + for (int s = 0; s < max_index - 1; s += num_step) { const double length = s * slice_length; const double next_length = (s + num_step) * resolution; - for (int d = 0; d < num_lateral_slice; d++) { - const double ratio_dist = ratio_dist_start + d * ratio_dist_increment; - const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment; - Slice slice; - BasicLineString2d inner_polygons; - BasicLineString2d outer_polygons; - // build interpolated polygon for lateral - for (int i = 0; i <= num_step; i++) { - if (s + i >= max_index) continue; - inner_polygons.emplace_back( - lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist)); - outer_polygons.emplace_back( - lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist)); - } - // Build polygon - inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); - slice.polygon = lanelet::BasicPolygon2d(inner_polygons); - // add range info - slice.range.min_length = length; - slice.range.max_length = next_length; - slice.range.min_distance = ratio_dist * range.max_distance; - slice.range.max_distance = next_ratio_dist * range.max_distance; - slices.emplace_back(slice); + Slice slice; + BasicLineString2d inner_polygons; + BasicLineString2d outer_polygons; + // build interpolated polygon for lateral + for (int i = 0; i <= num_step; i++) { + if (s + i >= max_index) continue; + next_ratio_dist = static_cast(s + i) / static_cast(max_index); + if (ratio_dist_start > next_ratio_dist) continue; + inner_polygons.emplace_back( + lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist_start)); + outer_polygons.emplace_back( + lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist)); } + if (inner_polygons.empty()) continue; + // Build polygon + inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); + slice.polygon = lanelet::BasicPolygon2d(inner_polygons); + // add range info + slice.range.min_length = length; + slice.range.max_length = next_length; + slice.range.min_distance = ratio_dist_start * range.max_distance; + slice.range.max_distance = next_ratio_dist * range.max_distance; + slices.emplace_back(slice); } } -void buildSidewalkSlices( +void buildDetectionAreaPolygon( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, const double lateral_offset, const double slice_size, const double lateral_max_dist) { std::vector left_slices; std::vector right_slices; - const double longitudinal_max_dist = lg::length2d(path_lanelet); SliceRange left_slice_range = { - longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist}; + longitudinal_offset, 0.0, lateral_offset, lateral_offset + lateral_max_dist}; // in most case lateral distance is much more effective for velocity planning const double slice_length = 4.0 * slice_size; - const double slice_width = slice_size; const double resolution = 1.0; - buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution); + buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, resolution); SliceRange right_slice_range = { - longitudinal_offset, longitudinal_max_dist, -lateral_offset, - -lateral_offset - lateral_max_dist}; - buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution); + longitudinal_offset, 0.0, -lateral_offset, -lateral_offset - lateral_max_dist}; + buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, resolution); // Properly order lanelets from closest to furthest slices = left_slices; slices.insert(slices.end(), right_slices.begin(), right_slices.end()); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 4922d994bc8e0..506472ab88a43 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -425,7 +425,7 @@ void generateSidewalkPossibleCollisions( return; } std::vector sidewalk_slices; - geometry::buildSidewalkSlices( + geometry::buildDetectionAreaPolygon( sidewalk_slices, path_lanelet, 0.0, param.half_vehicle_width, param.sidewalk.slice_size, param.sidewalk.focus_range); double length_lower_bound = std::numeric_limits::max(); diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/planning/behavior_velocity_planner/test/src/test_geometry.cpp index cc1e631e91c58..0d7abf4bf7271 100644 --- a/planning/behavior_velocity_planner/test/src/test_geometry.cpp +++ b/planning/behavior_velocity_planner/test/src/test_geometry.cpp @@ -19,75 +19,3 @@ #include #include - -TEST(buildSlices, one_full_slice) -{ - using behavior_velocity_planner::geometry::buildSlices; - using behavior_velocity_planner::geometry::Slice; - using behavior_velocity_planner::geometry::SliceRange; - - /* Simple vertical lanelet - 3x3 slice on the left - 0 1 2 3 4 5 6 - 0 | |------ - 1 | | | - 2 | |SLICE| - 3 | |-----| - 4 | | - 5 | | - */ - const int nb_points = 6; - lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); - SliceRange range; - range.min_distance = 0.0; - range.max_distance = -3.0; - const double slice_length = 3.0; - const double slice_width = 1.5; - std::vector slices; - buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5); - ASSERT_EQ(slices.size(), static_cast(4)); - EXPECT_EQ(slices[0].range.min_length, 0.0); - EXPECT_EQ(slices[0].range.min_distance, 0.0); - EXPECT_EQ(slices[0].range.max_length, 3.0); - EXPECT_EQ(slices[0].range.max_distance, -1.5); -} - -TEST(buildSlices, 3x3square_slice) -{ - using behavior_velocity_planner::geometry::buildSlices; - using behavior_velocity_planner::geometry::Slice; - using behavior_velocity_planner::geometry::SliceRange; - - /* Simple vertical lanelet - 3x3 slice on the left - 0 1 2 3 4 5 6 - 0 | |------ - 1 | | | - 2 | |SLICE| - 3 | |-----| - */ - const int nb_points = 4; - const double len = 3.0; - lanelet::ConstLanelet lanelet = test::createLanelet({0.0, 0.0}, {0.0, len}, nb_points); - SliceRange range; - range.min_distance = -1.0; - range.max_distance = -3.0; - const double slice_length = 1.0; - const double slice_width = 1.0; - const int num_right_slice = - (len / slice_length) * (std::abs(range.max_distance - range.min_distance) / slice_width); - // - { - std::vector slices; - buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); - ASSERT_EQ(slices.size(), static_cast(num_right_slice)); - } - // change to the left side (positive distance) - range.min_distance = 1.0; - range.max_distance = 3.0; - { - std::vector slices; - buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); - ASSERT_EQ(slices.size(), static_cast(num_right_slice)); - } -}