Add a-star based global planner

mg_planner/include/mg_planner/a_star/a_star.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "mg_planner/config.hpp"
+#include "rclcpp/node.hpp"
+
+#include <glm/fwd.hpp>
+#include <glm/glm.hpp>
+#include <glm/gtx/fast_square_root.hpp>
+#include <memory>
+#include <queue>
+#include <rclcpp/parameter.hpp>
+#include <vector>
+
+#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
+#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
+
+namespace mg {
+
+    class PlannerNode;
+
+    constexpr std::array<glm::ivec2, 9> neighbors{ glm::ivec2(1, 0), glm::ivec2(1, 1), glm::ivec2(1, 2),
+                                                   glm::ivec2(1, 3), glm::ivec2(2, 1), glm::ivec2(2, 3),
+                                                   glm::ivec2(3, 1), glm::ivec2(3, 2), glm::ivec2(0, 1) };
+
+    constexpr std::array<glm::ivec2, 4> directions{
+        glm::ivec2(1, 1), glm::ivec2(1, -1), glm::ivec2(-1, -1), glm::ivec2(-1, 1)
+    };
+
+    static inline std::array<float, 9> precalc_Distances() {
+        std::array<float, 9> dist{};
+        for (int i = 0; i < 9; i++) { dist[i] = glm::fastLength((glm::vec2)neighbors[i]); }
+        return dist;
+    }
+
+    const std::array<float, 9> distances = precalc_Distances();
+
+    class AStar {
+      public:
+        friend mg::PlannerNode;
+        AStar()        = delete;
+        AStar(AStar&&) = delete;
+        AStar(AStar&)  = delete;
+
+        static glm::ivec2 CoordsToGrid(const glm::ivec2 pos) {
+            return (pos - glm::ivec2{ MIN_X, MIN_Y }) / glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE };
+        }
+
+        static glm::ivec2 GridToCoords(const glm::ivec2 grid) {
+            return grid * glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::ivec2{ MIN_X, MIN_Y };
+        }
+
+        static glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
+        static int        GridToId(const glm::ivec2 grid) { return grid.x + GRID_X * grid.y; }
+
+        static glm::ivec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); }
+        static int        CoordsToId(const glm::ivec2 pos) { return GridToId(CoordsToGrid(pos)); }
+
+        static void gen_path(int start, std::array<int, (GRID_X * GRID_Y)>& parents, int end, std::vector<int>& path);
+        float       dist_to_goal(glm::ivec2 pos);
+
+        void execute(glm::ivec2 pos, std::vector<glm::ivec2>&& goals, std::vector<int>& path);
+
+        bool isRunning() const { return running; }
+
+      private:
+        AStar(PlannerNode* node) : node_(node) { }
+
+        bool                    running;
+        PlannerNode*            node_;
+        std::vector<glm::ivec2> targets_;
+    };
+}

mg_planner/include/mg_planner/config.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#define AREAX  3000
+#define AREAY  2000
+#define GRID_Y 66ul
+#define GRID_X 106ul
+
+#define MIN_X 175
+#define MIN_Y 175
+
+#define MAX_X 2825
+#define MAX_Y 1825
+
+#define SEARCH_DISTANCE 5
+
+#define CLOSE       100000
+#define MAXOBSTC    50
+#define CLOSEDIST   110
+#define GOALDELTA2  2500
+#define ROBOTRADIUS 165
+
+#define ELIPSEPERCENT 0.5
+#define GOALPERCENT   0.1
+#define MAXDTIME      0.1

mg_planner/include/mg_planner/mg_planner_node.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <array>
+#include <glm/fwd.hpp>
+#include <memory>
+#include <rclcpp/client.hpp>
+#include <rclcpp/service.hpp>
+#include "mg_planner/a_star/a_star.hpp"
+#include "mg_planner/obstacleManager.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "mg_msgs/srv/calc_path.hpp"
+
+#define TREE_SIZE 9
+
+namespace mg {
+
+    class PlannerNode : public rclcpp::Node {
+      public:
+        PlannerNode();
+
+        void fill_obstacles() { }
+
+        ObstacleManager& getObstacleManager() { return obstacle_manager_; }
+
+        glm::ivec2 get_pos();
+
+      private:
+        rclcpp::Service<mg_msgs::srv::CalcPath>::SharedPtr astar_service;
+
+        AStar           astar_;
+        ObstacleManager obstacle_manager_;
+
+        std::shared_ptr<tf2_ros::Buffer>            tf_buf_;
+        std::shared_ptr<tf2_ros::TransformListener> tf_listener;
+    };
+
+}

mg_planner/include/mg_planner/obstacleManager.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <glm/glm.hpp>
+#include <vector>
+
+namespace mg {
+    class ObstacleManager {
+        const std::vector<glm::vec2> static_obstacles_;
+        const std::vector<glm::vec2> permanent_obstacles_;
+
+        glm::vec2 dynamic_obstacle_;
+
+      public:
+        bool check_collision(glm::ivec2 pos, float size);
+    };
+}