feat(planning): create multi-layered costmap system (#21)

* create occupancy_grid utils Signed-off-by: Autumn60 <[email protected]> * add update_origin func to occupancy_grid_utils.hpp Signed-off-by: Autumn60 <[email protected]> (cherry picked from commit 96000cbb96377321fee819f19033eea061c37c3c) * create multi_layered_costmap and costmap_base Signed-off-by: Autumn60 <[email protected]> * create lanelet_costmap Signed-off-by: Autumn60 <[email protected]> * add dummy subscriber to lanelet_costmap Signed-off-by: Autumn60 <[email protected]> * fix license notation Signed-off-by: Autumn60 <[email protected]> * fix depend Signed-off-by: Autumn60 <[email protected]> * replace throw to return Signed-off-by: Autumn60 <[email protected]> * create predicted_object_costmap Signed-off-by: Autumn60 <[email protected]> * add create_costmap func to lanelet_costmap Signed-off-by: Autumn60 <[email protected]> * add create_costmap func to predicted_object_costmap Signed-off-by: Autumn60 <[email protected]> * add SharedPtr to MultiLayeredCostmap Signed-off-by: Autumn60 <[email protected]> * create booars_costmap_utils Signed-off-by: Autumn60 <[email protected]> * create multi_layered_costmap_visualizer Signed-off-by: Autumn60 <[email protected]> * fix virtual function Signed-off-by: Autumn60 <[email protected]> * add lanelet_costmap initialization Signed-off-by: Autumn60 <[email protected]> * add get_parameter func for costmap layers Signed-off-by: Autumn60 <[email protected]> * fix param.yaml Signed-off-by: Autumn60 <[email protected]> * delete unnecessary space Signed-off-by: Autumn60 <[email protected]> * rename pkg Signed-off-by: Autumn60 <[email protected]> * update costmap_generator Signed-off-by: Autumn60 <[email protected]> * add get_index_to_table func to occupancy_grid_utils Signed-off-by: Autumn60 <[email protected]> * fix MOROMORO Signed-off-by: Autumn60 <[email protected]> * update .param.yaml Signed-off-by: Autumn60 <[email protected]> * update CODEOWNERS Signed-off-by: Autumn60 <[email protected]> --------- Signed-off-by: Autumn60 <[email protected]>
booars · Aug 29, 2024 · 839feb6 · 839feb6
1 parent 32ebd37
commit 839feb6
Show file tree

Hide file tree

Showing 26 changed files with 1,050 additions and 0 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -2,6 +2,7 @@ aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booar
 aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/** @Autumn60
 aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60
 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60
+aichallenge/workspace/src/aichallenge_submit/costmap/** @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

diff --git a/...rc/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp b/...rc/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_
diff --git a/...ace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp b/...ace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp
@@ -0,0 +1,85 @@
+// 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()};
+}
+
+std::vector<tier4_autoware_utils::Point2d> get_index_to_point_table(
+  const OccupancyGridParameters::SharedPtr parameters)
+{
+  std::vector<tier4_autoware_utils::Point2d> table;
+  table.reserve(parameters->grid_num());
+  for (int i = 0; i < parameters->grid_num(); ++i) {
+    table.push_back(index_to_point(parameters, i));
+  }
+  return table;
+}
+
+}  // namespace booars_utils::nav::occupancy_grid_utils
+
+#endif  // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_
diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml
@@ -10,7 +10,9 @@
 
   <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>
 

diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(booars_costmap_generator)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+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(costmap_generator SHARED
+  src/costmap_generator.cpp
+)
+
+rclcpp_components_register_node(costmap_generator
+  PLUGIN "booars_costmap_generator::CostmapGenerator"
+  EXECUTABLE costmap_generator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)
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
@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    map_frame_id: "map"
+    costmap_center_frame_id: "base_link"
+    costmap_width: 20.0
+    costmap_resolution: 0.1
+    multi_layered_costmap:
+      layers:
+        - "lanelet_layer"
+        - "predicted_object_layer"
+      lanelet_layer:
+        type: "lanelet"
+        map_topic: "/map/vector_map"
+      predicted_object_layer:
+        type: "predicted_object"
+        predicted_objects_topic: "/perception/object_recognition/objects"
+        distance_threshold: 3.0
diff --git a/...t/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp b/...t/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp
@@ -0,0 +1,68 @@
+// 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_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
+#define BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
+
+#include <booars_costmap_utils/booars_costmap_utils.hpp>
+#include <booars_utils/nav/occupancy_grid_parameters.hpp>
+#include <booars_utils/ros/function_timer.hpp>
+#include <multi_layered_costmap/multi_layered_costmap.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace booars_costmap_generator
+{
+
+using FunctionTimer = booars_utils::ros::FunctionTimer;
+using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap;
+using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
+using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters;
+using PointStamped = geometry_msgs::msg::PointStamped;
+using Point2d = tier4_autoware_utils::Point2d;
+using Vector3 = geometry_msgs::msg::Vector3;
+
+class CostmapGenerator : public rclcpp::Node
+{
+public:
+  explicit CostmapGenerator(const rclcpp::NodeOptions & options);
+
+private:
+  void update();
+  bool try_get_transform(geometry_msgs::msg::TransformStamped & transform);
+
+  FunctionTimer::SharedPtr update_timer_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  rclcpp::Publisher<OccupancyGrid>::SharedPtr costmap_pub_;
+  MultiLayeredCostmap::SharedPtr multi_layered_costmap_;
+
+  OccupancyGrid::SharedPtr costmap_;
+  OccupancyGridParameters::SharedPtr costmap_parameters_;
+
+  std::string map_frame_;
+  std::string target_frame_;
+
+  std::vector<Point2d> index_to_point_table_;
+};
+};  // namespace booars_costmap_generator
+
+#endif  // BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
diff --git a/...c/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/...c/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
@@ -0,0 +1,7 @@
+<launch>
+  <arg name="config_file" default="$(find-pkg-share booars_costmap_generator)/config/costmap_generator.param.yaml"/>
+
+  <node pkg="booars_costmap_generator" exec="costmap_generator_node" name="costmap_generator" output="screen">
+    <param from="$(var config_file)"/>
+  </node>
+</launch>
diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>booars_costmap_generator</name>
+  <version>0.1.0</version>
+  <description>The booars_costmap_generator package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>booars_costmap_utils</depend>
+  <depend>booars_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>