Add global path planner (#12)

Merged global-planner with main

mg_msgs/CMakeLists.txt

@@ -10,11 +10,14 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Path.msg"
   "msg/Point2d.msg"
+  "action/MoveGlobal.action"
   "action/MoveStraight.action"
   "action/MovePoint.action"
   "action/LookAt.action"
   "action/Rotate.action"
+  "srv/CalcPath.srv"
   "srv/SendDouble.srv"
 )
 

mg_msgs/action/MoveGlobal.action

@@ -0,0 +1,25 @@
+float64[] x 
+float64[] y 
+
+float64 step 0.01
+float64 tolerance 0.5
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/msg/Path.msg

@@ -0,0 +1 @@
+int32[] indicies

mg_msgs/srv/CalcPath.srv

@@ -0,0 +1,4 @@
+float64[] x
+float64[] y
+---
+int32[] indicies

mg_navigation/CMakeLists.txt

@@ -30,6 +30,7 @@ set(PACKAGE_DEPS
 
 add_executable(mg_nav_server 
   src/mg_navigation.cpp
+  src/path_buffer.cpp
 )
 ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})
 

mg_navigation/include/handlers/dwa.hpp

@@ -2,6 +2,7 @@
 
 #include "handlers/dwa_core.hpp"
 #include "handlers/dwa_point.hpp"
+#include "handlers/dwa_global.hpp"
 #include "handlers/dwa_forward.hpp"
 #include "handlers/dwa_lookat.hpp"
 #include "handlers/dwa_rotate.hpp"

mg_navigation/include/handlers/dwa_core.hpp

@@ -109,6 +109,7 @@ namespace mg {
         rclcpp::Rate rate(UPDATE_RATE);
 
         while (target_check() && rclcpp::ok()) {
+            target_update();
             if (hgoal->is_canceling()) {
                 cancel();
                 return;

mg_navigation/include/handlers/dwa_global.hpp

@@ -0,0 +1,89 @@
+#pragma once
+#include "handlers/dwa_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+
+#define LOOKAHEAD 0.2
+
+namespace mg {
+
+    template <>
+    inline bool DWA<MgNavigationServer::MoveGlobal>::target_check() {
+        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
+    }
+
+    template <>
+    inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
+        baseNode.path_buffer()->update_path_block(goal);
+        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
+    }
+
+    template <>
+    inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
+        baseNode.path_buffer()->update_path();
+        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
+    }
+
+    template <>
+    inline bool DWA<MgNavigationServer::MoveGlobal>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWA<MgNavigationServer::MoveGlobal>::calc_score(int vl, int vr) {
+        auto [v, w]        = to_vel(step * vl, step * vr);
+        const double delta = 0.5;
+        glm::dvec2   n_pos;
+        double       n_theta = NAN;
+        double       score   = 0;
+
+        n_theta = w * delta;
+
+        if (n_theta <= 1e-6) { //NOLINT
+            n_theta += theta;
+            const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
+            n_pos               = dp + pos;
+        } else {
+            n_theta += theta;
+            const double r = v / w;
+            n_pos.x        = r * (glm::sin(n_theta) - glm::sin(theta));
+            n_pos.y        = -r * (glm::cos(n_theta) - glm::cos(theta));
+            n_pos += pos;
+        }
+
+        const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
+        const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);
+
+        const double angl   = glm::angle(face, glm::normalize(target_pos - pos));
+        const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
+
+        score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
+        score += goal->ornt_mult * (angl - n_angl);
+
+        return score;
+    }
+
+    template <>
+    inline void DWA<MgNavigationServer::MoveGlobal>::populate_candidates(std::vector<int>& vl,
+                                                                         std::vector<int>& vr,
+                                                                         int               dimx,
+                                                                         int               dimy) {
+        vl.clear();
+        vr.clear();
+
+        for (int i = -dimx / 2; i <= dimx / 2; i++) {
+            for (int j = -dimy / 2; j <= dimy / 2; j++) {
+                auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j));
+
+                if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed
+                    && glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) {
+                    vl.push_back(cvl + i);
+                    vr.push_back(cvr + j);
+                }
+            }
+        }
+    }
+
+}

mg_navigation/include/mg_navigation.hpp

@@ -17,6 +17,8 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/buffer.h"
 
+#include "path_buffer.hpp"
+
 namespace mg {
 
     namespace Geometry = geometry_msgs::msg;
@@ -25,12 +27,14 @@ namespace mg {
       public:
         RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
         using MovePoint    = mg_msgs::action::MovePoint;
+        using MoveGlobal   = mg_msgs::action::MoveGlobal;
         using MoveStraight = mg_msgs::action::MoveStraight;
         using LookAt       = mg_msgs::action::LookAt;
         using Rotate       = mg_msgs::action::Rotate;
 
         rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
 
+        rclcpp_action::Server<MoveGlobal>::SharedPtr   sv_move_global;
         rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
         rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
         rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
@@ -43,9 +47,13 @@ namespace mg {
 
         MgNavigationServer(rclcpp::NodeOptions& opts);
 
+        std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
+
       private:
         bool is_processing = false;
 
+        std::shared_ptr<PathBuffer> path_buffer_;
+
         template <typename T>
         rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
                                                 typename T::Goal::ConstSharedPtr,

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_;
+    };
+
+}

mg_navigation/src/mg_navigation.cpp

@@ -50,9 +50,19 @@ namespace mg {
         tf2_buffer   = std::make_shared<tf2_ros::Buffer>(get_clock());
         tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
 
+        path_buffer_ = std::make_shared<PathBuffer>(this);
+
         pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
 
         using namespace std::placeholders;
+
+        sv_move_global = rclcpp_action::create_server<MoveGlobal>(
+          this,
+          "MoveGlobal",
+          std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
+          std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
+          std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));
+
         sv_move_point = rclcpp_action::create_server<MovePoint>(
           this,
           "MovePoint",

mg_navigation/src/path_buffer.cpp

@@ -0,0 +1,80 @@
+#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();
+        }
+
+        resp_.wait();
+        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;
+    }
+}

mg_planner/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_planner)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(mg_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include(FindPkgConfig)
+pkg_search_module(LIBGLM REQUIRED glm)
+
+set(PACKAGE_DEPS
+  rclcpp
+  mg_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+add_executable(mg_planner
+  src/mg_planner.cpp
+  src/mg_planner_node.cpp
+  src/obstacleManager.cpp
+  src/a_star.cpp
+)
+
+ament_target_dependencies(mg_planner ${PACKAGE_DEPS})
+
+target_include_directories(
+  mg_planner
+  PRIVATE
+  ${LIBGLM_INCLUDE_DIRS}
+  include
+)
+
+target_link_libraries(
+  mg_planner
+  ${LIBGLM_LIBRARIES}
+)
+
+install( TARGETS
+  mg_planner
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+target_compile_features(mg_planner PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_package()
+

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,43 @@
+#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/msg/path.hpp"
+#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;
+        rclcpp::Publisher<mg_msgs::msg::Path>::SharedPtr   path_pub_;
+
+        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);
+    };
+}

mg_planner/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_planner</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>mg_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  
+
+  <build_depend>libglm-dev</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_planner/src/a_star.cpp

@@ -0,0 +1,93 @@
+#include "mg_planner/a_star/a_star.hpp"
+#include "mg_planner/config.hpp"
+#include "mg_planner/mg_planner_node.hpp"
+#include <array>
+#include <cmath>
+#include <codecvt>
+#include <functional>
+#include <glm/fwd.hpp>
+#include <glm/geometric.hpp>
+#include <iterator>
+#include <memory>
+#include <queue>
+#include <rclcpp/logging.hpp>
+#include <rcutils/types/rcutils_ret.h>
+#include <unordered_set>
+#include <vector>
+
+namespace mg {
+
+    void AStar::execute(glm::ivec2 pos, std::vector<glm::ivec2>&& goals, std::vector<int>& path) {
+        std::
+          priority_queue<std::pair<float, int>, std::vector<std::pair<float, int>>, std::greater<std::pair<float, int>>>
+                                             pq_tasks;
+        std::array<bool, (GRID_X * GRID_Y)>  visited{};
+        std::array<int, (GRID_X * GRID_Y)>   parent{};
+        std::array<float, (GRID_X * GRID_Y)> total_distance{};
+
+        targets_         = std::move(goals);
+        const int posIdx = CoordsToId(pos);
+
+        RCLCPP_INFO(node_->get_logger(), "Got pos: %d %d", pos.x, pos.y);
+
+        pq_tasks.emplace(0, posIdx);
+        visited.at(posIdx) = true;
+        node_->fill_obstacles();
+
+        while (!pq_tasks.empty()) {
+            const auto [score, idx]   = pq_tasks.top();
+            const glm::ivec2 grid_loc = IdToGrid(idx);
+            pq_tasks.pop();
+
+            RCLCPP_DEBUG(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y);
+
+            for (int i = 0; i < 4; i++) {
+                for (int j = 0; j < 9; j++) {
+                    const glm::ivec2 gridy = IdToGrid(idx) + neighbors[j] * directions[i];
+                    const int        idy   = GridToId(gridy);
+                    //RCLCPP_INFO(node_->get_logger(), "Checking [idx, pos]: %d, %d;%d", idy, gridy.x, gridy.y);
+
+                    if (node_->getObstacleManager().check_collision(GridToCoords(gridy), 320)) {
+                        if (!visited[idy]) {
+                            parent[idy]         = idx;
+                            visited[idy]        = true;
+                            total_distance[idy] = distances[j] + total_distance[idx];
+
+                            RCLCPP_DEBUG(node_->get_logger(), "Gridy: %d %d", gridy.x, gridy.y);
+                            RCLCPP_DEBUG(node_->get_logger(), "idy: %d", idy);
+                            RCLCPP_DEBUG(node_->get_logger(), "Distance: %f", total_distance[idy]);
+                            pq_tasks.emplace(total_distance[idy] + dist_to_goal(gridy), idy);
+                            for (const auto& goal : targets_) {
+                                if (goal == gridy) {
+                                    gen_path(posIdx, parent, idy, path);
+                                    return;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    void AStar::gen_path(int start, std::array<int, (GRID_X * GRID_Y)>& parents, int end, std::vector<int>& path) {
+
+        int curr = end;
+
+        while (curr != start) {
+            path.push_back(curr);
+            curr = parents[curr];
+        }
+
+        path.push_back(start);
+    }
+
+    float AStar::dist_to_goal(glm::ivec2 pos) {
+        float mini = INFINITY;
+        for (const auto& goal : targets_) {
+            // Find closest of multiple targets
+            mini = glm::min(glm::distance((glm::vec2)goal, (glm::vec2)pos), mini);
+        }
+        return mini;
+    }
+}

mg_planner/src/mg_planner.cpp

@@ -0,0 +1,11 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "mg_planner/mg_planner_node.hpp"
+#include <rclcpp/executors.hpp>
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<mg::PlannerNode>());
+
+    return 0;
+}

mg_planner/src/mg_planner_node.cpp

@@ -0,0 +1,63 @@
+#include "mg_planner/mg_planner_node.hpp"
+#include "mg_msgs/msg/t_edit_cycle.hpp"
+#include "mg_msgs/srv/calc_path.hpp"
+#include "mg_planner/a_star/a_star.hpp"
+#include "mg_planner/config.hpp"
+#include <array>
+#include <exception>
+#include <iostream>
+#include <memory>
+#include <rclcpp/time.hpp>
+#include <rclcpp/wait_result_kind.hpp>
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/convert.hpp"
+
+mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
+    astar_service = create_service<mg_msgs::srv::CalcPath>(
+      "GeneratePath",
+      [this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
+             mg_msgs::srv::CalcPath::Response::SharedPtr     resp) -> void {
+          std::cout << "Bro recieved request\n";
+          auto                    elapsed = get_clock()->now();
+          std::vector<glm::ivec2> goals;
+          std::transform(req->x.begin(),
+                         req->x.end(),
+                         req->y.begin(),
+                         std::back_inserter(goals),
+                         [](const auto& aa, const auto& bb) {
+                             std::cout << aa << " " << bb << "\n";
+                             return AStar::CoordsToGrid(glm::ivec2(aa * 1000, bb * 1000));
+                         });
+          for (const auto& goal : goals) {
+              if (goal.x < 0 || GRID_X < goal.x || goal.y < 0 || GRID_Y < goal.y) {
+                  return;
+              }
+          }
+          astar_.execute(get_pos(), std::move(goals), resp->indicies);
+          mg_msgs::msg::Path path;
+          path.indicies = resp->indicies;
+          path_pub_->publish(path);
+          RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
+      });
+
+    path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));
+
+    tf_buf_     = std::make_shared<tf2_ros::Buffer>(get_clock());
+    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
+}
+
+glm::ivec2 mg::PlannerNode::get_pos() {
+    try {
+        auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+
+        tf2::Transform t;
+        tf2::convert(tmsg.transform, t);
+        auto vec3 = tmsg.transform.translation;
+
+        return glm::clamp(
+          glm::ivec2{ vec3.x * 1000, vec3.y * 1000 }, glm::ivec2(MIN_X, MIN_Y), glm::ivec2(MAX_X, MAX_Y));
+
+    } catch (const std::exception& e) { RCLCPP_DEBUG(get_logger(), "Failed to find transform, ohno"); }
+    return { 1000, 1000 };
+}

mg_planner/src/obstacleManager.cpp

@@ -0,0 +1,8 @@
+#include "mg_planner/obstacleManager.hpp"
+#include "mg_planner/config.hpp"
+
+namespace mg {
+    bool ObstacleManager::check_collision(glm::ivec2 pos, float size) {
+        return (pos.x >= MIN_X && pos.x < MAX_X) && (pos.y >= MIN_Y && pos.y < MAX_Y);
+    }
+}