Add a-star based global planner

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,41 @@
+#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/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;
+
+        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_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);
+    }
+}