From 2f279896a7ef3f1724c5402362c178577538e7da Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Sun, 23 Mar 2025 10:26:47 +0100
Subject: [PATCH] DWM -> DWA (Dynamic window approach)

---
 mg_navigation/include/handlers/dwa.hpp         |  7 +++++++
 .../handlers/{dwm_core.hpp => dwa_core.hpp}    | 18 +++++++++---------
 .../{dwm_forward.hpp => dwa_forward.hpp}       | 12 ++++++------
 .../{dwm_lookat.hpp => dwa_lookat.hpp}         | 14 +++++++-------
 .../handlers/{dwm_point.hpp => dwa_point.hpp}  | 12 ++++++------
 .../{dwm_rotate.hpp => dwa_rotate.hpp}         | 12 ++++++------
 mg_navigation/include/handlers/dwm.hpp         |  7 -------
 mg_navigation/src/mg_navigation.cpp            |  4 ++--
 8 files changed, 43 insertions(+), 43 deletions(-)
 create mode 100644 mg_navigation/include/handlers/dwa.hpp
 rename mg_navigation/include/handlers/{dwm_core.hpp => dwa_core.hpp} (93%)
 rename mg_navigation/include/handlers/{dwm_forward.hpp => dwa_forward.hpp} (81%)
 rename mg_navigation/include/handlers/{dwm_lookat.hpp => dwa_lookat.hpp} (85%)
 rename mg_navigation/include/handlers/{dwm_point.hpp => dwa_point.hpp} (87%)
 rename mg_navigation/include/handlers/{dwm_rotate.hpp => dwa_rotate.hpp} (83%)
 delete mode 100644 mg_navigation/include/handlers/dwm.hpp

diff --git a/mg_navigation/include/handlers/dwa.hpp b/mg_navigation/include/handlers/dwa.hpp
new file mode 100644
index 0000000..321d3a6
--- /dev/null
+++ b/mg_navigation/include/handlers/dwa.hpp
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "handlers/dwa_core.hpp"
+#include "handlers/dwa_point.hpp"
+#include "handlers/dwa_forward.hpp"
+#include "handlers/dwa_lookat.hpp"
+#include "handlers/dwa_rotate.hpp"
diff --git a/mg_navigation/include/handlers/dwm_core.hpp b/mg_navigation/include/handlers/dwa_core.hpp
similarity index 93%
rename from mg_navigation/include/handlers/dwm_core.hpp
rename to mg_navigation/include/handlers/dwa_core.hpp
index eee0618..2bf599c 100644
--- a/mg_navigation/include/handlers/dwm_core.hpp
+++ b/mg_navigation/include/handlers/dwa_core.hpp
@@ -30,7 +30,7 @@
 namespace mg {
 
     template <typename T>
-    class DWM {
+    class DWA {
       public:
         using Goal       = typename T::Goal;
         using Result     = typename T::Result;
@@ -62,7 +62,7 @@ namespace mg {
         glm::dvec2 pos;
         double     theta = 0;
 
-        DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
+        DWA(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
 
         void execute();
 
@@ -88,7 +88,7 @@ namespace mg {
     };
 
     template <typename T>
-    DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
+    DWA<T>::DWA(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
       baseNode(mns),
       hgoal(g),
       pub_twist(mns.pub_twist),
@@ -99,7 +99,7 @@ namespace mg {
       pos(0, 0) { }
 
     template <typename T>
-    void DWM<T>::execute() {
+    void DWA<T>::execute() {
         std::vector<int> spacevl(dimx * dimy);
         std::vector<int> spacevr(dimx * dimy);
 
@@ -144,7 +144,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::pos_query() {
+    void DWA<T>::pos_query() {
         try {
             double x = NAN;
             double y = NAN;
@@ -164,7 +164,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::send_speed(double vl, double vr) {
+    void DWA<T>::send_speed(double vl, double vr) {
         auto [v, w] = to_vel(vl, vr);
         Geometry::TwistStamped twist;
         twist.twist.angular.z = w;
@@ -174,7 +174,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::abort(int error) {
+    void DWA<T>::abort(int error) {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();
@@ -183,7 +183,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::succeed() {
+    void DWA<T>::succeed() {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();
@@ -192,7 +192,7 @@ namespace mg {
     }
 
     template <typename T>
-    void DWM<T>::cancel() {
+    void DWA<T>::cancel() {
         baseNode.mtx.lock();
         send_speed(0, 0);
         auto x   = std::make_shared<Result>();
diff --git a/mg_navigation/include/handlers/dwm_forward.hpp b/mg_navigation/include/handlers/dwa_forward.hpp
similarity index 81%
rename from mg_navigation/include/handlers/dwm_forward.hpp
rename to mg_navigation/include/handlers/dwa_forward.hpp
index 5b32674..9afceb4 100644
--- a/mg_navigation/include/handlers/dwm_forward.hpp
+++ b/mg_navigation/include/handlers/dwa_forward.hpp
@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -8,22 +8,22 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
+    inline bool DWA<MgNavigationServer::MoveStraight>::target_check() {
         return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
+    inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
         target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
+    inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
+    inline double DWA<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
         constexpr double delta = 0.8;
 
         auto [v, w] = to_vel(step * vl, step * vr);
@@ -37,7 +37,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
                                                                            std::vector<int>& vr,
                                                                            int               dimx,
                                                                            int) {
diff --git a/mg_navigation/include/handlers/dwm_lookat.hpp b/mg_navigation/include/handlers/dwa_lookat.hpp
similarity index 85%
rename from mg_navigation/include/handlers/dwm_lookat.hpp
rename to mg_navigation/include/handlers/dwa_lookat.hpp
index 8bc3fe7..2684765 100644
--- a/mg_navigation/include/handlers/dwm_lookat.hpp
+++ b/mg_navigation/include/handlers/dwa_lookat.hpp
@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -9,7 +9,7 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::LookAt>::target_check() {
+    inline bool DWA<MgNavigationServer::LookAt>::target_check() {
 
         const double a = glm::abs(theta - target_ornt);
         const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
@@ -18,7 +18,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::target_init() {
+    inline void DWA<MgNavigationServer::LookAt>::target_init() {
 
         target_pos = glm::vec2(goal->x, goal->y);
 
@@ -30,7 +30,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::target_update() {
+    inline void DWA<MgNavigationServer::LookAt>::target_update() {
 
         const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
         const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
@@ -40,12 +40,12 @@ namespace mg {
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
+    inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
+    inline double DWA<MgNavigationServer::LookAt>::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;
@@ -62,7 +62,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
                                                                      std::vector<int>& vr,
                                                                      int               dimx,
                                                                      int) {
diff --git a/mg_navigation/include/handlers/dwm_point.hpp b/mg_navigation/include/handlers/dwa_point.hpp
similarity index 87%
rename from mg_navigation/include/handlers/dwm_point.hpp
rename to mg_navigation/include/handlers/dwa_point.hpp
index a6f69cc..145af60 100644
--- a/mg_navigation/include/handlers/dwm_point.hpp
+++ b/mg_navigation/include/handlers/dwa_point.hpp
@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -8,22 +8,22 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
+    inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
         return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MovePoint>::target_update() {
+    inline void DWA<MgNavigationServer::MovePoint>::target_update() {
         target_pos = glm::dvec2(goal->x, goal->y);
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
+    inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
+    inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
         auto [v, w]        = to_vel(step * vl, step * vr);
         const double delta = 0.5;
         glm::dvec2   n_pos;
@@ -57,7 +57,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
                                                                         std::vector<int>& vr,
                                                                         int               dimx,
                                                                         int               dimy) {
diff --git a/mg_navigation/include/handlers/dwm_rotate.hpp b/mg_navigation/include/handlers/dwa_rotate.hpp
similarity index 83%
rename from mg_navigation/include/handlers/dwm_rotate.hpp
rename to mg_navigation/include/handlers/dwa_rotate.hpp
index 44670ce..cd0b870 100644
--- a/mg_navigation/include/handlers/dwm_rotate.hpp
+++ b/mg_navigation/include/handlers/dwa_rotate.hpp
@@ -1,5 +1,5 @@
 #pragma once
-#include "handlers/dwm_core.hpp"
+#include "handlers/dwa_core.hpp"
 
 #include "glm/gtx/norm.hpp"
 #include "glm/gtx/rotate_vector.hpp"
@@ -9,7 +9,7 @@
 namespace mg {
 
     template <>
-    inline bool DWM<MgNavigationServer::Rotate>::target_check() {
+    inline bool DWA<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;
@@ -18,17 +18,17 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::Rotate>::target_init() {
+    inline void DWA<MgNavigationServer::Rotate>::target_init() {
         target_ornt = goal->angle;
     }
 
     template <>
-    inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
+    inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
         return false;
     }
 
     template <>
-    inline double DWM<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
+    inline double DWA<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;
@@ -45,7 +45,7 @@ namespace mg {
     }
 
     template <>
-    inline void DWM<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
+    inline void DWA<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
                                                                      std::vector<int>& vr,
                                                                      int               dimx,
                                                                      int) {
diff --git a/mg_navigation/include/handlers/dwm.hpp b/mg_navigation/include/handlers/dwm.hpp
deleted file mode 100644
index 642455d..0000000
--- a/mg_navigation/include/handlers/dwm.hpp
+++ /dev/null
@@ -1,7 +0,0 @@
-#pragma once
-
-#include "handlers/dwm_core.hpp"
-#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/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp
index f1fa37d..ff925b0 100644
--- a/mg_navigation/src/mg_navigation.cpp
+++ b/mg_navigation/src/mg_navigation.cpp
@@ -4,7 +4,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "mg_navigation.hpp"
-#include "handlers/dwm.hpp"
+#include "handlers/dwa.hpp"
 
 namespace mg {
 
@@ -40,7 +40,7 @@ namespace mg {
 
     template <typename T>
     void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
-        DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
+        DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
         dwm.execute();
         is_processing = false;
         mtx.unlock();