From 3d65d1697c550f9d390bdf8a904e954352ad9415 Mon Sep 17 00:00:00 2001
From: Dave Hershberger <hersh@willowgarage.com>
Date: Fri, 4 Jan 2013 17:07:00 -0800
Subject: [PATCH] Fixed issue #18. When oscillation is detected during gradient
 following, calcPath() falls back on simple grid following.

---
 navfn/src/navfn.cpp           | 24 ++++++++++----
 navfn/test/path_calc_test.cpp | 61 ++++++++++++++++++++++++++++++++++-
 2 files changed, 77 insertions(+), 8 deletions(-)

diff --git a/navfn/src/navfn.cpp b/navfn/src/navfn.cpp
index 4db2f75082..bc7e16983f 100644
--- a/navfn/src/navfn.cpp
+++ b/navfn/src/navfn.cpp
@@ -865,6 +865,15 @@ namespace navfn {
         pathy[npath] = stc/nx + dy;
         npath++;
 
+        bool oscillation_detected = false;
+        if( npath > 2 &&
+            pathx[npath-1] == pathx[npath-3] &&
+            pathy[npath-1] == pathy[npath-3] )
+        {
+          ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
+          oscillation_detected = true;
+        }
+
         int stcnx = stc+nx;
         int stcpx = stc-nx;
 
@@ -877,8 +886,8 @@ namespace navfn {
             potarr[stcnx-1] >= POT_HIGH ||
             potarr[stcpx] >= POT_HIGH ||
             potarr[stcpx+1] >= POT_HIGH ||
-            potarr[stcpx-1] >= POT_HIGH)
-
+            potarr[stcpx-1] >= POT_HIGH ||
+            oscillation_detected)
         {
           ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
           // check eight neighbors to find the lowest
@@ -926,11 +935,6 @@ namespace navfn {
           gradCell(stcnx+1);
 
 
-          // show gradients
-          ROS_DEBUG("[Path] %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f\n",
-              gradx[stc], grady[stc], gradx[stc+1], grady[stc+1], 
-              gradx[stcnx], grady[stcnx], gradx[stcnx+1], grady[stcnx+1]);
-
           // get interpolated gradient
           float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];
           float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];
@@ -939,6 +943,12 @@ namespace navfn {
           float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];
           float y = (1.0-dy)*y1 + dy*y2; // interpolated y
 
+          // show gradients
+          ROS_DEBUG("[Path] %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f; final x=%.3f, y=%.3f\n",
+                    gradx[stc], grady[stc], gradx[stc+1], grady[stc+1], 
+                    gradx[stcnx], grady[stcnx], gradx[stcnx+1], grady[stcnx+1],
+                    x, y);
+
           // check for zero gradient, failed
           if (x == 0.0 && y == 0.0)
           {
diff --git a/navfn/test/path_calc_test.cpp b/navfn/test/path_calc_test.cpp
index 182d06d4c1..067ff02125 100644
--- a/navfn/test/path_calc_test.cpp
+++ b/navfn/test/path_calc_test.cpp
@@ -54,6 +54,60 @@ navfn::NavFn* make_willow_nav()
   return nav;
 }
 
+void print_neighborhood_of_last_path_entry( navfn::NavFn* nav )  
+{
+  printf("last path entries:\n");
+  for( int i = nav->npath - 4; i < nav->npath; i++ )
+  {
+    printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]);
+  }
+  printf("potential field neighborhood of last entry:\n");
+  int xf = nav->pathx[ nav->npath-1 ];
+  int yf = nav->pathy[ nav->npath-1 ];
+
+  printf( "   " );
+  for( int x = xf - 2; x <= xf + 2; x++ )
+  {
+    printf( " %6d", x );
+  }
+  printf( "\n" );
+
+  for( int y = yf - 2; y <= yf + 2; y++ )
+  {
+    printf( "%5d:", y );
+    for( int x = xf - 2; x <= xf + 2; x++ )
+    {
+      printf( " %5.1f", nav->potarr[ y * nav->nx + x ] );
+    }
+    printf( "\n" );
+  }
+
+  printf("gradient neighborhood of last entry:\n");
+  printf( "     " );
+  for( int x = xf - 2; x <= xf + 2; x++ )
+  {
+    printf( " %6d", x );
+  }
+  printf( "\n" );
+
+  for( int y = yf - 2; y <= yf + 2; y++ )
+  {
+    printf( "%5d x:", y );
+    for( int x = xf - 2; x <= xf + 2; x++ )
+    {
+      printf( " %5.1f", nav->gradx[ y * nav->nx + x ] );
+    }
+    printf( "\n" );
+
+    printf( "      y:" );
+    for( int x = xf - 2; x <= xf + 2; x++ )
+    {
+      printf( " %5.1f", nav->grady[ y * nav->nx + x ] );
+    }
+    printf( "\n" );
+  }
+}
+
 TEST(PathCalc, oscillate_in_pinch_point)
 {
   navfn::NavFn* nav = make_willow_nav();
@@ -71,7 +125,12 @@ TEST(PathCalc, oscillate_in_pinch_point)
   nav->setGoal( goal );
   nav->setStart( start );
 
-  EXPECT_TRUE( nav->calcNavFnDijkstra( true ));
+  bool plan_success = nav->calcNavFnDijkstra( true );
+  EXPECT_TRUE( plan_success );
+  if( !plan_success )
+  {
+    print_neighborhood_of_last_path_entry( nav );
+  }
 }
 
 TEST(PathCalc, easy_nav_should_always_work)