feat: add costmap 2d

navyu_costmap_2d/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.5)
+project(navyu_costmap_2d)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  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(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "NavyuCostmap2D"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+  rviz
+)
+
+install(FILES DESTINATION share/${PROJECT_NAME})

navyu_costmap_2d/config/navyu_costmap_2d_params.yaml

@@ -0,0 +1,11 @@
+global_costmap_node:
+  ros__parameters:
+    update_frequency: 1.0
+    base_frame_id: "base_link"
+    map_frame_id: "map"
+    plugins: ["static_layer", "inflation_layer"]
+    inflation_layer:
+      inflation_radius: 0.55
+      robot_radius: 0.22
+    static_layer:
+      map_topic: "map"

navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp

@@ -0,0 +1,44 @@
+#ifndef NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_
+#define NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_
+
+#include "navyu_costmap_2d/plugins/layer.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+class NavyuCostmap2D : public rclcpp::Node
+{
+public:
+  explicit NavyuCostmap2D(const rclcpp::NodeOptions & node_options);
+  ~NavyuCostmap2D();
+
+  void update();
+
+private:
+  geometry_msgs::msg::TransformStamped get_transform(
+    const std::string target_frame, const std::string source_frame);
+
+private:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::vector<std::string> plugins_;
+  std::map<std::string, std::shared_ptr<Layer>> layer_function_;
+
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+
+  double update_frequency_;
+  std::string base_frame_id_;
+  std::string map_frame_id_;
+};
+
+#endif  // NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_

navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp

@@ -0,0 +1,77 @@
+#ifndef NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_
+#define NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_
+
+#include "navyu_costmap_2d/plugins/layer.hpp"
+
+class InflationLayer : public Layer
+{
+public:
+  explicit InflationLayer(rclcpp::Node * node) : Layer(node) { node_ = node; }
+
+  void initialize() override
+  {
+    node_->declare_parameter<double>("inflation_layer.inflation_radius");
+    node_->get_parameter("inflation_layer.inflation_radius", inflation_radius_);
+    node_->declare_parameter<double>("inflation_layer.robot_radius");
+    node_->get_parameter("inflation_layer.robot_radius", robot_radius_);
+  }
+
+  void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override
+  {
+    if (master_costmap == nullptr) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is nullptr.");
+      return;
+    }
+
+    const int size_x = master_costmap->info.width;
+    const int size_y = master_costmap->info.height;
+
+    for (int map_y = 0; map_y < size_y; map_y++) {
+      for (int map_x = 0; map_x < size_x; map_x++) {
+        int map_index = map_x + size_x * map_y;
+
+        if (master_costmap->data[map_index] == LETHAL_COST) {
+          expand_cost(master_costmap, map_x, map_y);
+        }
+      }
+    }
+  }
+
+  void expand_cost(nav_msgs::msg::OccupancyGrid::SharedPtr & costmap, const int mx, const int my)
+  {
+    const int size_x = costmap->info.width;
+    const double resolution = costmap->info.resolution;
+    const int cell_inflation_radius = std::ceil(inflation_radius_ / resolution);
+
+    const int min_x = mx - cell_inflation_radius;
+    const int max_x = mx + cell_inflation_radius;
+    const int min_y = my - cell_inflation_radius;
+    const int max_y = my + cell_inflation_radius;
+
+    for (int y = min_y; y < max_y; y++) {
+      for (int x = min_x; x < max_x; x++) {
+        const double distance = std::hypot(x - mx, y - my);
+        const int index = x + size_x * y;
+        const auto old_cost = costmap->data[index];
+
+        if (distance * resolution < robot_radius_) {
+          costmap->data[index] = std::max(INSCRIBED_COST, old_cost);
+        } else {
+          if (cell_inflation_radius < distance) continue;
+          if (costmap->data[index] == -1) continue;
+          const int8_t new_cost =
+            std::exp(-1 * 3.0 * (distance * resolution - robot_radius_)) * INSCRIBED_COST - 1;
+          costmap->data[index] = std::max(new_cost, old_cost);
+        }
+      }
+    }
+  }
+
+private:
+  rclcpp::Node * node_;
+
+  double inflation_radius_;
+  double robot_radius_;
+};
+
+#endif  // NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_

navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp

@@ -0,0 +1,23 @@
+#ifndef NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_
+#define NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+static constexpr int8_t LETHAL_COST = 100;
+static constexpr int8_t INSCRIBED_COST = 99;
+
+class Layer
+{
+public:
+  explicit Layer(rclcpp::Node * node) { node_ = node; }
+  virtual ~Layer() {}
+  virtual void initialize() = 0;
+  virtual void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) = 0;
+
+private:
+  rclcpp::Node * node_;
+};
+
+#endif  // NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_

navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp

@@ -0,0 +1,40 @@
+#ifndef NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_
+#define NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_
+
+#include "navyu_costmap_2d/plugins/layer.hpp"
+
+#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
+
+class StaticLayer : public Layer
+{
+public:
+  explicit StaticLayer(rclcpp::Node * node) : Layer(node) { node_ = node; }
+
+  void initialize() override
+  {
+    rclcpp::QoS qos(10);
+    qos.transient_local();
+    qos.reliable();
+    qos.keep_last(1);
+
+    const std::string map_topic = node_->declare_parameter<std::string>("static_layer.map_topic");
+    map_subscriber_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
+      map_topic, qos, std::bind(&StaticLayer::callback_map, this, std::placeholders::_1));
+  }
+
+  void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { costmap_ = msg; }
+
+  void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override
+  {
+    master_costmap = costmap_;
+  }
+
+private:
+  rclcpp::Node * node_;
+
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_subscriber_;
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr costmap_;
+};
+
+#endif  // NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_

navyu_costmap_2d/launch/navyu_costmap_2d.launch.py

@@ -0,0 +1,58 @@
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration("use_sim_time", default="true")
+
+    rviz_config_path = PathJoinSubstitution(
+        [FindPackageShare("navyu_costmap_2d"), "rviz", "costmap_test.rviz"]
+    )
+
+    costmap_map_config_path = PathJoinSubstitution(
+        [FindPackageShare("navyu_costmap_2d"), "config", "navyu_costmap_2d_params.yaml"]
+    )
+
+    map_path = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "map", "map.yaml"])
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="true"),
+            Node(
+                package="navyu_costmap_2d",
+                executable="navyu_costmap_2d_node",
+                name="global_costmap_node",
+                output="screen",
+                parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}],
+            ),
+            Node(
+                package="nav2_map_server",
+                executable="map_server",
+                name="map_server",
+                parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}],
+            ),
+            Node(
+                package="nav2_lifecycle_manager",
+                executable="lifecycle_manager",
+                name="map_server_lifecycle_manager",
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {"use_sim_time": True},
+                    {"autostart": True},
+                    {"node_names": ["map_server"]},
+                ],
+            ),
+            Node(
+                package="rviz2",
+                executable="rviz2",
+                output="screen",
+                arguments=["-d", rviz_config_path],
+            ),
+        ]
+    )

navyu_costmap_2d/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>navyu_costmap_2d</name>
+  <version>0.1.0</version>
+  <description>The navyu_costmap_2d package</description>
+  <maintainer email="[email protected]">RyuYamamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <exec_depend>launch_ros</exec_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>