From 89acb611f6a69ba08197a988abada24b60559e8a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 29 Oct 2024 15:00:18 +0900 Subject: [PATCH] perf(costmap_generator): manual blurring and fill polygons without OpenCV (#9160) Signed-off-by: Maxime CLEMENT --- .../costmap_generator.hpp | 25 +++---- .../object_map_utils.hpp | 34 ++++----- .../objects_to_costmap.hpp | 8 +- .../costmap_generator_node.cpp | 60 ++++++--------- .../object_map_utils.cpp | 75 ++++++------------- .../objects_to_costmap.cpp | 65 ++++++++++++---- .../autoware_costmap_generator/package.xml | 1 + 7 files changed, 126 insertions(+), 142 deletions(-) diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index 434440359f282..a26589775ff3e 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -70,7 +70,6 @@ #include #include -#include #include namespace autoware::costmap_generator @@ -106,9 +105,9 @@ class CostmapGenerator : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::vector> primitives_points_; + std::vector primitives_polygons_; - PointsToCostmap points2costmap_; + PointsToCostmap points2costmap_{}; ObjectsToCostmap objects2costmap_; tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; @@ -150,19 +149,19 @@ class CostmapGenerator : public rclcpp::Node /// \param[in] gridmap with calculated cost void publishCostmap(const grid_map::GridMap & costmap); - /// \brief set area_points from lanelet polygons - /// \param [in] input lanelet_map - /// \param [out] calculated area_points of lanelet polygons - void loadRoadAreasFromLaneletMap( + /// \brief fill a vector with road area polygons + /// \param [in] lanelet_map input lanelet map + /// \param [out] area_polygons polygon vector to fill + static void loadRoadAreasFromLaneletMap( const lanelet::LaneletMapPtr lanelet_map, - std::vector> * area_points); + std::vector & area_polygons); - /// \brief set area_points from parking-area polygons - /// \param [in] input lanelet_map - /// \param [out] calculated area_points of lanelet polygons - void loadParkingAreasFromLaneletMap( + /// \brief fill a vector with parking-area polygons + /// \param [in] lanelet_map input lanelet map + /// \param [out] area_polygons polygon vector to fill + static void loadParkingAreasFromLaneletMap( const lanelet::LaneletMapPtr lanelet_map, - std::vector> * area_points); + std::vector & area_polygons); /// \brief calculate cost from pointcloud data /// \param[in] in_points: subscribed pointcloud data diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp index 55a032fc8bb5a..737af55aa0b92 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,46 +33,38 @@ #ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ #define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ -#include #include #include +#include #include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else #include -#endif + #include #include #include #include -namespace object_map +namespace autoware::costmap_generator::object_map { /*! - * Projects the in_area_points forming the road, stores the result in out_grid_map. + * Fill the grid_map in the areas defined by the given polygons * @param[out] out_grid_map GridMap object to add the road grid - * @param[in] in_points Array of points containing the selected primitives + * @param[in] in_polygons polygons to fill in the grid map * @param[in] in_grid_layer_name Name to assign to the layer * @param[in] in_layer_background_value Empty state value - * @param[in] in_fill_color Value to fill on selected primitives - * @param[in] in_layer_min_value Minimum value in the layer - * @param[in] in_layer_max_value Maximum value in the layer + * @param[in] in_fill_value Value to fill inside the given polygons * @param[in] in_tf_target_frame Target frame to transform the points * @param[in] in_tf_source_frame Source frame, where the points are located * @param[in] in_tf_listener Valid listener to obtain the transformation */ -void FillPolygonAreas( - grid_map::GridMap & out_grid_map, - const std::vector> & in_points, - const std::string & in_grid_layer_name, const int in_layer_background_value, - const int in_fill_color, const int in_layer_min_value, const int in_layer_max_value, - const std::string & in_tf_target_frame, const std::string & in_tf_source_frame, - const tf2_ros::Buffer & in_tf_buffer); +void fill_polygon_areas( + grid_map::GridMap & out_grid_map, const std::vector & in_polygons, + const std::string & in_grid_layer_name, const float in_layer_background_value, + const float in_fill_value, const std::string & in_tf_target_frame, + const std::string & in_tf_source_frame, const tf2_ros::Buffer & in_tf_buffer); -} // namespace object_map +} // namespace autoware::costmap_generator::object_map #endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp index aa11a98830dc3..a04e139fdaa6e 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp @@ -72,7 +72,7 @@ class ObjectsToCostmap /// \param[out] calculated cost in grid_map::Matrix format grid_map::Matrix makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, - const double size_of_expansion_kernel, + const int64_t size_of_expansion_kernel, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); private: @@ -103,7 +103,7 @@ class ObjectsToCostmap /// \param[in] in_corner_point one of convex hull points /// \param[in] expand_polygon_size the param for expanding convex_hull points /// \param[out] expanded point - geometry_msgs::msg::Point makeExpandedPoint( + static geometry_msgs::msg::Point makeExpandedPoint( const geometry_msgs::msg::Point & in_centroid, const geometry_msgs::msg::Point32 & in_corner_point, const double expand_polygon_size); @@ -111,7 +111,7 @@ class ObjectsToCostmap /// \param[in] in_centroid: object's centroid /// \param[in] expand_polygon_size: expanding convex_hull points /// \param[out] polygon object with convex hull points - grid_map::Polygon makePolygonFromObjectConvexHull( + static grid_map::Polygon makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size); @@ -121,7 +121,7 @@ class ObjectsToCostmap /// \param[in] gridmap_layer_name: target gridmap layer name for calculated cost /// \param[in] score: set score as a cost for costmap /// \param[in] objects_costmap: update cost in this objects_costmap[gridmap_layer_name] - void setCostInPolygon( + static void setCostInPolygon( const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score, grid_map::GridMap & objects_costmap); }; diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 07bf9eb33ffbe..0a85f7520b317 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -91,15 +92,14 @@ std::shared_ptr findNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) { - const auto linked_parking_lot = std::make_shared(); + auto linked_parking_lot = std::make_shared(); const auto result = lanelet::utils::query::getLinkedParkingLot( current_position, lanelet_map_ptr, linked_parking_lot.get()); if (result) { return linked_parking_lot; - } else { - return {}; } + return {}; } // copied from scenario selector @@ -120,20 +120,6 @@ bool isInParkingLot( return lanelet::geometry::within(search_point, nearest_parking_lot->basicPolygon()); } -// Convert from Point32 to Point -std::vector poly2vector(const geometry_msgs::msg::Polygon & poly) -{ - std::vector ps; - for (const auto & p32 : poly.points) { - geometry_msgs::msg::Point p; - p.x = p32.x; - p.y = p32.y; - p.z = p32.z; - ps.push_back(p); - } - return ps; -} - pcl::PointCloud getTransformedPointCloud( const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const geometry_msgs::msg::Transform & transform) @@ -206,7 +192,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) void CostmapGenerator::loadRoadAreasFromLaneletMap( const lanelet::LaneletMapPtr lanelet_map, - std::vector> * area_points) + std::vector & area_polygons) { // use all lanelets in map of subtype road to give way area lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map); @@ -215,21 +201,25 @@ void CostmapGenerator::loadRoadAreasFromLaneletMap( // convert lanelets to polygons and put into area_points array for (const auto & ll : road_lanelets) { geometry_msgs::msg::Polygon poly; - lanelet::visualization::lanelet2Polygon(ll, &poly); - area_points->push_back(poly2vector(poly)); + geometry_msgs::msg::Point32 pt; + for (const auto & p : ll.polygon3d().basicPolygon()) { + lanelet::utils::conversion::toGeomMsgPt32(p, &pt); + poly.points.push_back(pt); + } + area_polygons.push_back(poly); } } void CostmapGenerator::loadParkingAreasFromLaneletMap( const lanelet::LaneletMapPtr lanelet_map, - std::vector> * area_points) + std::vector & area_polygons) { // Parking lots lanelet::ConstPolygons3d all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map); for (const auto & ll_poly : all_parking_lots) { geometry_msgs::msg::Polygon poly; lanelet::utils::conversion::toGeomMsgPoly(ll_poly, &poly); - area_points->push_back(poly2vector(poly)); + area_polygons.push_back(poly); } // Parking spaces @@ -238,10 +228,9 @@ void CostmapGenerator::loadParkingAreasFromLaneletMap( for (const auto & parking_space : all_parking_spaces) { lanelet::ConstPolygon3d ll_poly; lanelet::utils::lineStringWithWidthToPolygon(parking_space, &ll_poly); - geometry_msgs::msg::Polygon poly; lanelet::utils::conversion::toGeomMsgPoly(ll_poly, &poly); - area_points->push_back(poly2vector(poly)); + area_polygons.push_back(poly); } } @@ -252,11 +241,11 @@ void CostmapGenerator::onLaneletMapBin( lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); if (param_->use_wayarea) { - loadRoadAreasFromLaneletMap(lanelet_map_, &primitives_points_); + loadRoadAreasFromLaneletMap(lanelet_map_, primitives_polygons_); } if (param_->use_parkinglot) { - loadParkingAreasFromLaneletMap(lanelet_map_, &primitives_points_); + loadParkingAreasFromLaneletMap(lanelet_map_, primitives_polygons_); } } @@ -346,11 +335,10 @@ bool CostmapGenerator::isActive() } } return false; - } else { - const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); - if (!current_pose_wrt_map) return false; - return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); } + const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); + if (!current_pose_wrt_map) return false; + return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); } void CostmapGenerator::initGridmap() @@ -403,7 +391,8 @@ autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects } for (auto & object : objects->objects) { - geometry_msgs::msg::PoseStamped output_stamped, input_stamped; + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; input_stamped.pose = object.kinematics.initial_pose_with_covariance.pose; tf2::doTransform(input_stamped, output_stamped, objects2costmap); object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; @@ -428,11 +417,10 @@ grid_map::Matrix CostmapGenerator::generateObjectsCostmap( grid_map::Matrix CostmapGenerator::generatePrimitivesCostmap() { grid_map::GridMap lanelet2_costmap = costmap_; - if (!primitives_points_.empty()) { - object_map::FillPolygonAreas( - lanelet2_costmap, primitives_points_, LayerName::primitives, param_->grid_max_value, - param_->grid_min_value, param_->grid_min_value, param_->grid_max_value, param_->costmap_frame, - param_->map_frame, tf_buffer_); + if (!primitives_polygons_.empty()) { + object_map::fill_polygon_areas( + lanelet2_costmap, primitives_polygons_, LayerName::primitives, param_->grid_max_value, + param_->grid_min_value, param_->costmap_frame, param_->map_frame, tf_buffer_); } return lanelet2_costmap[LayerName::primitives]; } diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp index 297b6fe984cbd..d8413f197a349 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -32,74 +32,43 @@ #include "autoware_costmap_generator/object_map_utils.hpp" +#include +#include + #include #include #include -namespace object_map +namespace autoware::costmap_generator::object_map { -void FillPolygonAreas( - grid_map::GridMap & out_grid_map, - const std::vector> & in_points, - const std::string & in_grid_layer_name, const int in_layer_background_value, - const int in_fill_color, const int in_layer_min_value, const int in_layer_max_value, - const std::string & in_tf_target_frame, const std::string & in_tf_source_frame, - const tf2_ros::Buffer & in_tf_buffer) +void fill_polygon_areas( + grid_map::GridMap & out_grid_map, const std::vector & in_polygons, + const std::string & in_grid_layer_name, const float in_layer_background_value, + const float in_fill_value, const std::string & in_tf_target_frame, + const std::string & in_tf_source_frame, const tf2_ros::Buffer & in_tf_buffer) { if (!out_grid_map.exists(in_grid_layer_name)) { out_grid_map.add(in_grid_layer_name); } out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value); - cv::Mat original_image; - grid_map::GridMapCvConverter::toImage( - out_grid_map, in_grid_layer_name, CV_8UC1, in_layer_min_value, in_layer_max_value, - original_image); - - cv::Mat merged_filled_image = original_image.clone(); - - geometry_msgs::msg::TransformStamped transform; - transform = + const geometry_msgs::msg::TransformStamped transform = in_tf_buffer.lookupTransform(in_tf_target_frame, in_tf_source_frame, tf2::TimePointZero); - // calculate out_grid_map position - grid_map::Position map_pos = out_grid_map.getPosition(); - const double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x(); - const double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y(); - - for (const auto & points : in_points) { - std::vector cv_polygon; - - for (const auto & p : points) { - // transform to GridMap coordinate - geometry_msgs::msg::Point transformed_point; - geometry_msgs::msg::PointStamped output_stamped, input_stamped; - input_stamped.point = p; - tf2::doTransform(input_stamped, output_stamped, transform); - transformed_point = output_stamped.point; - - // coordinate conversion for cv image - const double cv_x = (out_grid_map.getLength().y() - origin_y_offset - transformed_point.y) / - out_grid_map.getResolution(); - const double cv_y = (out_grid_map.getLength().x() - origin_x_offset - transformed_point.x) / - out_grid_map.getResolution(); - cv_polygon.emplace_back(cv_x, cv_y); + for (const auto & poly : in_polygons) { + // transform from Map to GridMap coordinates + geometry_msgs::msg::Polygon transformed_poly; + tf2::doTransform(poly, transformed_poly, transform); + grid_map::Polygon grid_map_poly; + for (const auto & p : transformed_poly.points) { + grid_map_poly.addVertex({p.x, p.y}); + } + for (grid_map_utils::PolygonIterator it(out_grid_map, grid_map_poly); !it.isPastEnd(); ++it) { + out_grid_map.at(in_grid_layer_name, *it) = in_fill_value; } - - cv::Mat filled_image = original_image.clone(); - - std::vector> cv_polygons; - cv_polygons.push_back(cv_polygon); - cv::fillPoly(filled_image, cv_polygons, cv::Scalar(in_fill_color)); - - merged_filled_image &= filled_image; } - - // convert to ROS msg - grid_map::GridMapCvConverter::addLayerFromImage( - merged_filled_image, in_grid_layer_name, out_grid_map, in_layer_min_value, in_layer_max_value); } -} // namespace object_map +} // namespace autoware::costmap_generator::object_map diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp index afb2dbb9874c9..149586edeea28 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp @@ -44,6 +44,10 @@ #include "autoware_costmap_generator/objects_to_costmap.hpp" +#include +#include + +#include #include #include @@ -148,7 +152,7 @@ void ObjectsToCostmap::setCostInPolygon( const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score, grid_map::GridMap & objects_costmap) { - for (grid_map::PolygonIterator itr(objects_costmap, polygon); !itr.isPastEnd(); ++itr) { + for (grid_map_utils::PolygonIterator itr(objects_costmap, polygon); !itr.isPastEnd(); ++itr) { const float current_score = objects_costmap.at(gridmap_layer_name, *itr); if (score > current_score) { objects_costmap.at(gridmap_layer_name, *itr) = score; @@ -156,14 +160,40 @@ void ObjectsToCostmap::setCostInPolygon( } } +void naive_mean_filter_on_grid_edges( + // cppcheck-suppress constParameterReference + const grid_map::Matrix & input, const int kernel_size, grid_map::Matrix & output) +{ + for (auto i = 0; i < input.rows(); ++i) { + for (auto j = 0; j < input.cols(); ++j) { + const auto is_inside = + (i > kernel_size + 1 && j > kernel_size + 1) && + (i + kernel_size + 1 < input.rows() && j + kernel_size + 1 < input.cols()); + if (is_inside) { + continue; + } + auto size = 0.0f; + auto sum = 0.0f; + for (auto i_offset = std::max(0, i - kernel_size); + i_offset < i + kernel_size && i_offset < input.rows(); ++i_offset) { + for (auto j_offset = std::max(0, j - kernel_size); + j_offset < j + kernel_size && j_offset < input.cols(); ++j_offset) { + ++size; + sum += input(i_offset, j_offset); + } + } + output(i, j) = sum / size; + } + } +} + grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, - const double size_of_expansion_kernel, + const int64_t size_of_expansion_kernel, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { grid_map::GridMap objects_costmap = costmap; objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0); - objects_costmap.add(BLURRED_OBJECTS_COSTMAP_LAYER_, 0); for (const auto & object : in_objects->objects) { grid_map::Polygon polygon; @@ -178,23 +208,28 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const auto highest_probability_label = *std::max_element( object.classification.begin(), object.classification.end(), [](const auto & c1, const auto & c2) { return c1.probability < c2.probability; }); - const double highest_probability = static_cast(highest_probability_label.probability); - setCostInPolygon(polygon, OBJECTS_COSTMAP_LAYER_, highest_probability, objects_costmap); - setCostInPolygon(polygon, BLURRED_OBJECTS_COSTMAP_LAYER_, highest_probability, objects_costmap); + setCostInPolygon( + polygon, OBJECTS_COSTMAP_LAYER_, highest_probability_label.probability, objects_costmap); } + objects_costmap.add(BLURRED_OBJECTS_COSTMAP_LAYER_, 0.0); // Applying mean filter to expanded gridmap - const grid_map::SlidingWindowIterator::EdgeHandling edge_handling = - grid_map::SlidingWindowIterator::EdgeHandling::CROP; - for (grid_map::SlidingWindowIterator iterator( - objects_costmap, BLURRED_OBJECTS_COSTMAP_LAYER_, edge_handling, size_of_expansion_kernel); - !iterator.isPastEnd(); ++iterator) { - objects_costmap.at(BLURRED_OBJECTS_COSTMAP_LAYER_, *iterator) = - iterator.getData().meanOfFinites(); // Blurring. + const auto & original_matrix = objects_costmap[OBJECTS_COSTMAP_LAYER_]; + Eigen::MatrixXf & filtered_matrix = objects_costmap[BLURRED_OBJECTS_COSTMAP_LAYER_]; + // edge of the grid: naive filter + const auto kernel_size = static_cast(static_cast(size_of_expansion_kernel) / 2.0); + naive_mean_filter_on_grid_edges(original_matrix, kernel_size, filtered_matrix); + // inside the grid: optimized filter using Eigen block + for (auto i = 0; i < filtered_matrix.rows() - size_of_expansion_kernel; ++i) { + for (auto j = 0; j < filtered_matrix.cols() - size_of_expansion_kernel; ++j) { + const auto mean = + original_matrix.block(i, j, size_of_expansion_kernel, size_of_expansion_kernel).mean(); + filtered_matrix(i + kernel_size, j + kernel_size) = mean; + } } - objects_costmap[OBJECTS_COSTMAP_LAYER_] = objects_costmap[OBJECTS_COSTMAP_LAYER_].cwiseMax( - objects_costmap[BLURRED_OBJECTS_COSTMAP_LAYER_]); + objects_costmap[OBJECTS_COSTMAP_LAYER_] = + objects_costmap[OBJECTS_COSTMAP_LAYER_].cwiseMax(filtered_matrix); return objects_costmap[OBJECTS_COSTMAP_LAYER_]; } diff --git a/planning/autoware_costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml index 29c1be77d3d75..8ad90e12d4cd3 100644 --- a/planning/autoware_costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -15,6 +15,7 @@ ament_cmake_auto autoware_cmake + autoware_grid_map_utils autoware_lanelet2_extension autoware_map_msgs autoware_perception_msgs