path_calc_test.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <string>
00031 #include <ros/package.h>
00032 #include <gtest/gtest.h>
00033 #include <navfn/navfn.h>
00034 #include <navfn/read_pgm_costmap.h>
00035 
00036 // Load a willow garage costmap and return a NavFn instance using it.
00037 // If the costmap file fails to load, returns NULL.
00038 navfn::NavFn* make_willow_nav()
00039 {
00040   int sx,sy;
00041   std::string path = ros::package::getPath( ROS_PACKAGE_NAME ) + "/test/willow_costmap.pgm";
00042 
00043   COSTTYPE *cmap = readPGM( path.c_str(), &sx, &sy, true );
00044   if( cmap == NULL )
00045   {
00046     return NULL;
00047   }
00048   navfn::NavFn* nav = new navfn::NavFn(sx,sy);
00049 
00050   nav->priInc = 2*COST_NEUTRAL; // thin wavefront
00051 
00052   memcpy( nav->costarr, cmap, sx*sy );
00053 
00054   return nav;
00055 }
00056 
00057 void print_neighborhood_of_last_path_entry( navfn::NavFn* nav )  
00058 {
00059   printf("last path entries:\n");
00060   for( int i = nav->npath - 4; i < nav->npath; i++ )
00061   {
00062     printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]);
00063   }
00064   printf("potential field neighborhood of last entry:\n");
00065   int xf = nav->pathx[ nav->npath-1 ];
00066   int yf = nav->pathy[ nav->npath-1 ];
00067 
00068   printf( "   " );
00069   for( int x = xf - 2; x <= xf + 2; x++ )
00070   {
00071     printf( " %6d", x );
00072   }
00073   printf( "\n" );
00074 
00075   for( int y = yf - 2; y <= yf + 2; y++ )
00076   {
00077     printf( "%5d:", y );
00078     for( int x = xf - 2; x <= xf + 2; x++ )
00079     {
00080       printf( " %5.1f", nav->potarr[ y * nav->nx + x ] );
00081     }
00082     printf( "\n" );
00083   }
00084 
00085   printf("gradient neighborhood of last entry:\n");
00086   printf( "     " );
00087   for( int x = xf - 2; x <= xf + 2; x++ )
00088   {
00089     printf( " %6d", x );
00090   }
00091   printf( "\n" );
00092 
00093   for( int y = yf - 2; y <= yf + 2; y++ )
00094   {
00095     printf( "%5d x:", y );
00096     for( int x = xf - 2; x <= xf + 2; x++ )
00097     {
00098       printf( " %5.1f", nav->gradx[ y * nav->nx + x ] );
00099     }
00100     printf( "\n" );
00101 
00102     printf( "      y:" );
00103     for( int x = xf - 2; x <= xf + 2; x++ )
00104     {
00105       printf( " %5.1f", nav->grady[ y * nav->nx + x ] );
00106     }
00107     printf( "\n" );
00108   }
00109 }
00110 
00111 TEST(PathCalc, oscillate_in_pinch_point)
00112 {
00113   navfn::NavFn* nav = make_willow_nav();
00114   ASSERT_TRUE( nav != NULL );
00115 
00116   int goal[2];
00117   int start[2];
00118 
00119   start[0] = 428;
00120   start[1] = 746;
00121   
00122   goal[0] = 350;
00123   goal[1] = 450;
00124 
00125   nav->setGoal( goal );
00126   nav->setStart( start );
00127 
00128   bool plan_success = nav->calcNavFnDijkstra( true );
00129   EXPECT_TRUE( plan_success );
00130   if( !plan_success )
00131   {
00132     print_neighborhood_of_last_path_entry( nav );
00133   }
00134 }
00135 
00136 TEST(PathCalc, easy_nav_should_always_work)
00137 {
00138   navfn::NavFn* nav = make_willow_nav();
00139   ASSERT_TRUE( nav != NULL );
00140 
00141   int goal[2];
00142   int start[2];
00143 
00144   start[0] = 350;
00145   start[1] = 400;
00146 
00147   goal[0] = 350;
00148   goal[1] = 450;
00149 
00150   nav->setGoal( goal );
00151   nav->setStart( start );
00152 
00153   EXPECT_TRUE( nav->calcNavFnDijkstra( true ));
00154 }
00155 
00156 int main(int argc, char **argv)
00157 {
00158   testing::InitGoogleTest(&argc, argv);
00159   return RUN_ALL_TESTS();
00160 }


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:14:13