feat(behavior_velocity_planner): use grid_map_utils::PolygonIterator (#…

…1018) Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Jun 1, 2022 · 2bbb23a · 2bbb23a
1 parent b0ef888
commit 2bbb23a
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 5 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp
@@ -17,8 +17,8 @@
 
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/LineIterator.hpp>
-#include <grid_map_core/iterators/PolygonIterator.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
+#include <grid_map_utils/polygon_iterator.hpp>
 #include <opencv2/opencv.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/math/normalization.hpp>

diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
@@ -19,6 +19,7 @@
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>grid_map_ros</depend>
+  <depend>grid_map_utils</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>

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
@@ -68,7 +68,7 @@ void addObjectsToGridMap(const std::vector<PredictedObject> & objs, grid_map::Gr
       for (const auto & point : foot_print_polygon.outer()) {
         grid_polygon.addVertex({point.x(), point.y()});
       }
-      for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
+      for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
            ++iterator) {
         const grid_map::Index & index = *iterator;
         if (!grid.isValid(index)) continue;
@@ -89,7 +89,8 @@ void findOcclusionSpots(
   for (const auto & point : polygon.outer()) {
     grid_polygon.addVertex({point.x(), point.y()});
   }
-  for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); ++iterator) {
+  for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
+       ++iterator) {
     const grid_map::Index & index = *iterator;
     if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::UNKNOWN) {
       grid_map::Position occlusion_spot_position;
@@ -115,7 +116,7 @@ bool isCollisionFree(
       for (const auto & point : polygon.outer()) {
         grid_polygon.addVertex({point.x(), point.y()});
       }
-      for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
+      for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
            ++iterator) {
         const grid_map::Index & index = *iterator;
         if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) {

diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp
@@ -99,7 +99,7 @@ TEST(compareTime, polygon_vs_line_iterator)
         for (const auto & point : polygon.outer()) {
           grid_polygon.addVertex({point.x(), point.y()});
         }
-        for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
+        for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
              ++iterator) {
           const grid_map::Index & index = *iterator;
           if (grid_data(index.x(), index.y()) == OCCUPIED) {