Added dwm rotate

mg_navigation/include/handlers/dwm.hpp

@@ -4,3 +4,4 @@
 #include "handlers/dwm_point.hpp"
 #include "handlers/dwm_forward.hpp"
 #include "handlers/dwm_lookat.hpp"
+#include "handlers/dwm_rotate.hpp"

mg_navigation/include/handlers/dwm_rotate.hpp

@@ -0,0 +1,66 @@
+#pragma once
+#include "handlers/dwm_core.hpp"
+
+#include "glm/gtx/norm.hpp"
+#include "glm/gtx/rotate_vector.hpp"
+#include "glm/gtx/vector_angle.hpp"
+#include <glm/gtc/constants.hpp>
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::Rotate>::target_check() {
+
+        const double a = glm::abs(theta - target_ornt);
+        const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
+
+        return b > goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::Rotate>::target_init() {
+        target_ornt = goal->angle;
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
+        constexpr double delta = 0.5;
+        const auto [v, w]      = to_vel(step * vl, step * vr);
+        const double n_theta   = theta + w * delta;
+
+        double dist_old = glm::abs(target_ornt - theta);
+        double dist_new = glm::abs(target_ornt - n_theta);
+
+        dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
+        dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
+
+        const double score = goal->ornt_mult * (dist_old - dist_new);
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::Rotate>::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 && step * abs(cvr + i) <= goal->max_wheel_speed
+                && glm::abs(w) < goal->max_angular) {
+                vl.push_back(cvl - i);
+                vr.push_back(cvr + i);
+            }
+        }
+    }
+
+}