feat(planning): create cached lanelet costmap

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml

@@ -1,18 +1,25 @@
 /**:
   ros__parameters:
-    update_rate: 10.0
+    update_rate: 20.0
     map_frame_id: "map"
     costmap_center_frame_id: "base_link"
     costmap_width: 50.0
     costmap_resolution: 0.2
     multi_layered_costmap:
       layers:
-        - "lanelet_layer"
+        - "cached_lanelet_layer"
         - "predicted_object_layer"
-      lanelet_layer:
-        type: "lanelet"
+      cached_lanelet_layer:
+        type: "cached_lanelet"
         map_topic: "/map/vector_map"
+        costmap_topic: "~/debug/cached_costmap"
+        cached_costmap:
+          min_x: 89607.0
+          max_x: 89687.0
+          min_y: 43114.0
+          max_y: 43194.0
+          resolution: 0.1
       predicted_object_layer:
         type: "predicted_object"
         predicted_objects_topic: "/perception/object_recognition/objects"
-        distance_threshold: 1.2
+        distance_threshold: 3.0

aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp

@@ -25,37 +25,49 @@ class OccupancyGridParameters
   using SharedPtr = std::shared_ptr<OccupancyGridParameters>;
 
   static OccupancyGridParameters::SharedPtr create_parameters(
-    const double width, const double resolution)
+    const double width, const double height, const double resolution)
   {
-    return std::make_shared<OccupancyGridParameters>(width, resolution);
+    return std::make_shared<OccupancyGridParameters>(width, height, resolution);
   }
 
-  explicit OccupancyGridParameters(const double width, const double resolution)
+  explicit OccupancyGridParameters(const double width, const double height, const double resolution)
   {
     width_ = width;
     width_2_ = width / 2.0;
+    height_ = height;
+    height_2_ = height / 2.0;
     resolution_ = resolution;
     resolution_inv_ = 1.0 / resolution;
     grid_width_2_ = static_cast<int>(width * resolution_inv_) / 2;
     grid_width_ = grid_width_2_ * 2;
-    grid_num_ = grid_width_ * grid_width_;
+    grid_height_2_ = static_cast<int>(height * resolution_inv_) / 2;
+    grid_height_ = grid_height_2_ * 2;
+    grid_num_ = grid_width_ * grid_height_;
   }
 
   double width() const { return width_; }
   double width_2() const { return width_2_; }
+  double height() const { return height_; }
+  double height_2() const { return height_2_; }
   double resolution() const { return resolution_; }
   double resolution_inv() const { return resolution_inv_; }
   int grid_width_2() const { return grid_width_2_; }
   int grid_width() const { return grid_width_; }
+  int gird_height_2() const { return grid_height_2_; }
+  int grid_height() const { return grid_height_; }
   int grid_num() const { return grid_num_; }
 
 private:
   double width_;
   double width_2_;
+  double height_;
+  double height_2_;
   double resolution_;
   double resolution_inv_;
   int grid_width_2_;
   int grid_width_;
+  int grid_height_2_;
+  int grid_height_;
   int grid_num_;
 };
 

aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp

@@ -29,18 +29,19 @@ OccupancyGridParameters::SharedPtr create_occupancy_grid_parameters(
   const OccupancyGrid::SharedPtr occupancy_grid)
 {
   const double width = occupancy_grid->info.width * occupancy_grid->info.resolution;
-  return OccupancyGridParameters::create_parameters(width, occupancy_grid->info.resolution);
+  const double height = occupancy_grid->info.height * occupancy_grid->info.resolution;
+  return OccupancyGridParameters::create_parameters(width, height, occupancy_grid->info.resolution);
 }
 
 OccupancyGrid::SharedPtr create_occupancy_grid(
   const OccupancyGridParameters::SharedPtr parameters, const int8_t value = 0)
 {
   OccupancyGrid::SharedPtr occupancy_grid = std::make_shared<OccupancyGrid>();
   occupancy_grid->info.width = parameters->grid_width();
-  occupancy_grid->info.height = parameters->grid_width();
+  occupancy_grid->info.height = parameters->grid_height();
   occupancy_grid->info.resolution = parameters->resolution();
   occupancy_grid->info.origin.position.x = -parameters->width_2();
-  occupancy_grid->info.origin.position.y = -parameters->width_2();
+  occupancy_grid->info.origin.position.y = -parameters->height_2();
   occupancy_grid->data.resize(parameters->grid_num(), value);
   return occupancy_grid;
 }
@@ -50,7 +51,7 @@ void update_origin(
   const geometry_msgs::msg::Vector3 & translation)
 {
   occupancy_grid->info.origin.position.x = -parameters->width_2() + translation.x;
-  occupancy_grid->info.origin.position.y = -parameters->width_2() + translation.y;
+  occupancy_grid->info.origin.position.y = -parameters->height_2() + translation.y;
   occupancy_grid->info.origin.position.z = translation.z;
   occupancy_grid->info.origin.orientation.x = 0.0;
   occupancy_grid->info.origin.orientation.y = 0.0;
@@ -66,7 +67,7 @@ tier4_autoware_utils::Point2d index_to_point(
 
   return {
     index_x * parameters->resolution() - parameters->width_2(),
-    index_y * parameters->resolution() - parameters->width_2()};
+    index_y * parameters->resolution() - parameters->height_2()};
 }
 
 std::vector<tier4_autoware_utils::Point2d> get_index_to_point_table(
@@ -80,6 +81,16 @@ std::vector<tier4_autoware_utils::Point2d> get_index_to_point_table(
   return table;
 }
 
+int point_to_index(
+  const OccupancyGridParameters::SharedPtr parameters, const tier4_autoware_utils::Point2d & point)
+{
+  const int x =
+    static_cast<int>((point[0] + parameters->width_2()) * parameters->resolution_inv() + 0.5);
+  const int y =
+    static_cast<int>((point[1] + parameters->height_2()) * parameters->resolution_inv() + 0.5);
+  return y * parameters->grid_width() + x;
+}
+
 }  // namespace booars_utils::nav::occupancy_grid_utils
 
 #endif  // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_

aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml

@@ -1,14 +1,25 @@
 /**:
   ros__parameters:
-    update_rate: 10.0
+    update_rate: 1.0
     map_frame_id: "map"
     costmap_center_frame_id: "base_link"
-    costmap_width: 20.0
-    costmap_resolution: 0.1
+    costmap_width: 50.0
+    costmap_resolution: 0.2
     multi_layered_costmap:
       layers:
+        - "cached_lanelet_layer"
         - "lanelet_layer"
         - "predicted_object_layer"
+      cached_lanelet_layer:
+        type: "cached_lanelet"
+        map_topic: "/map/vector_map"
+        costmap_topic: "~/debug/cached_costmap"
+        cached_costmap:
+          min_x: 89607.0
+          max_x: 89687.0
+          min_y: 43114.0
+          max_y: 43194.0
+          resolution: 0.1
       lanelet_layer:
         type: "lanelet"
         map_topic: "/map/vector_map"

aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp

@@ -44,7 +44,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options)
     double costmap_width = this->declare_parameter("costmap_width", 10.0);
     double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1);
     costmap_parameters_ =
-      OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution);
+      OccupancyGridParameters::create_parameters(costmap_width, costmap_width, costmap_resolution);
     index_to_point_table_ =
       booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_);
   }

aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp

@@ -15,12 +15,14 @@
 #ifndef BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_
 #define BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_
 
+#include <cached_lanelet_costmap/cached_lanelet_costmap.hpp>
 #include <lanelet_costmap/lanelet_costmap.hpp>
 #include <multi_layered_costmap/multi_layered_costmap.hpp>
 #include <predicted_object_costmap/predicted_object_costmap.hpp>
 
 namespace booars_costmap_utils
 {
+using CachedLaneletCostmap = cached_lanelet_costmap::CachedLaneletCostmap;
 using LaneletCostmap = lanelet_costmap::LaneletCostmap;
 using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap;
 using PredictedObjectCostmap = predicted_object_costmap::PredictedObjectCostmap;
@@ -36,7 +38,10 @@ MultiLayeredCostmap::SharedPtr create_multi_layered_costmap(
     std::string layer_namespace = costmap_namespace + "." + layer;
     auto type = node.declare_parameter(layer_namespace + ".type", std::string());
 
-    if (type == "lanelet") {
+    if (type == "cached_lanelet") {
+      auto cached_lanelet_costmap = CachedLaneletCostmap::create_costmap(node, layer_namespace);
+      costmap->add_costmap_layer(cached_lanelet_costmap);
+    } else if (type == "lanelet") {
       auto lanelet_costmap = LaneletCostmap::create_costmap(node, layer_namespace);
       costmap->add_costmap_layer(lanelet_costmap);
     } else if (type == "predicted_object") {

aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>cached_lanelet_costmap</depend>
   <depend>lanelet_costmap</depend>
   <depend>multi_layered_costmap</depend>
   <depend>predicted_object_costmap</depend>

aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(cached_lanelet_costmap)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(cached_lanelet_costmap SHARED
+  src/cached_lanelet_costmap.cpp
+)
+
+ament_auto_package()

aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp

@@ -0,0 +1,75 @@
+// Copyright 2024 Booars
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_
+#define CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_
+
+#include <booars_utils/nav/occupancy_grid_parameters.hpp>
+#include <booars_utils/nav/occupancy_grid_utils.hpp>
+#include <multi_layered_costmap/costmap_base.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace cached_lanelet_costmap
+{
+
+using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
+using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters;
+using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+
+class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase
+{
+public:
+  using SharedPtr = std::shared_ptr<CachedLaneletCostmap>;
+  explicit CachedLaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace);
+
+  static CostmapBase::SharedPtr create_costmap(
+    rclcpp::Node & node, const std::string & layer_namespace)
+  {
+    return std::make_shared<CachedLaneletCostmap>(node, layer_namespace);
+  }
+
+  bool is_ready() override;
+  bool is_occupied(const geometry_msgs::msg::PointStamped & point) override;
+
+private:
+  bool try_transform_point(
+    const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point,
+    const std::string & target_frame);
+  void create_cached_costmap(
+    const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets);
+  void map_callback(const HADMapBin::SharedPtr msg);
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  rclcpp::Subscription<HADMapBin>::SharedPtr map_sub_;
+  rclcpp::Publisher<OccupancyGrid>::SharedPtr costmap_pub_;
+
+  OccupancyGrid::SharedPtr cached_costmap_;
+  OccupancyGridParameters::SharedPtr cached_costmap_parameters_;
+
+  double cached_costmap_origin_x_;
+  double cached_costmap_origin_y_;
+  bool is_map_ready_ = false;
+};
+}  // namespace cached_lanelet_costmap
+
+#endif  // CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_

aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cached_lanelet_costmap</name>
+  <version>0.0.0</version>
+  <description>The cached_lanelet_costmap pkg</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>booars_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>multi_layered_costmap</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>