Add a-star based global planner

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_INFO(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,58 @@
+#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);
+          RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
+      });
+
+    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);
+    }
+}