From 43da8a3a37ef1be0cafe352bc9bd76baa8406998 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Wed, 26 Feb 2025 16:29:20 +0100
Subject: [PATCH] Added dwm rotate

---
 mg_navigation/include/handlers/dwm.hpp        |  1 +
 mg_navigation/include/handlers/dwm_rotate.hpp | 66 +++++++++++++++++++
 mg_navigation/include/mg_navigation.hpp       |  5 +-
 mg_navigation/src/mg_navigation.cpp           | 15 +++--
 4 files changed, 82 insertions(+), 5 deletions(-)
 create mode 100644 mg_navigation/include/handlers/dwm_rotate.hpp

diff --git a/mg_navigation/include/handlers/dwm.hpp b/mg_navigation/include/handlers/dwm.hpp
index 6788d46..642455d 100644
--- a/mg_navigation/include/handlers/dwm.hpp
+++ b/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"
diff --git a/mg_navigation/include/handlers/dwm_rotate.hpp b/mg_navigation/include/handlers/dwm_rotate.hpp
new file mode 100644
index 0000000..44670ce
--- /dev/null
+++ b/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);
+            }
+        }
+    }
+
+}
\ No newline at end of file
diff --git a/mg_navigation/include/mg_navigation.hpp b/mg_navigation/include/mg_navigation.hpp
index c8787f7..7c92935 100644
--- a/mg_navigation/include/mg_navigation.hpp
+++ b/mg_navigation/include/mg_navigation.hpp
@@ -8,6 +8,7 @@
 #include "mg_msgs/action/move_point.hpp"
 #include "mg_msgs/action/move_straight.hpp"
 #include "mg_msgs/action/look_at.hpp"
+#include "mg_msgs/action/rotate.hpp"
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -26,12 +27,14 @@ namespace mg {
         using MovePoint    = mg_msgs::action::MovePoint;
         using MoveStraight = mg_msgs::action::MoveStraight;
         using LookAt       = mg_msgs::action::LookAt;
+        using Rotate       = mg_msgs::action::Rotate;
 
         rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
 
-        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
         rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
         rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
+        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
+        rclcpp_action::Server<Rotate>::SharedPtr       sv_rotate;
 
         rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
         tf2_ros::Buffer::SharedPtr                                  tf2_buffer;
diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp
index c1da57b..18a2b83 100644
--- a/mg_navigation/src/mg_navigation.cpp
+++ b/mg_navigation/src/mg_navigation.cpp
@@ -60,6 +60,13 @@ namespace mg {
           std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
           std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
 
+        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
+          this,
+          "MoveStraight",
+          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+
         sv_look_at = rclcpp_action::create_server<LookAt>(
           this,
           "LookAt",
@@ -67,12 +74,12 @@ namespace mg {
           std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
           std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
 
-        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
+        sv_rotate = rclcpp_action::create_server<Rotate>(
           this,
           "MoveStraight",
-          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "MoveStraight"));
     }
 
 };