Added collision avoidance to MoveGlobal routine

mg_obstacles/include/mg_obstacles/mg_obstacles.hpp

@@ -0,0 +1,60 @@
+#pragma once
+#include <string>
+#include <glm/glm.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include "mg_obstacles/permanent_obstacle.hpp"
+#include "mg_obstacles/static_obstacle.hpp"
+
+#include "std_msgs/msg/u_int64.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+
+#include "tf2_ros/buffer.h"
+
+namespace mg {
+    class ObstacleManager {
+        using StaticListMsg = std_msgs::msg::UInt64;
+        using DynamicPosMsg = geometry_msgs::msg::PointStamped;
+
+      public:
+        RCLCPP_SMART_PTR_DEFINITIONS(ObstacleManager)
+
+        enum ObstacleMask { // NOLINT
+            DYNAMIC   = 1,
+            STATIC    = 2,
+            MOVABLE   = 3,
+            PERMANENT = 4,
+            IMPORTANT = 5,
+            ALL       = 7,
+        };
+
+        ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf);
+        bool   contains(glm::dvec2 pos, double radius, uint mask = ObstacleMask::ALL);
+        double dist_to_nearest(glm::dvec2 pos, int mask = ObstacleMask::ALL);
+        double box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask = ObstacleMask::ALL);
+
+        static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat);
+        static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rotation);
+
+        void update_dynamic();
+        void update_static();
+
+      private:
+        rclcpp::Node* node_;
+
+        tf2_ros::Buffer::SharedPtr tf_buffer;
+
+        std::vector<StaticObstacle>    static_obstacles_;
+        std::vector<PermanentObstacle> permanent_obstacles_;
+
+        glm::dvec3 oponent_position_ = { 0, 0, 0.20 };
+        bool       openent_active_   = false;
+
+        rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
+
+        rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
+        rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
+
+        void load_permanent(Json::Value& json);
+        void load_static(Json::Value& json);
+    };
+}

mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <glm/glm.hpp>
+#include <jsoncpp/json/json.h>
+
+namespace mg {
+    class PermanentObstacle {
+      public:
+        /* Create a permanent obstacle from a json value */
+        PermanentObstacle(const Json::Value& json);
+
+        glm::dvec2 pos;
+        glm::dvec2 size;
+
+        double distance(glm::dvec2 position) const;
+        bool   contains(glm::dvec2 position, double radius) const;
+        double distanceBox(const glm::dvec2 position, const glm::dvec2 size, const glm::dmat2 rot_mat) const;
+    };
+
+}

mg_obstacles/include/mg_obstacles/sdf.hpp

@@ -0,0 +1,16 @@
+#include "glm/glm.hpp"
+
+namespace mg {
+    double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t);
+
+    double circleSdf(glm::dvec2 pos, glm::dvec2 t);
+
+    bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius);
+    bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius);
+
+    double boxToBox(const glm::dvec2 pos,
+                    const glm::dvec2 size,
+                    const glm::dvec2 t,
+                    const glm::dvec2 sizet,
+                    const glm::dmat2 rot_mat);
+}

mg_obstacles/include/mg_obstacles/static_obstacle.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <glm/glm.hpp>
+#include <jsoncpp/json/json.h>
+
+namespace mg {
+    class StaticObstacle {
+      public:
+        /* Create a static obstacle from a json value */
+        StaticObstacle(const Json::Value& json);
+
+        glm::dvec2 pos;
+
+        bool active = true;
+
+        bool horizontal;
+
+        static double width;
+        static double height;
+
+        double distance(glm::dvec2 position) const;
+        bool   contains(glm::dvec2 position, double radius) const;
+        double distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const;
+    };
+
+}