diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index c2c0ebf09c8a9..e6b412baa6834 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -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 @@ -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 diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index 498c873b30d5f..a5685449b82b5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -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; diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp index 7fcd790190a49..2f86c23491821 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -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 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 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 unknown_cells; - std::unordered_set 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 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); - } - } }