Implented point to point DWM action server

mg_navigation/include/handlers/dwm_forward.hpp

@@ -0,0 +1,57 @@
+#pragma once
+#include "handlers/dwm_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
+        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
+        target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
+        constexpr double delta = 0.8;
+
+        auto [v, w] = to_vel(step * vl, step * vr);
+
+        const glm::dvec2 dp    = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
+        const glm::dvec2 n_pos = dp + pos;
+
+        const double score = goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
+                                                                           std::vector<int>& vr,
+                                                                           int               dimx,
+                                                                           int) {
+        vl.clear();
+        vr.clear();
+
+        for (int i = -dimx / 2; i <= dimx / 2; i++) {
+            auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
+
+            if (step * abs(cvl + i) <= goal->max_wheel_speed && glm::abs(v) < goal->max_vel) {
+                vl.push_back(cvl + i);
+                vr.push_back(cvr + i);
+            }
+        }
+    }
+
+}