path_calc_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <string>
31 #include <ros/package.h>
32 #include <gtest/gtest.h>
33 #include <navfn/navfn.h>
34 #include <navfn/read_pgm_costmap.h>
35 
36 // Load a willow garage costmap and return a NavFn instance using it.
37 // If the costmap file fails to load, returns NULL.
39 {
40  int sx,sy;
41  std::string path = ros::package::getPath( ROS_PACKAGE_NAME ) + "/test/willow_costmap.pgm";
42 
43  COSTTYPE *cmap = readPGM( path.c_str(), &sx, &sy, true );
44  if( cmap == NULL )
45  {
46  return NULL;
47  }
48  navfn::NavFn* nav = new navfn::NavFn(sx,sy);
49 
50  nav->priInc = 2*COST_NEUTRAL; // thin wavefront
51 
52  memcpy( nav->costarr, cmap, sx*sy );
53 
54  return nav;
55 }
56 
58 {
59  printf("last path entries:\n");
60  for( int i = nav->npath - 4; i < nav->npath; i++ )
61  {
62  printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]);
63  }
64  printf("potential field neighborhood of last entry:\n");
65  int xf = nav->pathx[ nav->npath-1 ];
66  int yf = nav->pathy[ nav->npath-1 ];
67 
68  printf( " " );
69  for( int x = xf - 2; x <= xf + 2; x++ )
70  {
71  printf( " %6d", x );
72  }
73  printf( "\n" );
74 
75  for( int y = yf - 2; y <= yf + 2; y++ )
76  {
77  printf( "%5d:", y );
78  for( int x = xf - 2; x <= xf + 2; x++ )
79  {
80  printf( " %5.1f", nav->potarr[ y * nav->nx + x ] );
81  }
82  printf( "\n" );
83  }
84 
85  printf("gradient neighborhood of last entry:\n");
86  printf( " " );
87  for( int x = xf - 2; x <= xf + 2; x++ )
88  {
89  printf( " %6d", x );
90  }
91  printf( "\n" );
92 
93  for( int y = yf - 2; y <= yf + 2; y++ )
94  {
95  printf( "%5d x:", y );
96  for( int x = xf - 2; x <= xf + 2; x++ )
97  {
98  printf( " %5.1f", nav->gradx[ y * nav->nx + x ] );
99  }
100  printf( "\n" );
101 
102  printf( " y:" );
103  for( int x = xf - 2; x <= xf + 2; x++ )
104  {
105  printf( " %5.1f", nav->grady[ y * nav->nx + x ] );
106  }
107  printf( "\n" );
108  }
109 }
110 
111 TEST(PathCalc, oscillate_in_pinch_point)
112 {
113  navfn::NavFn* nav = make_willow_nav();
114  ASSERT_TRUE( nav != NULL );
115 
116  int goal[2];
117  int start[2];
118 
119  start[0] = 428;
120  start[1] = 746;
121 
122  goal[0] = 350;
123  goal[1] = 450;
124 
125  nav->setGoal( goal );
126  nav->setStart( start );
127 
128  bool plan_success = nav->calcNavFnDijkstra( true );
129  EXPECT_TRUE( plan_success );
130  if( !plan_success )
131  {
133  }
134 }
135 
136 TEST(PathCalc, easy_nav_should_always_work)
137 {
138  navfn::NavFn* nav = make_willow_nav();
139  ASSERT_TRUE( nav != NULL );
140 
141  int goal[2];
142  int start[2];
143 
144  start[0] = 350;
145  start[1] = 400;
146 
147  goal[0] = 350;
148  goal[1] = 450;
149 
150  nav->setGoal( goal );
151  nav->setStart( start );
152 
153  EXPECT_TRUE( nav->calcNavFnDijkstra( true ));
154 }
155 
156 int main(int argc, char **argv)
157 {
158  testing::InitGoogleTest(&argc, argv);
159  return RUN_ALL_TESTS();
160 }
COSTTYPE * readPGM(const char *fname, int *width, int *height, bool raw=false)
Definition: navtest.cpp:282
navfn::NavFn * make_willow_nav()
int npath
Definition: navfn.h:242
TEST(PathCalc, oscillate_in_pinch_point)
float * pathx
Definition: navfn.h:241
void setStart(int *start)
Sets the start position for the planner. Note: the navigation cost field computed gives the cost to g...
Definition: navfn.cpp:181
bool calcNavFnDijkstra(bool atStart=false)
Caclulates the full navigation function using Dijkstra.
Definition: navfn.cpp:290
TFSIMD_FORCE_INLINE const tfScalar & y() const
int nx
Definition: navfn.h:124
float * potarr
Definition: navfn.h:171
COSTTYPE * costarr
Definition: navfn.h:170
float * gradx
Definition: navfn.h:240
float * grady
Definition: navfn.h:240
void setGoal(int *goal)
Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to ge...
Definition: navfn.cpp:173
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
Definition: navfn.h:106
TFSIMD_FORCE_INLINE const tfScalar & x() const
ROSLIB_DECL std::string getPath(const std::string &package_name)
float * pathy
Definition: navfn.h:241
float priInc
Definition: navfn.h:182
void print_neighborhood_of_last_path_entry(navfn::NavFn *nav)
int main(int argc, char **argv)


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:06:04