perf(costmap_generator): manual blurring and fill polygons without Op…

…enCV (autowarefoundation#9160)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp

@@ -70,7 +70,6 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
-#include <string>
 #include <vector>
 
 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<std::vector<geometry_msgs::msg::Point>> primitives_points_;
+  std::vector<geometry_msgs::msg::Polygon> 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<std::vector<geometry_msgs::msg::Point>> * area_points);
+    std::vector<geometry_msgs::msg::Polygon> & 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<std::vector<geometry_msgs::msg::Point>> * area_points);
+    std::vector<geometry_msgs::msg::Polygon> & area_polygons);
 
   /// \brief calculate cost from pointcloud data
   /// \param[in] in_points: subscribed pointcloud data

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 <grid_map_cv/grid_map_cv.hpp>
 #include <grid_map_ros/grid_map_ros.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <geometry_msgs/msg/polygon.hpp>
 #include <grid_map_msgs/msg/grid_map.hpp>
-
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <string>
 #include <vector>
 
-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<std::vector<geometry_msgs::msg::Point>> & 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<geometry_msgs::msg::Polygon> & 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_

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,15 +103,15 @@ 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);
 
   /// \brief make polygon(grid_map::Polygon) from convex hull points
   /// \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);
 };

planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp

@@ -52,6 +52,7 @@
 #include <pcl_ros/transforms.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
+#include <lanelet2_core/Forward.h>
 #include <lanelet2_core/geometry/Polygon.h>
 #include <tf2/time.h>
 #include <tf2/utils.h>
@@ -91,15 +92,14 @@ std::shared_ptr<lanelet::ConstPolygon3d> findNearestParkinglot(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
   const lanelet::BasicPoint2d & current_position)
 {
-  const auto linked_parking_lot = std::make_shared<lanelet::ConstPolygon3d>();
+  auto linked_parking_lot = std::make_shared<lanelet::ConstPolygon3d>();
   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<geometry_msgs::msg::Point> poly2vector(const geometry_msgs::msg::Polygon & poly)
-{
-  std::vector<geometry_msgs::msg::Point> 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<pcl::PointXYZ> 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<std::vector<geometry_msgs::msg::Point>> * area_points)
+  std::vector<geometry_msgs::msg::Polygon> & 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<std::vector<geometry_msgs::msg::Point>> * area_points)
+  std::vector<geometry_msgs::msg::Polygon> & 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];
 }