feat(planning) create global_costmap_generator

.github/CODEOWNERS

@@ -1,5 +1,6 @@
 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booars/aic2024-developers
 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/costmap_generator/** @Autumn60
 aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp
 aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers
 aichallenge/workspace/src/aichallenge_submit/imu_corrector/** @booars/aic2024-developers

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

@@ -0,0 +1,64 @@
+// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_
+#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_
+
+#include <memory>
+
+namespace booars_utils::nav
+{
+class OccupancyGridParameters
+{
+public:
+  using SharedPtr = std::shared_ptr<OccupancyGridParameters>;
+
+  static OccupancyGridParameters::SharedPtr create_parameters(
+    const double width, const double resolution)
+  {
+    return std::make_shared<OccupancyGridParameters>(width, resolution);
+  }
+
+  explicit OccupancyGridParameters(const double width, const double resolution)
+  {
+    width_ = width;
+    width_2_ = width / 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_;
+  }
+
+  double width() const { return width_; }
+  double width_2() const { return width_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 grid_num() const { return grid_num_; }
+
+private:
+  double width_;
+  double width_2_;
+  double resolution_;
+  double resolution_inv_;
+  int grid_width_2_;
+  int grid_width_;
+  int grid_num_;
+};
+
+}  // namespace booars_utils::nav
+
+#endif  // BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_

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

@@ -0,0 +1,74 @@
+// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_
+#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_
+
+#include "booars_utils/nav/occupancy_grid_parameters.hpp"
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+namespace booars_utils::nav::occupancy_grid_utils
+{
+using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
+
+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);
+}
+
+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.resolution = parameters->resolution();
+  occupancy_grid->info.origin.position.x = -parameters->width_2();
+  occupancy_grid->info.origin.position.y = -parameters->width_2();
+  occupancy_grid->data.resize(parameters->grid_num(), value);
+  return occupancy_grid;
+}
+
+void update_origin(
+  OccupancyGrid::SharedPtr occupancy_grid, const OccupancyGridParameters::SharedPtr parameters,
+  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.z = translation.z;
+  occupancy_grid->info.origin.orientation.x = 0.0;
+  occupancy_grid->info.origin.orientation.y = 0.0;
+  occupancy_grid->info.origin.orientation.z = 0.0;
+  occupancy_grid->info.origin.orientation.w = 1.0;
+}
+
+tier4_autoware_utils::Point2d index_to_point(
+  const OccupancyGridParameters::SharedPtr parameters, const int index)
+{
+  const int index_x = index % parameters->grid_width();
+  const int index_y = index / parameters->grid_width();
+
+  return {
+    index_x * parameters->resolution() - parameters->width_2(),
+    index_y * parameters->resolution() - parameters->width_2()};
+}
+
+}  // namespace booars_utils::nav::occupancy_grid_utils
+
+#endif  // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_

aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml

@@ -10,11 +10,13 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(costmap_generator)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(global_costmap_generator SHARED
+  src/global_costmap_generator.cpp
+)
+
+rclcpp_components_register_node(global_costmap_generator
+  PLUGIN "costmap_generator::GlobalCostmapGenerator"
+  EXECUTABLE global_costmap_generator_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    update_rate: 5.0 # Hz
+    map_frame_id: "map"
+    costmap_target_frame_id: "base_link"
+    costmap_width: 20.0 # m
+    costmap_resolution: 0.2 # m/cell

aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp

@@ -0,0 +1,77 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_
+#define COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_
+
+#include "booars_utils/nav/occupancy_grid_parameters.hpp"
+
+#include <booars_utils/ros/function_timer.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace costmap_generator
+{
+
+class GlobalCostmapGenerator : public rclcpp::Node
+{
+  using FunctionTimer = booars_utils::ros::FunctionTimer;
+  using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+  using HADMapBinSubscription = rclcpp::Subscription<HADMapBin>;
+  using LinearRing2d = tier4_autoware_utils::LinearRing2d;
+  using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
+  using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters;
+  using OccupancyGridPublisher = rclcpp::Publisher<OccupancyGrid>;
+  using Point2d = tier4_autoware_utils::Point2d;
+  using Vector3 = geometry_msgs::msg::Vector3;
+
+public:
+  explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options);
+
+private:
+  void update();
+  lanelet::ConstLanelets get_intersected_lanelets(const Vector3 & costmap_center);
+  LinearRing2d get_costmap_contour(const Vector3 & costmap_center);
+  Point2d get_global_cell_position(const Vector3 & costmap_center, const int & index);
+
+  void map_callback(const HADMapBin::SharedPtr msg);
+
+  FunctionTimer::SharedPtr update_timer_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  HADMapBinSubscription::SharedPtr map_sub_;
+  lanelet::LaneletMapPtr map_;
+  lanelet::ConstLanelets roads_;
+
+  OccupancyGridPublisher::SharedPtr costmap_pub_;
+  OccupancyGrid::SharedPtr costmap_;
+  OccupancyGridParameters::SharedPtr costmap_parameters_;
+
+  std::string map_frame_;
+  std::string target_frame_;
+};
+}  // namespace costmap_generator
+
+#endif  // COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_

aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+  <group>
+    <arg name="global_costmap_generator_config_file" default="$(find-pkg-share costmap_generator)/config/costmap_generator/global_costmap_generator.param.yaml"/>
+
+    <push-ros-namespace namespace="costmap_generator"/>
+    <include file="$(find-pkg-share costmap_generator)/launch/costmap_generator/global_costmap_generator.launch.xml">
+      <arg name="config_file" value="$(var global_costmap_generator_config_file)"/>
+    </include>
+  </group>
+</launch>

aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <group>
+    <arg name="input_map_topic" default="/map/vector_map"/>
+    <arg name="output_costmap_topic" default="~/costmap"/>
+
+    <arg name="config_file" default="$(find-pkg-share costmap_generator)/config/costmap_generator/global_costmap_generator.param.yaml"/>
+
+    <node pkg="costmap_generator" name="global_costmap_generator" exec="global_costmap_generator_node" output="screen">
+      <remap from="~/input/map" to="$(var input_map_topic)"/>
+      <remap from="~/output/costmap" to="$(var output_costmap_topic)"/>
+      <param from="$(var config_file)"/>
+    </node>
+  </group>
+</launch>

aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml

@@ -0,0 +1,23 @@
+<?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>costmap_generator</name>
+  <version>0.0.0</version>
+  <description>The costmap_generator pkg</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>booars_utils</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>