From 659eead34dac6383ab273a1c995780929ef0c6e8 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Sun, 8 Dec 2024 14:34:52 +0100
Subject: [PATCH] Added aborting if robot gets misaligned

---
 mg_msgs/action/MoveStraight.action   |  1 +
 mg_navigation/src/mg_move_server.cpp | 20 ++++++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/mg_msgs/action/MoveStraight.action b/mg_msgs/action/MoveStraight.action
index c0eb11d..cbde139 100644
--- a/mg_msgs/action/MoveStraight.action
+++ b/mg_msgs/action/MoveStraight.action
@@ -5,6 +5,7 @@ string[] ignore_tags []
 uint8 SUCCESS=0
 uint8 BLOCKED=1
 uint8 TIMEOUT=2
+uint8 MISALIGNED=3
 uint8 UNKNOWN=254
 
 uint8 error
diff --git a/mg_navigation/src/mg_move_server.cpp b/mg_navigation/src/mg_move_server.cpp
index 1c9c627..68d9947 100644
--- a/mg_navigation/src/mg_move_server.cpp
+++ b/mg_navigation/src/mg_move_server.cpp
@@ -135,6 +135,14 @@ private:
         return twist;
     }
 
+    double CalcMiss(glm::dvec2 a, glm::dvec2 b, glm::dvec2 c, double theta) {
+        glm::dvec2 v1 = a-b;
+        glm::dvec2 v2 = c-b;
+        double prod = glm::clamp(glm::dot(v1,v2)/(glm::length(v1) * glm::length(v2)), -1.0, 1.0);
+        double phi = glm::acos(prod);
+        return glm::sin(phi+theta) * -glm::length(v2);
+    }
+
     void execute_move(const std::shared_ptr<MoveGoalHandle> goal_handle) {
 
         auto goal = goal_handle->get_goal();
@@ -153,12 +161,16 @@ private:
             c_theta = theta_;
         }
 
+        double og_theta = c_theta;
+        glm::dvec2 og_point = c_point;
+
         glm::dmat2 rotmat = glm::dmat2(glm::rotate(glm::dmat4(1), c_theta, glm::dvec3(0,0,1)));
 
         RCLCPP_INFO(this->get_logger(), "Rotmat: %lf;%lf;%lf;%lf;", rotmat[0].x,rotmat[0].y,rotmat[1].x,rotmat[1].y);
         glm::dvec2 dir(1,0);
 
         dir = rotmat * dir; 
+        
 
         t_point = c_point + dir*target_distance;
 
@@ -178,6 +190,14 @@ private:
                 return;
             }
 
+            if(CalcMiss(og_point, t_point, c_point, c_theta-og_theta) > tolerance) {
+                command_publisher_->publish(to_twist(0.0 ,0.0));
+                auto x = std::make_shared<MoveAction::Result>();
+                x->error = MoveAction::Result::MISALIGNED;
+                is_processing_goal_ = false;
+                goal_handle->abort(x);
+            }
+
             double best_score = -INFINITY;
             int i_b = 0;
             for(int i = -2; i<3 ;i++){