diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 52538dc9b1120..ced0bad376976 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -16,7 +16,7 @@ common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp -common/grid_map_utils/** maxime.clement@tier4.jp +common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/common/.pages b/common/.pages index 7d17018e2762b..1084ec0356336 100644 --- a/common/.pages +++ b/common/.pages @@ -7,12 +7,12 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons + - 'autoware_grid_map_utils': common/autoware_grid_map_utils - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils - 'Geography Utils': common/geography_utils - 'Global Parameter Loader': common/global_parameter_loader/Readme - 'Glog Component': common/glog_component - - 'grid_map_utils': common/grid_map_utils - 'interpolation': common/interpolation - 'Kalman Filter': common/kalman_filter - 'Motion Utils': common/motion_utils diff --git a/common/grid_map_utils/CMakeLists.txt b/common/autoware_grid_map_utils/CMakeLists.txt similarity index 96% rename from common/grid_map_utils/CMakeLists.txt rename to common/autoware_grid_map_utils/CMakeLists.txt index 0fbb34e94e688..677b59a93a514 100644 --- a/common/grid_map_utils/CMakeLists.txt +++ b/common/autoware_grid_map_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(grid_map_utils) +project(autoware_grid_map_utils) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/grid_map_utils/README.md b/common/autoware_grid_map_utils/README.md similarity index 76% rename from common/grid_map_utils/README.md rename to common/autoware_grid_map_utils/README.md index f3c6ae5d879be..34adfe230cf78 100644 --- a/common/grid_map_utils/README.md +++ b/common/autoware_grid_map_utils/README.md @@ -19,18 +19,18 @@ More details on the scan line algorithm can be found in the References. ## API -The `grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html). +The `autoware::grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html). ## Assumptions -The behavior of the `grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center. +The behavior of the `autoware::grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center. In such a case, whether the crossed cell is considered inside or outside of the polygon can vary due to floating precision error. ## Performances -Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`. +Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `autoware::grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`. -The following figure shows a comparison of the runtime between the implementation of this package (`grid_map_utils`) and the original implementation (`grid_map`). +The following figure shows a comparison of the runtime between the implementation of this package (`autoware_grid_map_utils`) and the original implementation (`grid_map`). The time measured includes the construction of the iterator and the iteration over all indexes and is shown using a logarithmic scale. Results were obtained varying the side size of a square grid map with `100 <= n <= 1000` (size=`n` means a grid of `n x n` cells), random polygons with a number of vertices `3 <= m <= 100` and with each parameter `(n,m)` repeated 10 times. diff --git a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp b/common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp similarity index 95% rename from common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp rename to common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp index 4f2e149a50f72..9bb82e7be50ea 100644 --- a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp +++ b/common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ -#define GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#ifndef AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#define AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ #include "grid_map_core/TypeDefs.hpp" @@ -24,7 +24,7 @@ #include #include -namespace grid_map_utils +namespace autoware::grid_map_utils { /// @brief Representation of a polygon edge made of 2 vertices @@ -124,6 +124,6 @@ class PolygonIterator int current_col_; int current_to_col_; }; -} // namespace grid_map_utils +} // namespace autoware::grid_map_utils -#endif // GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#endif // AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ diff --git a/common/grid_map_utils/media/runtime_comparison.png b/common/autoware_grid_map_utils/media/runtime_comparison.png similarity index 100% rename from common/grid_map_utils/media/runtime_comparison.png rename to common/autoware_grid_map_utils/media/runtime_comparison.png diff --git a/common/grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml similarity index 95% rename from common/grid_map_utils/package.xml rename to common/autoware_grid_map_utils/package.xml index 6e63271d56750..c15470168f318 100644 --- a/common/grid_map_utils/package.xml +++ b/common/autoware_grid_map_utils/package.xml @@ -1,7 +1,7 @@ - grid_map_utils + autoware_grid_map_utils 0.0.0 Utilities for the grid_map library Maxime CLEMENT diff --git a/common/grid_map_utils/src/polygon_iterator.cpp b/common/autoware_grid_map_utils/src/polygon_iterator.cpp similarity index 98% rename from common/grid_map_utils/src/polygon_iterator.cpp rename to common/autoware_grid_map_utils/src/polygon_iterator.cpp index d2a738a971263..1360ad6e6c255 100644 --- a/common/grid_map_utils/src/polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/src/polygon_iterator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "grid_map_utils/polygon_iterator.hpp" +#include "autoware_grid_map_utils/polygon_iterator.hpp" #include "grid_map_core/GridMap.hpp" #include "grid_map_core/Polygon.hpp" @@ -22,7 +22,7 @@ #include #include -namespace grid_map_utils +namespace autoware::grid_map_utils { std::vector PolygonIterator::calculateSortedEdges(const grid_map::Polygon & polygon) @@ -214,4 +214,4 @@ PolygonIterator & PolygonIterator::operator++() { return current_line_ >= intersections_per_line_.size(); } -} // namespace grid_map_utils +} // namespace autoware::grid_map_utils diff --git a/common/grid_map_utils/test/benchmark.cpp b/common/autoware_grid_map_utils/test/benchmark.cpp similarity index 96% rename from common/grid_map_utils/test/benchmark.cpp rename to common/autoware_grid_map_utils/test/benchmark.cpp index c8e352ce5d185..77da9ae589b73 100644 --- a/common/grid_map_utils/test/benchmark.cpp +++ b/common/autoware_grid_map_utils/test/benchmark.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_grid_map_utils/polygon_iterator.hpp" #include "grid_map_core/TypeDefs.hpp" #include "grid_map_cv/GridMapCvConverter.hpp" #include "grid_map_cv/GridMapCvProcessing.hpp" -#include "grid_map_utils/polygon_iterator.hpp" #include #include @@ -116,7 +116,7 @@ int main(int argc, char * argv[]) polygon.addVertex(base_polygon.getVertex(idx) + offset); } stopwatch.tic("gmu_ctor"); - grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); grid_map_utils_constructor_duration += stopwatch.toc("gmu_ctor"); stopwatch.tic("gm_ctor"); grid_map::PolygonIterator grid_map_iterator(map, polygon); @@ -143,8 +143,8 @@ int main(int argc, char * argv[]) if (diff || visualize) { // Prepare images of the cells selected by the two PolygonIterators auto gridmap = map; - for (grid_map_utils::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); - ++iterator) + for (autoware::grid_map_utils::PolygonIterator iterator(map, polygon); + !iterator.isPastEnd(); ++iterator) map.at("layer", *iterator) = 100; for (grid_map::PolygonIterator iterator(gridmap, polygon); !iterator.isPastEnd(); ++iterator) diff --git a/common/grid_map_utils/test/test_polygon_iterator.cpp b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp similarity index 92% rename from common/grid_map_utils/test/test_polygon_iterator.cpp rename to common/autoware_grid_map_utils/test/test_polygon_iterator.cpp index f39d080d7cad5..8d9cff560b5b7 100644 --- a/common/grid_map_utils/test/test_polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "grid_map_utils/polygon_iterator.hpp" +#include "autoware_grid_map_utils/polygon_iterator.hpp" #include #include @@ -45,7 +45,7 @@ TEST(PolygonIterator, FullCover) polygon.addVertex(Position(100.0, -100.0)); polygon.addVertex(Position(-100.0, -100.0)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -83,7 +83,7 @@ TEST(PolygonIterator, Outside) polygon.addVertex(Position(101.0, 99.0)); polygon.addVertex(Position(99.0, 99.0)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_TRUE(iterator.isPastEnd()); } @@ -100,7 +100,7 @@ TEST(PolygonIterator, Square) polygon.addVertex(Position(1.0, -1.5)); polygon.addVertex(Position(-1.0, -1.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(3, (*iterator)(0)); @@ -146,7 +146,7 @@ TEST(PolygonIterator, TopLeftTriangle) polygon.addVertex(Position(40.1, 20.4)); polygon.addVertex(Position(-40.1, -20.6)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -170,7 +170,7 @@ TEST(PolygonIterator, MoveMap) polygon.addVertex(Position(0.9, 1.6)); polygon.addVertex(Position(0.9, -1.6)); polygon.addVertex(Position(6.1, -1.6)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(6, (*iterator)(0)); @@ -213,7 +213,7 @@ TEST(PolygonIterator, Difference) polygon.addVertex(Position(2.5, 2.5)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); grid_map::PolygonIterator gm_iterator(map, polygon); bool diff = false; while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { @@ -231,7 +231,7 @@ TEST(PolygonIterator, Difference) polygon.addVertex(Position(2.5, 2.1)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.9)); - iterator = grid_map_utils::PolygonIterator(map, polygon); + iterator = autoware::grid_map_utils::PolygonIterator(map, polygon); gm_iterator = grid_map::PolygonIterator(map, polygon); diff = false; while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { @@ -256,7 +256,7 @@ TEST(PolygonIterator, SelfCrossingPolygon) polygon.addVertex(Position(2.5, -2.9)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); grid_map::PolygonIterator gm_iterator(map, polygon); const std::vector expected_indexes = { diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 26412539d8571..e287b83aaa504 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -12,11 +12,11 @@ ament_cmake_auto autoware_cmake + autoware_grid_map_utils autoware_map_msgs grid_map_cv grid_map_pcl grid_map_ros - grid_map_utils lanelet2_extension libpcl-all-dev pcl_conversions diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index afb85fc9318de..7b7c529e17dcc 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -14,12 +14,12 @@ #include "elevation_map_loader/elevation_map_loader_node.hpp" +#include #include #include #include #include #include -#include #include #include #include @@ -382,8 +382,8 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) for (const auto & p : lane_polygon) { polygon.addVertex(grid_map::Position(p[0], p[1])); } - for (grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); + !iterator.isPastEnd(); ++iterator) { if (!elevation_map_.isValid(*iterator, layer_name_)) { elevation_map_.at("inpaint_mask", *iterator) = 1.0; } diff --git a/planning/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/autoware_behavior_velocity_crosswalk_module/package.xml index c3cf1339bbe6a..5e2a1fdf50544 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/autoware_behavior_velocity_crosswalk_module/package.xml @@ -22,6 +22,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_grid_map_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -30,7 +31,6 @@ geometry_msgs grid_map_core grid_map_ros - grid_map_utils interpolation lanelet2_extension libboost-dev diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 9b66802c1e5ed..04559a7888036 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -16,8 +16,8 @@ #include "autoware_behavior_velocity_crosswalk_module/util.hpp" +#include #include -#include #include #include @@ -129,7 +129,8 @@ void clear_occlusions_behind_objects( polygon_to_clear.addVertex( interpolate_point({grid_map_position, edge_points.back()}, 10.0 * range)); polygon_to_clear.addVertex(edge_points.back()); - for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it) + for (autoware::grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); + !it.isPastEnd(); ++it) grid_map.at("layer", *it) = 0; } } @@ -156,7 +157,7 @@ bool is_crosswalk_occluded( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { grid_map::Polygon poly; for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y())); - for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) + for (autoware::grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; } return false; diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml index e229a69032c27..eaffade354061 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -17,13 +17,13 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_grid_map_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler autoware_vehicle_info_utils geometry_msgs grid_map_ros - grid_map_utils interpolation lanelet2_extension libboost-dev diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 6ab88baf77b20..0557922da53fe 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -52,8 +52,8 @@ void findOcclusionSpots( for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::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; @@ -77,8 +77,8 @@ bool isCollisionFree( for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::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) { return false; diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 8dc59d07173dc..f5ae35861d35c 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -17,10 +17,10 @@ #include #include +#include #include #include #include -#include #include #include #include diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index a21e0333082a9..fd36d7941a4d7 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -102,8 +102,8 @@ TEST(compareTime, polygon_vs_line_iterator) for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::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) { std::cout << "occupied" << std::endl; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/README.md index f299df5564fe2..ef9fd770c6289 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/README.md @@ -111,7 +111,7 @@ Obstacles from the lanelet map are not impacted by the masks. #### Occupancy Grid -Masking is performed by iterating through the cells inside each polygon mask using the [`grid_map_utils::PolygonIterator`](https://github.com/autowarefoundation/autoware.universe/tree/main/common/grid_map_utils) function. +Masking is performed by iterating through the cells inside each polygon mask using the [`autoware::grid_map_utils::PolygonIterator`](https://github.com/autowarefoundation/autoware.universe/tree/main/common/autoware_grid_map_utils) function. A threshold is then applied to only keep cells with an occupancy value above parameter `obstacles.occupancy_grid_threshold`. Finally, the image is converted to an image and obstacle linestrings are extracted using the opencv function [`findContour`](https://docs.opencv.org/3.4/d3/dc0/group__imgproc__shape.html#ga17ed9f5d79ae97bd4c7cf18403e1689a). diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index ae2dad9eb365b..ff30193cafee1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -11,6 +11,7 @@ autoware_cmake eigen3_cmake_module + autoware_grid_map_utils autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs @@ -19,7 +20,6 @@ eigen grid_map_msgs grid_map_ros - grid_map_utils lanelet2_core lanelet2_extension libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.cpp index 6ced7a011ecbe..424734801eb8f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.cpp @@ -14,11 +14,11 @@ #include "occupancy_grid_utils.hpp" +#include #include #include #include #include -#include #include #include @@ -38,13 +38,14 @@ void maskPolygons(grid_map::GridMap & grid_map, const ObstacleMasks & obstacle_m const auto layer_copy = grid_map["layer"]; layer.setConstant(0.0); grid_map::Position position; - for (grid_map_utils::PolygonIterator iterator(grid_map, convert(obstacle_masks.positive_mask)); + for (autoware::grid_map_utils::PolygonIterator iterator( + grid_map, convert(obstacle_masks.positive_mask)); !iterator.isPastEnd(); ++iterator) layer((*iterator)(0), (*iterator)(1)) = layer_copy((*iterator)(0), (*iterator)(1)); } for (const auto & negative_mask : obstacle_masks.negative_masks) - for (grid_map_utils::PolygonIterator iterator(grid_map, convert(negative_mask)); + for (autoware::grid_map_utils::PolygonIterator iterator(grid_map, convert(negative_mask)); !iterator.isPastEnd(); ++iterator) layer((*iterator)(0), (*iterator)(1)) = 0.0; }