Skip to content

Commit

Permalink
feat(behavior_velocity): better occlusion spot detection
Browse files Browse the repository at this point in the history 
Signed-off-by: tanaka3 <[email protected]>
  • Loading branch information
@taikitanaka3
taikitanaka3 committed Feb 14, 2022
1 parent 3761309 commit 6c501de
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 160 deletions.
6 changes: 3 additions & 3 deletions planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/**:
ros__parameters:
occlusion_spot:
debug: false # [-] whether to publish debug markers. Note Default should be false for performance
debug: true # [-] whether to publish debug markers. Note Default should be false for performance
pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity
threshold:
detection_area_length: 100.0 # [m] the length of path to consider perception range
Expand All @@ -15,8 +15,8 @@
safe_margin: 1.0 # [m] maximum safety distance for any error
detection_area:
min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
slice_length: 5.0 # [m] size of slices in both length and distance relative to the ego path.
max_lateral_distance: 5.0 # [m] buffer around the ego path used to build the detection area.
slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path.
max_lateral_distance: 4.0 # [m] buffer around the ego path used to build the detection area.
grid:
free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid
occupied_min: 60 # [-] minimum value of an occupied cell in the occupancy grid
18 changes: 13 additions & 5 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,24 +41,32 @@ bool isOcclusionSpotSquare(
* . .
* (min_x,max_y)...(max_x,max_y)
*/
const int offset = side_size - 1;
min_x = cell.x();
const int offset = (side_size != 1) ? (side_size - 1) : side_size;
min_x = cell.x() - offset;
max_x = cell.x() + offset;
min_y = cell.y() - offset;
max_y = cell.y();
max_y = cell.y() + offset;
// Ensure we stay inside the grid
min_x = std::max(0, min_x);
max_x = std::min(grid_size.x() - 1, max_x);
min_y = std::max(0, min_y);
max_y = std::min(grid_size.y() - 1, max_y);
int unknown_count = 0;
for (int x = min_x; x <= max_x; ++x) {
for (int y = min_y; y <= max_y; ++y) {
// if the value is not unknown value return false
if (grid_data(x, y) != grid_utils::occlusion_cost_value::UNKNOWN) {
return false;
if (grid_data(x, y) == grid_utils::occlusion_cost_value::UNKNOWN) {
unknown_count++;
}
}
}
/**
* @brief case pass o: unknown x: freespace or occupied
* oxx oxo oox xxx oxo oxo
* oox oxx oox ooo oox oxo ... etc
* ooo ooo oox ooo xoo oxo
*/
if (unknown_count < (side_size + 1) * side_size) return false;
occlusion_spot.side_size = side_size;
occlusion_spot.index = cell;
return true;
Expand Down
198 changes: 46 additions & 152 deletions planning/behavior_velocity_planner/test/src/test_grid_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,172 +37,66 @@ struct indexEq

using test::grid_param;

TEST(isOcclusionSpotSquare, one_cell_occlusion_spot)
TEST(isOcclusionSpotSquare, occlusion_spot_cell)
{
using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare;
using behavior_velocity_planner::grid_utils::OcclusionSpotSquare;
using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN;
// Prepare an occupancy grid with a single occlusion_spot
/*
0 1 2 3
0 ? ? ?
1 ? ?
2 ? ? ?
3 ? ?
*/
grid_map::GridMap grid = test::generateGrid(4, 4, 1.0);
std::unordered_set<grid_map::Index, indexHash, indexEq> unknown_cells = {
{0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}
const int min_occlusion_spot_size = 2;

for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found = isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, 1, grid.getSize());
if (unknown_cells.count({i, j})) {
ASSERT_TRUE(found);
} else {
ASSERT_FALSE(found);
}
// case simple
{
/*
0 1 2 3
0 ?
1 x x
2 ? ? ? ?
3
*/
grid_map::GridMap grid = test::generateGrid(4, 4, 1.0);
std::vector<grid_map::Index> unknown_cells = {{0, 2}, {1, 1}, {1, 2}, {2, 0},
{2, 1}, {2, 2}, {3, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}
}
}
TEST(isOcclusionSpotSquare, many_cells_occlusion_spot)
{
using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare;
using behavior_velocity_planner::grid_utils::OcclusionSpotSquare;
using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN;
const double resolution = 1.0;
const double size = 2 * resolution;
/*
0 1 2 3
0 ? ? ?
1 ? ?
2 ? ? ?
3 ? ?
*/
grid_map::GridMap grid = test::generateGrid(4, 4, resolution);
std::unordered_set<grid_map::Index, indexHash, indexEq> unknown_cells;
std::unordered_set<grid_map::Index, indexHash, indexEq> occlusion_spot_cells;
unknown_cells = {{0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}};
occlusion_spot_cells = {{2, 0}, {3, 0}, {1, 2}, {3, 3}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}

for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found =
isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize());
if (occlusion_spot_cells.count({i, j})) {
ASSERT_TRUE(found);
} else {
ASSERT_FALSE(found);
// occlusion spot (2,2)
for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found = isOcclusionSpotSquare(
occlusion_spot, grid["layer"], {i, j}, min_occlusion_spot_size, grid.getSize());
if ((i == 1 && j == 1) || (i == 2 && j == 1)) {
EXPECT_TRUE(found);
} else {
EXPECT_FALSE(found);
}
}
}
}
/*
0 1 2 3
0 ?
1 ? ?
2 ? ? ?
3
*/
grid = test::generateGrid(4, 4, 1.0);
unknown_cells = {{0, 2}, {1, 1}, {2, 0}, {2, 1}, {2, 2}, {3, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}

// No occlusion_spots
for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found =
isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize());
ASSERT_FALSE(found);
}
}
/*
0 1 2 3
0
1 ? ?
2 ? ?
3
*/
grid = test::generateGrid(4, 4, 1.0);
unknown_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}

// Only one occlusion_spot square
for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found =
isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize());
if (i == 1 && j == 2) {
ASSERT_TRUE(found);
} else {
ASSERT_FALSE(found);
}
// case noisy
{
std::cout << "TooNoisyCase->No OcclusionSpot 2by2" << std::endl
<< " 0|1|2|3|4| " << std::endl
<< " 0 |?| | | | " << std::endl
<< " 1 |?| | | | " << std::endl
<< " 2 ?|?|?|?| | " << std::endl;
grid_map::GridMap grid = test::generateGrid(5, 3, 1.0);
std::vector<grid_map::Index> unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {2, 2}, {3, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}
}

/*
0 1 2 3
0 ?
1 ? ?
2 ? ? ? ?
3 ?
*/
grid = test::generateGrid(4, 4, 1.0);
unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {2, 2}, {3, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}
occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}};

for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found =
isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize());
if (i == 1 && j == 2) {
ASSERT_TRUE(found);
} else {
for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found = isOcclusionSpotSquare(
occlusion_spot, grid["layer"], {i, j}, min_occlusion_spot_size, grid.getSize());
if (found) {
std::cout << "i: " << i << " j: " << j << " change algorithm or update test" << std::endl;
}
ASSERT_FALSE(found);
}
}
}

std::cout << "TooNoisyCase->No OcclusionSpot 2by2" << std::endl
<< " 0|1|2|3|4| " << std::endl
<< " 0 |?| | | | " << std::endl
<< " 1 |?|?|?| | " << std::endl
<< " 2 ?|?| |?| | " << std::endl
<< " 3 |?| | | | " << std::endl
<< " 4 |?| | | | " << std::endl;
grid = test::generateGrid(5, 5, 1.0);
unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {3, 1}, {3, 2}};
for (grid_map::Index index : unknown_cells) {
grid.at("layer", index) = UNKNOWN;
}
occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}};

for (int i = 0; i < grid.getLength().x(); ++i) {
for (int j = 0; j < grid.getLength().y(); ++j) {
OcclusionSpotSquare occlusion_spot;
bool found =
isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize());
if (found) {
std::cout << "i: " << i << " j: " << j << " change algorithm or update test" << std::endl;
}
ASSERT_FALSE(found);
}
}
}

0 comments on commit 6c501de

Please sign in to comment.