feat: add global planner

navyu_planner/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(navyu_planner)
+
+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(backward_ros REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/navyu_global_planner.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "NavyuGlobalPlanner"
+  EXECUTABLE navyu_global_planner_node
+)
+
+add_backward(navyu_global_planner_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
+)
+
+install(FILES DESTINATION share/${PROJECT_NAME})

navyu_planner/config/navyu_global_planner_params.yaml

@@ -0,0 +1,5 @@
+navyu_global_planner_node:
+  ros__parameters:
+    map_frame_id: "map"
+
+    lethal_costmap_threshold: 45

navyu_planner/include/navyu_planner/astar_planner.hpp

@@ -0,0 +1,91 @@
+#ifndef NAVYU_PLANNER__ASTAR_PLANNER_HPP_
+#define NAVYU_PLANNER__ASTAR_PLANNER_HPP_
+
+#include "navyu_planner/base_global_planner.hpp"
+#include "navyu_planner/node.hpp"
+
+#include <Eigen/Core>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <queue>
+#include <unordered_map>
+#include <vector>
+
+class AstarPlanner : public BaseGlobalPlanner
+{
+public:
+  AstarPlanner(int size_x, int size_y, double resolution, double origin_x, double origin_y)
+  : BaseGlobalPlanner(size_x, size_y, resolution, origin_x, origin_y)
+  {
+  }
+  ~AstarPlanner() {}
+
+  bool plan(Node2D * start_node, Node2D * goal_node, std::vector<Node2D *> & path)
+  {
+    auto compare = [](Node2D * n1, Node2D * n2) { return n1->f_ > n2->f_; };
+    std::priority_queue<Node2D *, std::vector<Node2D *>, decltype(compare)> open_list(compare);
+    std::unordered_map<int, Node2D *> close_list;
+
+    open_list.push(start_node);
+
+    while (!open_list.empty()) {
+      Node2D * current_node = open_list.top();
+
+      open_list.pop();
+
+      if (close_list.find(current_node->grid_index_) != close_list.end()) continue;
+
+      close_list.insert(std::make_pair(current_node->grid_index_, current_node));
+
+      if (goal_node->x_ == current_node->x_ and goal_node->y_ == current_node->y_) {
+        close_list.insert(std::make_pair(goal_node->grid_index_, goal_node));
+        path = find_path(close_list, start_node, goal_node);
+        return true;
+      }
+
+      std::vector<Node2D> motion_list = current_node->get_motion();
+      for (auto motion : motion_list) {
+        Node2D * node = new Node2D(
+          current_node->x_ + motion.x_, current_node->y_ + motion.y_, current_node->g_ + motion.g_,
+          current_node);
+        node->set_grid_index(get_grid_index(node->x_, node->y_));
+
+        // check obstacle
+        if (lethal_cost_threshold_ < costmap_[node->grid_index_]) {
+          continue;
+        }
+
+        // already exist in close list
+        if (close_list.find(node->grid_index_) != close_list.end()) {
+          continue;
+        }
+
+        // update heuristic cost of new node
+        node->cost(node, goal_node);
+
+        open_list.push(node);
+      }
+    }
+    return false;
+  }
+
+  std::vector<Node2D *> find_path(
+    std::unordered_map<int, Node2D *> node_list, Node2D * start, Node2D * goal)
+  {
+    std::vector<Node2D *> path;
+
+    Node2D * current = node_list.find(goal->grid_index_)->second;
+    while (current != NULL) {
+      path.emplace_back(current);
+      current = current->parent_;
+    }
+    path.emplace_back(start);
+
+    return path;
+  }
+};
+
+#endif  // NAVYU_PLANNER__ASTAR_PLANNER_HPP_

navyu_planner/include/navyu_planner/base_global_planner.hpp

@@ -0,0 +1,68 @@
+#ifndef NAVYU_PLANNER__BASE_GLOBAL_PLANNER_HPP_
+#define NAVYU_PLANNER__BASE_GLOBAL_PLANNER_HPP_
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+class BaseGlobalPlanner
+{
+public:
+  BaseGlobalPlanner(int size_x, int size_y, double resolution, double origin_x, double origin_y)
+  : size_x_(size_x),
+    size_y_(size_y),
+    resolution_(resolution),
+    origin_x_(origin_x),
+    origin_y_(origin_y)
+  {
+  }
+
+  void set_size(int size_x, int size_y)
+  {
+    size_x_ = size_x;
+    size_y_ = size_y;
+  }
+
+  void set_origin(double origin_x, double origin_y)
+  {
+    origin_x_ = origin_x;
+    origin_y_ = origin_y;
+  }
+
+  void set_resolution(double resolution) { resolution_ = resolution; }
+
+  void set_costmap(std::vector<int8_t> costmap) { costmap_ = costmap; }
+
+  void set_lethal_cost_threshold(int8_t lethal_cost_threshold)
+  {
+    lethal_cost_threshold_ = lethal_cost_threshold;
+  }
+
+  int get_grid_index(int x, int y) { return x + size_x_ * y; }
+
+  void convert_map_to_grid(double map_x, double map_y, int & grid_x, int & grid_y)
+  {
+    grid_x = static_cast<int>((map_x - origin_x_) / resolution_);
+    grid_y = static_cast<int>((map_y - origin_y_) / resolution_);
+  }
+
+  void convert_grid_to_map(int grid_x, int grid_y, double & map_x, double & map_y)
+  {
+    map_x = origin_x_ + grid_x * resolution_;
+    map_y = origin_y_ + grid_y * resolution_;
+  }
+
+protected:
+  // costmap parameter
+  int size_x_;
+  int size_y_;
+  double resolution_;
+  double origin_x_;
+  double origin_y_;
+
+  std::vector<int8_t> costmap_;
+
+  double lethal_cost_threshold_;
+};
+
+#endif  // NAVYU_PLANNER__BASE_GLOBAL_PLANNER_HPP_

navyu_planner/include/navyu_planner/navyu_global_planner.hpp

@@ -0,0 +1,43 @@
+#ifndef NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
+#define NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
+
+#include "navyu_planner/astar_planner.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/path.hpp>
+
+class NavyuGlobalPlanner : public rclcpp::Node
+{
+public:
+  explicit NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options);
+  ~NavyuGlobalPlanner();
+
+  bool plan(
+    const geometry_msgs::msg::Pose start, const geometry_msgs::msg::Pose goal,
+    std::vector<Node2D *> & path);
+
+  void callback_costmap(const nav_msgs::msg::OccupancyGrid & msg);
+  void callback_goal_pose(const geometry_msgs::msg::PoseStamped & msg);
+
+  void publish_path(std::vector<Node2D *> path);
+
+  void wait_for_costmap();
+
+private:
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_subscriber_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscriber_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
+
+  std::shared_ptr<AstarPlanner> planner_;
+
+  std::string map_frame_id_;
+
+  bool costmap_initialized_{false};
+};
+
+#endif  // NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_

navyu_planner/include/navyu_planner/node.hpp

@@ -0,0 +1,49 @@
+#ifndef NAVYU_PLANNER__NODE_HPP_
+#define NAVYU_PLANNER__NODE_HPP_
+
+#include <cmath>
+#include <vector>
+
+class Node2D
+{
+public:
+  Node2D(int x = 0, int y = 0, double g = 0.0, Node2D * parent = NULL)
+  : x_(x), y_(y), g_(g), h_(0.0), f_(0.0), parent_(parent)
+  {
+  }
+
+  void cost(Node2D * n1, Node2D * n2)
+  {
+    h_ = std::hypot(n1->x_ - n2->x_, n1->y_ - n2->y_);
+    f_ = h_ + g_;
+  }
+
+  void set_grid_index(int grid_index) { grid_index_ = grid_index; }
+
+  std::vector<Node2D> get_motion()
+  {
+    std::vector<Node2D> motion{
+      Node2D(1, 0, 1),
+      Node2D(0, 1, 1),
+      Node2D(-1, 0, 1),
+      Node2D(0, -1, 1),
+      Node2D(1, 1, std::sqrt(2)),
+      Node2D(1, -1, std::sqrt(2)),
+      Node2D(-1, 1, std::sqrt(2)),
+      Node2D(-1, -1, std::sqrt(2)),
+    };
+
+    return motion;
+  }
+
+public:
+  int x_;
+  int y_;
+  int grid_index_;
+  Node2D * parent_;
+  double g_;
+  double h_;
+  double f_;
+};
+
+#endif  // NAVYU_PLANNER__NODE_HPP_

navyu_planner/launch/navyu_global_planner.launch.py

@@ -0,0 +1,28 @@
+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")
+
+    navyu_global_planner_config = PathJoinSubstitution(
+        [FindPackageShare("navyu_planner"), "config", "navyu_global_planner_params.yaml"]
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="true"),
+            Node(
+                package="navyu_planner",
+                executable="navyu_global_planner_node",
+                name="navyu_global_planner_node",
+                output="screen",
+                parameters=[navyu_global_planner_config, {"use_sim_time": use_sim_time}],
+            ),
+        ]
+    )

navyu_planner/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>navyu_planner</name>
+  <version>0.1.0</version>
+  <description>The navyu_planner 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>nav2_costmap_2d</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</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>