fix(costmap_generator): restrict costmap within parking lot (#996)

* fix(costmap_generator): restrict costmap within parking lot

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>

* add parameters for free space planning area selection

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py

@@ -73,6 +73,7 @@ def launch_setup(context, *args, **kwargs):
                         "map_frame": "map",
                         "update_rate": 10.0,
                         "use_wayarea": True,
+                        "use_parkinglot": True,
                         "use_objects": True,
                         "use_points": True,
                         "grid_min_value": 0.0,

planning/costmap_generator/README.md

@@ -38,6 +38,7 @@ None
 | `use_objects`                | bool   | whether using `~input/objects` or not                                                          |
 | `use_points`                 | bool   | whether using `~input/points_no_ground` or not                                                 |
 | `use_wayarea`                | bool   | whether using `wayarea` from `~input/vector_map` or not                                        |
+| `use_parkinglot`             | bool   | whether using `parkinglot` from `~input/vector_map` or not                                     |
 | `costmap_frame`              | string | created costmap's coordinate                                                                   |
 | `vehicle_frame`              | string | vehicle's coordinate                                                                           |
 | `map_frame`                  | string | map's coordinate                                                                               |
@@ -69,8 +70,8 @@ endif
 
 :set the center of costmap to current pose;
 
-if (use wayarea?) then (yes)
- :generate wayarea costmap;
+if (use wayarea or use parkinglot?) then (yes)
+ :generate map primitives costmap;
 endif
 
 if (use objects?) then (yes)

planning/costmap_generator/include/costmap_generator/costmap_generator.hpp

@@ -81,6 +81,7 @@ class CostmapGenerator : public rclcpp::Node
   bool use_objects_;
   bool use_points_;
   bool use_wayarea_;
+  bool use_parkinglot_;
 
   lanelet::LaneletMapPtr lanelet_map_;
   autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_;
@@ -123,7 +124,7 @@ class CostmapGenerator : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  std::vector<std::vector<geometry_msgs::msg::Point>> area_points_;
+  std::vector<std::vector<geometry_msgs::msg::Point>> primitives_points_;
 
   PointsToCostmap points2costmap_;
   ObjectsToCostmap objects2costmap_;
@@ -134,7 +135,7 @@ class CostmapGenerator : public rclcpp::Node
   {
     static constexpr const char * objects = "objects";
     static constexpr const char * points = "points";
-    static constexpr const char * wayarea = "wayarea";
+    static constexpr const char * primitives = "primitives";
     static constexpr const char * combined = "combined";
   };
 
@@ -192,7 +193,7 @@ class CostmapGenerator : public rclcpp::Node
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects);
 
   /// \brief calculate cost from lanelet2 map
-  grid_map::Matrix generateWayAreaCostmap();
+  grid_map::Matrix generatePrimitivesCostmap();
 
   /// \brief calculate cost for final output
   grid_map::Matrix generateCombinedCostmap();

planning/costmap_generator/include/costmap_generator/object_map_utils.hpp

@@ -78,19 +78,19 @@ void PublishOccupancyGrid(
 /*!
  * Projects the in_area_points forming the road, stores the result in out_grid_map.
  * @param[out] out_grid_map GridMap object to add the road grid
- * @param[in] in_area_points Array of points containing the wayareas
+ * @param[in] in_points Array of points containing the selected primitives
  * @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 wayareas
+ * @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 later
- * @param[in] in_tf_target_frame Target frame to transform the wayarea points
+ * @param[in] in_layer_max_value Maximum value in the layer
+ * @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_area_points,
+  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,

planning/costmap_generator/launch/costmap_generator.launch.xml

@@ -21,6 +21,7 @@
     <param name="update_rate" value="10.0"/>
 
     <param name="use_wayarea" value="true"/>
+    <param name="use_parkinglot" value="true"/>
     <param name="use_objects" value="true"/>
     <param name="use_points" value="true"/>
 

planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp

@@ -178,6 +178,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
   use_objects_ = this->declare_parameter<bool>("use_objects", true);
   use_points_ = this->declare_parameter<bool>("use_points", true);
   use_wayarea_ = this->declare_parameter<bool>("use_wayarea", true);
+  use_parkinglot_ = this->declare_parameter<bool>("use_parkinglot", true);
   expand_polygon_size_ = this->declare_parameter<double>("expand_polygon_size", 1.0);
   size_of_expansion_kernel_ = this->declare_parameter<int>("size_of_expansion_kernel", 9);
 
@@ -268,8 +269,11 @@ void CostmapGenerator::onLaneletMapBin(
   lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_);
 
   if (use_wayarea_) {
-    loadRoadAreasFromLaneletMap(lanelet_map_, &area_points_);
-    loadParkingAreasFromLaneletMap(lanelet_map_, &area_points_);
+    loadRoadAreasFromLaneletMap(lanelet_map_, &primitives_points_);
+  }
+
+  if (use_parkinglot_) {
+    loadParkingAreasFromLaneletMap(lanelet_map_, &primitives_points_);
   }
 }
 
@@ -311,8 +315,8 @@ void CostmapGenerator::onTimer()
   p.y() = tf.transform.translation.y;
   costmap_.setPosition(p);
 
-  if (use_wayarea_ && lanelet_map_) {
-    costmap_[LayerName::wayarea] = generateWayAreaCostmap();
+  if ((use_wayarea_ || use_parkinglot_) && lanelet_map_) {
+    costmap_[LayerName::primitives] = generatePrimitivesCostmap();
   }
 
   if (use_objects_ && objects_) {
@@ -355,7 +359,7 @@ void CostmapGenerator::initGridmap()
 
   costmap_.add(LayerName::points, grid_min_value_);
   costmap_.add(LayerName::objects, grid_min_value_);
-  costmap_.add(LayerName::wayarea, grid_min_value_);
+  costmap_.add(LayerName::primitives, grid_min_value_);
   costmap_.add(LayerName::combined, grid_min_value_);
 }
 
@@ -418,15 +422,15 @@ grid_map::Matrix CostmapGenerator::generateObjectsCostmap(
   return objects_costmap;
 }
 
-grid_map::Matrix CostmapGenerator::generateWayAreaCostmap()
+grid_map::Matrix CostmapGenerator::generatePrimitivesCostmap()
 {
   grid_map::GridMap lanelet2_costmap = costmap_;
-  if (!area_points_.empty()) {
+  if (!primitives_points_.empty()) {
     object_map::FillPolygonAreas(
-      lanelet2_costmap, area_points_, LayerName::wayarea, grid_max_value_, grid_min_value_,
+      lanelet2_costmap, primitives_points_, LayerName::primitives, grid_max_value_, grid_min_value_,
       grid_min_value_, grid_max_value_, costmap_frame_, map_frame_, tf_buffer_);
   }
-  return lanelet2_costmap[LayerName::wayarea];
+  return lanelet2_costmap[LayerName::primitives];
 }
 
 grid_map::Matrix CostmapGenerator::generateCombinedCostmap()
@@ -440,7 +444,7 @@ grid_map::Matrix CostmapGenerator::generateCombinedCostmap()
     combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::points]);
 
   combined_costmap[LayerName::combined] =
-    combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::wayarea]);
+    combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::primitives]);
 
   combined_costmap[LayerName::combined] =
     combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::objects]);

planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp

@@ -59,7 +59,7 @@ void PublishOccupancyGrid(
 
 void FillPolygonAreas(
   grid_map::GridMap & out_grid_map,
-  const std::vector<std::vector<geometry_msgs::msg::Point>> & in_area_points,
+  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_layer_min_value, const int in_fill_color, const int in_layer_max_value,
   const std::string & in_tf_target_frame, const std::string & in_tf_source_frame,
@@ -86,7 +86,7 @@ void FillPolygonAreas(
   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_area_points) {
+  for (const auto & points : in_points) {
     std::vector<cv::Point> cv_polygon;
 
     for (const auto & p : points) {