Skip to content

Commit

Permalink
feat(behavior_velocity): create triangle detection area polygon
Browse files Browse the repository at this point in the history 
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
  • Loading branch information
@taikitanaka3
taikitanaka3 committed Feb 9, 2022
1 parent 028a6eb commit aeb7a88
Show file tree
Hide file tree
Showing 4 changed files with 33 additions and 111 deletions.
4 changes: 2 additions & 2 deletions planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,9 @@ struct Slice
// using the given range and desired slice length and width
void buildSlices(
std::vector<Slice> & 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<geometry::Slice> & slice, const lanelet::ConstLanelet & path_lanelet,
const double longitudinal_offset, const double lateral_offset, const double min_size,
const double lateral_max_dist);
Expand Down
66 changes: 30 additions & 36 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,8 @@ void createOffsetLineString(

void buildSlices(
std::vector<Slice> & 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<int>(std::abs(range.max_distance - range.min_distance) / slice_width);
/**
* @brief bounds
* +---------- outer bounds
Expand All @@ -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<int>(slice_length / resolution);
//! max index is the last index of path point
const int max_index = static_cast<int>(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<double>(s + i) / static_cast<double>(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<Slice> & 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<Slice> left_slices;
std::vector<Slice> 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());
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -425,7 +425,7 @@ void generateSidewalkPossibleCollisions(
return;
}
std::vector<geometry::Slice> 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<double>::max();
Expand Down
72 changes: 0 additions & 72 deletions planning/behavior_velocity_planner/test/src/test_geometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,75 +19,3 @@
#include <gtest/gtest.h>

#include <vector>

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<Slice> slices;
buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5);
ASSERT_EQ(slices.size(), static_cast<size_t>(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<Slice> slices;
buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0);
ASSERT_EQ(slices.size(), static_cast<size_t>(num_right_slice));
}
// change to the left side (positive distance)
range.min_distance = 1.0;
range.max_distance = 3.0;
{
std::vector<Slice> slices;
buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0);
ASSERT_EQ(slices.size(), static_cast<size_t>(num_right_slice));
}
}

0 comments on commit aeb7a88

Please sign in to comment.