Fixed DubinsLength bug.

angularMod.m

@@ -0,0 +1,7 @@
+function [ m ] = angularMod(x,y)
+%angularMod Modulo safe for angles.
+%   Behaves as expected for angular quantities (negatives?).
+   n = floor(x./y);
+   m = x - n.*y;
+end
+

findDubinsLength.m

@@ -12,6 +12,7 @@
 %
 DUBINS_DEBUG = 0;
 DEBUG_VERBOSE = 0; % plots dubins trajectories
+DEBUG_VVERBOSE = 0; % extra verbosity
 %============= Input Validation ===============
 if nargin < 1
     error('No input arguments given!');
@@ -43,7 +44,7 @@
 % TODO be sure we aren't using anonymous functions anywhere. They are slow.
 %rotm = @(theta) [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]';
 
-% TODO replace everywhere with wrapTo2Pi()
+% TODO replace everywhere with wrapAngle()
 %wrap = @(theta) mod(theta,2*pi);
 
 
@@ -76,8 +77,15 @@
 
 % Case I, R-S-R
 theta = findHeadingFrom(c_rs,c_re);
-L1 = norm(c_rs - c_re) + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta - pi/2) - wrapTo2Pi(x_s - pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta - pi/2));
+if (DEBUG_VVERBOSE)
+fprintf('norm(c_rs - c_re)=%.6f \nr*wrap1=%0.6f \nr*wrap2=%0.6f\n',...
+   norm(c_rs - c_re), r*wrapAngle(2*pi + wrapAngle(theta - pi/2) - wrapAngle(x_s - pi/2)),...
+   r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta - pi/2)));
+fprintf('r*wrap2: wrapAngle(x_e - pi/2)=%0.6f, wrapAngle(theta - pi/2)=%0.6f\n',...
+    wrapAngle(x_e - pi/2), wrapAngle(theta - pi/2));
+end
+L1 = norm(c_rs - c_re) + r*wrapAngle(2*pi + wrapAngle(theta - pi/2) - wrapAngle(x_s - pi/2))...
+    + r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta - pi/2));
 if (DUBINS_DEBUG & DEBUG_VERBOSE)
     theta
     L1
@@ -87,8 +95,8 @@
 len = norm(c_le - c_rs);
 theta = findHeadingFrom(c_rs,c_le);
 theta2 = theta - pi/2 + asin((2*r)/len);
-L2 = sqrt(len^2 - 4*r^2)+r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2) - wrapTo2Pi(x_s - pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2 + pi) - wrapTo2Pi(x_e + pi/2));
+L2 = sqrt(len^2 - 4*r^2)+r*wrapAngle(2*pi + wrapAngle(theta2) - wrapAngle(x_s - pi/2))...
+    + r*wrapAngle(2*pi + wrapAngle(theta2 + pi) - wrapAngle(x_e + pi/2));
 if (DUBINS_DEBUG & DEBUG_VERBOSE)
     L2
 end
@@ -102,16 +110,16 @@
     error('Error in case III');
 end
 
-L3 = sqrt(len^2 - 4*r^2) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + theta2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta + theta2 - pi));
+L3 = sqrt(len^2 - 4*r^2) + r*wrapAngle(2*pi + wrapAngle(x_s + pi/2) - wrapAngle(theta + theta2))...
+    + r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta + theta2 - pi));
 if (DUBINS_DEBUG & DEBUG_VERBOSE)
     L3
 end
 
 % Case IV, L-S-L
 theta = findHeadingFrom(c_ls,c_le);
-L4 = norm(c_ls - c_le) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta + pi/2) - wrapTo2Pi(x_e + pi/2));
+L4 = norm(c_ls - c_le) + r*wrapAngle(2*pi + wrapAngle(x_s + pi/2) - wrapAngle(theta + pi/2))...
+    + r*wrapAngle(2*pi + wrapAngle(theta + pi/2) - wrapAngle(x_e + pi/2));
 if (DUBINS_DEBUG & DEBUG_VERBOSE)
     L4
 end
@@ -178,3 +186,5 @@ function plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le, r)
 M = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]';
 end
 
+
+

heading2Theta.m

@@ -12,13 +12,13 @@
     error('psi must be between 0 and 2*pi')
 end
 % =============================================================
-theta = mod(-(psi - pi/2),2*pi);
+theta = angularMod(-(psi - pi/2),2*pi);
 
 end %% function heading2Theta()
 
-%Note mod is actually:
+%Note angle safe mod if:
 %
-%function [m] = myMod(x,y)
+%function [m] = mymod(x,y)
 %   n = floor(x./y);
 %   m = x - n.*y;
 %end

test/testFindDubinsLength.m

@@ -213,9 +213,53 @@
     testsPassed = testsPassed + 1;
 end
 
-fprintf('----------------------------\n');
+%% Boundaries when angle distance is small
+%subplot(2,2,4);
+
+% Position
+%findDubinsLength([450 0], 3.927, [350, -100], 3.927, 10.2)
+startPosition = [450 0];
+startHeading = 3.92699 + 0.00001; % rad
+endPosition = [350, -100];
+endHeading = 3.92699; % rad
+q0 = [startPosition heading2Theta(startHeading)];
+q1 = [endPosition heading2Theta(endHeading)];
+
+% Plotting
+path = dubins(q0, q1, opts.TurnRadius, opts.DubinsStepSize);
+%plot([startPosition(1) endPosition(1)], [startPosition(2) endPosition(2)],...
+%    'ko', 'MarkerFaceColor', 'k')
+L =  findDubinsLength(startPosition, startHeading, endPosition, endHeading,...
+    opts.TurnRadius);
+hold on;
+%set(0,'currentFigure',fh)
+%plot(path(1,1:end), path(2,1:end), 'Color', 'g');
+%title('Case IV: L-S-L');
+%yl = ylim();
+%text(0,yl(1)+5,sprintf('L = %.2f',L));
+
+% Count length of path from DubinsPlot tool
+Lexpected = 0;
+for i=2:length(path)
+    l_i = sqrt((path(1,i) - path(1,i-1))^2 + (path(2,i) - path(2,i-1))^2);
+    Lexpected  = Lexpected + l_i;
+end % for
+
+% Results
+result = L;
+resultExpected = Lexpected;
+theta_diff = endHeading - startHeading;
+fprintf('Boundary (Line) Test: L = %.2f, CWdiff=%0.6f',result, theta_diff);
+if (abs(result - resultExpected) > EPSILON_ERROR)
+    fprintf('\t-- FAILED: expected %.2f\n',resultExpected);
+else
+    fprintf('\t-- PASS\n');
+    testsPassed = testsPassed + 1;
+end
+
 
 %% Results
+fprintf('----------------------------\n');
 if (testsPassed ~= totalTests)
     fprintf('\nFAILED %i of %i tests\n\n',totalTests - testsPassed, totalTests);
 else

wrapAngle.m

@@ -0,0 +1,5 @@
+function [ theta ] = wrapAngle( theta )
+%wrapAngle Wraps an angle to 2*pi using angularMod.
+%   Uses modulo function safe for angles.
+    theta = angularMod(theta,2*pi);
+end