feat: costmap inflation (#31)

* implement costmap inflation Signed-off-by: Autumn60 <[email protected]> * adjust params for costmap inflation Signed-off-by: Autumn60 <[email protected]> --------- Signed-off-by: Autumn60 <[email protected]>
booars · Sep 5, 2024 · 837cd05 · 837cd05
1 parent bb49763
commit 837cd05
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 8 deletions.
diff --git a/...rkspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/...rkspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml
@@ -1,10 +1,10 @@
 /**:
   ros__parameters:
-    update_rate: 20.0
+    update_rate: 20.0       # [Hz]
     map_frame_id: "map"
     costmap_center_frame_id: "base_link"
-    costmap_width: 50.0
-    costmap_resolution: 0.2
+    costmap_width: 50.0     # [m]
+    costmap_resolution: 0.2 # [m/cell]
     multi_layered_costmap:
       layers:
         - "cached_lanelet_layer"
@@ -13,12 +13,13 @@
         type: "cached_lanelet"
         map_topic: "/map/vector_map"
         costmap_topic: "~/debug/cached_costmap"
+        inflation_radius: 0.6 # [m]
         cached_costmap:
-          min_x: 89607.0
-          max_x: 89687.0
-          min_y: 43114.0
-          max_y: 43194.0
-          resolution: 0.1
+          min_x: 89607.0  # [m]
+          max_x: 89687.0  # [m]
+          min_y: 43114.0  # [m]
+          max_y: 43194.0  # [m]
+          resolution: 0.1 # [m/cell]
       predicted_object_layer:
         type: "predicted_object"
         predicted_objects_topic: "/perception/object_recognition/objects"

diff --git a/...c/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/...c/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml
@@ -14,6 +14,7 @@
         type: "cached_lanelet"
         map_topic: "/map/vector_map"
         costmap_topic: "~/debug/cached_costmap"
+        inflation_radius: 0.5
         cached_costmap:
           min_x: 89607.0
           max_x: 89687.0

diff --git a/.../costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp b/.../costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp
@@ -68,6 +68,7 @@ class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase
 
   double cached_costmap_origin_x_;
   double cached_costmap_origin_y_;
+  int inflation_radius_index_;
   bool is_map_ready_ = false;
 };
 }  // namespace cached_lanelet_costmap

diff --git a/...pace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp b/...pace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp
@@ -40,6 +40,7 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin
   costmap_pub_ = node.create_publisher<OccupancyGrid>(
     costmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local());
 
+  // Declare cached costmap
   {
     double cached_costmap_min_x =
       node.declare_parameter(layer_namespace + ".cached_costmap.min_x", 0.0);
@@ -68,6 +69,12 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin
     booars_utils::nav::occupancy_grid_utils::update_origin(
       cached_costmap_, cached_costmap_parameters_, origin);
   }
+
+  // Declare other parameters
+  {
+    double inflation_radius = node.declare_parameter(layer_namespace + ".inflation_radius", 0.0);
+    inflation_radius_index_ = std::ceil(inflation_radius / cached_costmap_parameters_->resolution());
+  }
 }
 
 bool CachedLaneletCostmap::is_ready()
@@ -115,6 +122,9 @@ bool CachedLaneletCostmap::try_transform_point(
 void CachedLaneletCostmap::create_cached_costmap(
   const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets)
 {
+  std::vector<int> unoccupied_indices; 
+
+  // Create costmap
   cached_costmap_->header.frame_id = map_frame_id;
   for (int i = 0; i < cached_costmap_parameters_->grid_num(); i++) {
     tier4_autoware_utils::Point2d point2d =
@@ -125,9 +135,33 @@ void CachedLaneletCostmap::create_cached_costmap(
     for (const auto & road_lanelet : roads_lanelets) {
       if (!lanelet::geometry::within(point2d, road_lanelet.polygon2d().basicPolygon())) continue;
       cached_costmap_->data[i] = 0;
+      unoccupied_indices.push_back(i);
       break;
     }
   }
+
+  // Inflate costmap
+  if (inflation_radius_index_ > 0) {
+    int inflation_radius_index_sqr = inflation_radius_index_ * inflation_radius_index_;
+    for (const auto & unoccupied_index : unoccupied_indices)
+    {
+      bool occupied = false;
+      for (int i = -inflation_radius_index_; i <= inflation_radius_index_; i++) {
+        for (int j = -inflation_radius_index_; j <= inflation_radius_index_; j++) {
+          if (i*i + j*j > inflation_radius_index_sqr) continue;
+          int index = unoccupied_index + i * cached_costmap_parameters_->grid_width() + j;
+          if (index < 0 || index >= cached_costmap_parameters_->grid_num()) continue;
+          if(cached_costmap_->data[index] != 100) continue;
+          cached_costmap_->data[unoccupied_index] = 50;
+          occupied = true;
+          break;
+        }
+        if(occupied) break;
+      }
+    }
+  }
+
+  // Publish costmap
   costmap_pub_->publish(*cached_costmap_);
 
   is_map_ready_ = true;