Add a-star based global planner

mg_navigation/include/path_buffer.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <vector>
+#include <mutex>
+#include <thread>
+#include <glm/glm.hpp>
+
+#include "mg_msgs/srv/calc_path.hpp"
+#include "mg_msgs/action/move_global.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#define AREAX  3000
+#define AREAY  2000
+#define GRID_Y 66
+#define GRID_X 106
+
+#define MIN_X 175
+#define MIN_Y 175
+
+#define MAX_X 2825
+#define MAX_Y 1825
+
+#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
+#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
+
+namespace mg {
+    inline glm::vec2 GridToCoords(const glm::ivec2 grid) {
+        return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y };
+    }
+    inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
+    inline glm::vec2  IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); }
+
+    class PathBuffer {
+        using PathService = mg_msgs::srv::CalcPath;
+        using PathGoal    = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr;
+        using PathFuture  = rclcpp::Client<PathService>::SharedFuture;
+
+      public:
+        PathBuffer(rclcpp::Node* node);
+        // ~PathBuffer();
+
+        glm::vec2 get_next(glm::vec2 pos, const double lookahead);
+
+        bool update_path(PathGoal goal = nullptr);
+        void update_path_block(PathGoal goal = nullptr);
+
+      private:
+        int current;
+
+        rclcpp::Node* node_;
+
+        PathGoal                              goal_;
+        PathService::Response::ConstSharedPtr path_msg_;
+
+        rclcpp::Client<PathService>::Client::SharedPtr client_;
+
+        PathFuture resp_;
+    };
+
+}