Add a-star based global planner

mg_navigation/src/path_buffer.cpp

@@ -0,0 +1,78 @@
+#include "path_buffer.hpp"
+
+#include <glm/gtx/norm.hpp>
+
+namespace mg {
+
+    PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
+        client_ = node_->create_client<PathService>("GeneratePath");
+    }
+
+    glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) {
+        if (current < 0) {
+            current++;
+            while (current < (int)path_msg_->indicies.size()
+                   && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) {
+                current++;
+            }
+        }
+        while (current < (int)path_msg_->indicies.size()
+               && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
+            current++;
+        }
+
+        if (current == (int)path_msg_->indicies.size()) {
+            double best = 10000000;
+            int    id   = 0;
+
+            for (int i = 0; i < (int)goal_->x.size(); i++) {
+                const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos);
+                if (dist2 < best) {
+                    best = dist2;
+                    id   = i;
+                }
+            }
+            return { goal_->x[id], goal_->y[id] };
+        }
+        return IdToCoords(path_msg_->indicies[current]);
+    }
+
+    bool PathBuffer::update_path(PathGoal goal) {
+        if (goal) {
+            goal_ = goal;
+            client_->prune_pending_requests();
+            auto req = std::make_shared<PathService::Request>();
+            req->x   = goal_->x;
+            req->y   = goal_->y;
+            resp_    = client_->async_send_request(req).share();
+        }
+        if (resp_.valid()) {
+            path_msg_ = resp_.get();
+            auto req  = std::make_shared<PathService::Request>();
+            req->x    = goal_->x;
+            req->y    = goal_->y;
+            resp_     = client_->async_send_request(req).share();
+            current   = -1;
+            return true;
+        }
+        return false;
+    }
+
+    void PathBuffer::update_path_block(PathGoal goal) {
+        if (goal) {
+            goal_ = goal;
+            client_->prune_pending_requests();
+            auto req = std::make_shared<PathService::Request>();
+            req->x   = goal_->x;
+            req->y   = goal_->y;
+            resp_    = client_->async_send_request(req).share();
+        }
+
+        path_msg_ = resp_.get();
+        auto req  = std::make_shared<PathService::Request>();
+        req->x    = goal_->x;
+        req->y    = goal_->y;
+        resp_     = client_->async_send_request(req).share();
+        current   = -1;
+    }
+}