Add a-star based global planner

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