First set of behaviors

2025-05-08 12:10:13 +02:00
parent da3d1d9d15
commit bce371dba2
13 changed files with 710 additions and 105 deletions
--- a/mg_navigation/include/handlers/dwa_point.hpp
+++ b/mg_navigation/include/handlers/dwa_point.hpp
@ -49,6 +49,13 @@ namespace mg {
        const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
        const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);

+        const double dist = baseNode.obstacle_manager()->box_colide(
+          n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
+
+        if (dist < 0.01) {
+            return NAN;
+        }
+
        // Calculate angle to goal post/pre movement
        const double angl   = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
        const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));