From 2c4188371e18e2fb3d083e0867b5e4d537a42860 Mon Sep 17 00:00:00 2001 From: oscardegroot Date: Thu, 9 Jan 2025 15:00:33 +0100 Subject: [PATCH] Update winding angle to take the safe angle between two angles. --- src/homotopy_comparison/winding_angle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/homotopy_comparison/winding_angle.cpp b/src/homotopy_comparison/winding_angle.cpp index a491422..cb8d4d2 100644 --- a/src/homotopy_comparison/winding_angle.cpp +++ b/src/homotopy_comparison/winding_angle.cpp @@ -101,7 +101,7 @@ namespace GuidancePlanner double angle = std::atan2(relative_position(1), relative_position(0)); // Current angle if (k > 1) - lambda += angle - prev_angle; // Relative angle + lambda += RosTools::angularDifference(prev_angle, angle); // angle - prev_angle, but safe prev_angle = angle; }