global_planner_tests.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <nav_core2/exceptions.h>
38 #include <string>
39 
40 namespace global_planner_tests
41 {
42 void groupCells(const nav_core2::Costmap& costmap, PoseList& free_cells, PoseList& occupied_cells, bool include_edges)
43 {
44  const nav_grid::NavGridInfo& info = costmap.getInfo();
45  int start = include_edges ? 0 : 1;
46  unsigned int x_max = include_edges ? info.width : info.width - 1;
47  unsigned int y_max = include_edges ? info.height : info.height - 1;
48  nav_2d_msgs::Pose2DStamped pose;
49  pose.header.frame_id = info.frame_id;
50 
51  for (unsigned int i = start; i < x_max; i++)
52  {
53  for (unsigned int j = start; j < y_max; j++)
54  {
55  gridToWorld(info, i, j, pose.pose.x, pose.pose.y);
56  unsigned char cost = costmap(i, j);
57  if (cost < costmap.INSCRIBED_INFLATED_OBSTACLE)
58  {
59  free_cells.push_back(pose);
60  }
61  else if (cost != costmap.NO_INFORMATION)
62  {
63  occupied_cells.push_back(pose);
64  }
65  }
66  }
67 }
68 
70 {
71  PoseList poses;
72  double ox = costmap.getOriginX(), oy = costmap.getOriginY();
73  double res = costmap.getResolution();
74  double w = costmap.getWidth() * res, h = costmap.getHeight() * res;
75  double offset = 3.0 * res; // arbitrary distance outside costmap
76  nav_2d_msgs::Pose2DStamped pose;
77  pose.header.frame_id = costmap.getFrameId();
78  pose.pose.x = ox - offset;
79  pose.pose.y = oy - offset;
80  poses.push_back(pose);
81  pose.pose.y = oy + h + offset;
82  poses.push_back(pose);
83  pose.pose.x = ox + w + offset;
84  poses.push_back(pose);
85  pose.pose.y = oy - offset;
86  poses.push_back(pose);
87  return poses;
88 }
89 
90 PoseList subsetPoseList(const PoseList& cells, unsigned int num_cells)
91 {
92  PoseList subset;
93  if (cells.size() == 0 || num_cells == 0)
94  {
95  return subset;
96  }
97 
98  if (cells.size() <= num_cells)
99  {
100  unsigned int i = 0;
101  while (subset.size() < num_cells)
102  {
103  subset.push_back(cells[i++]);
104  if (i >= cells.size())
105  {
106  i = 0;
107  }
108  }
109  }
110  else
111  {
112  double inc = cells.size() / static_cast<double>(num_cells);
113  double i = 0.0;
114  while (subset.size() < num_cells)
115  {
116  subset.push_back(cells[static_cast<int>(i)]);
117  i += inc;
118  }
119  }
120 
121  return subset;
122 }
123 
124 bool planExists(nav_core2::GlobalPlanner& planner, nav_2d_msgs::Pose2DStamped start, nav_2d_msgs::Pose2DStamped goal)
125 {
126  try
127  {
128  planner.makePlan(start, goal);
129  return true;
130  }
131  catch (nav_core2::PlannerException& e)
132  {
133  return false;
134  }
135 }
136 
138  bool verbose, bool quit_early)
139 {
140  int passing_plans = 0, total_plans = 0;
141  for (nav_2d_msgs::Pose2DStamped start_pose : free_cells)
142  {
143  for (nav_2d_msgs::Pose2DStamped goal_pose : free_cells)
144  {
145  total_plans++;
146 
147  if (planExists(planner, start_pose, goal_pose))
148  {
149  passing_plans++;
150  }
151  else if (quit_early)
152  {
153  ROS_INFO("Failed to find a path between %.2f %.2f and %.2f %.2f",
154  start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
155  return false;
156  }
157  if (!ros::ok()) return false;
158  }
159  }
160 
161  if (verbose)
162  ROS_INFO("%d/%d valid plans found.", passing_plans, total_plans);
163 
164  return passing_plans == total_plans;
165 }
166 
168  const PoseList& start_cells, const PoseList& goal_cells,
169  const std::string& test_name,
170  bool check_exception_type, bool verbose, bool quit_early, bool invalid_starts)
171 {
172  int passing_plans = 0, total_plans = 0;
173 
174  for (nav_2d_msgs::Pose2DStamped start_pose : start_cells)
175  {
176  for (nav_2d_msgs::Pose2DStamped goal_pose : goal_cells)
177  {
178  total_plans++;
179  bool valid = true;
180  try
181  {
182  planner.makePlan(start_pose, goal_pose);
183  // Plan should throw exception. If it doesn't, this test fails
184  if (quit_early)
185  {
186  ROS_INFO("Found an unexpected valid %s path between %.2f %.2f and %.2f %.2f", test_name.c_str(),
187  start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
188  return false;
189  }
190  valid = false;
191  }
193  {
194  if (check_exception_type)
195  valid = invalid_starts;
196  }
198  {
199  if (check_exception_type)
200  valid = !invalid_starts;
201  }
202  catch (nav_core2::PlannerException& e)
203  {
204  valid = !check_exception_type;
205  }
206  if (valid)
207  {
208  passing_plans++;
209  }
210  else if (quit_early)
211  {
212  ROS_INFO("An unexpected exception was thrown attempting a %s path between %.2f %.2f and %.2f %.2f",
213  test_name.c_str(), start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
214  return false;
215  }
216  }
217  }
218 
219  if (verbose)
220  ROS_INFO("%d/%d %s plans correctly aborted.", passing_plans, total_plans, test_name.c_str());
221  return passing_plans == total_plans;
222 }
223 
225  const PoseList& start_cells, const PoseList& goal_cells,
226  const std::string& test_name,
227  bool check_exception_type, bool verbose, bool quit_early, bool invalid_starts)
228 {
229  int passing_plans = 0, total_plans = 0;
230 
231  for (nav_2d_msgs::Pose2DStamped start_pose : start_cells)
232  {
233  for (nav_2d_msgs::Pose2DStamped goal_pose : goal_cells)
234  {
235  total_plans++;
236  bool valid = true;
237  try
238  {
239  planner.makePlan(start_pose, goal_pose);
240  // Plan should throw exception. If it doesn't, this test fails
241  if (quit_early)
242  {
243  ROS_INFO("Found an unexpected valid %s path between %.2f %.2f and %.2f %.2f", test_name.c_str(),
244  start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
245  return false;
246  }
247  valid = false;
248  }
250  {
251  if (check_exception_type)
252  valid = invalid_starts;
253  }
255  {
256  if (check_exception_type)
257  valid = !invalid_starts;
258  }
259  catch (nav_core2::PlannerException& e)
260  {
261  valid = !check_exception_type;
262  }
263  if (valid)
264  {
265  passing_plans++;
266  }
267  else if (quit_early)
268  {
269  ROS_INFO("An unexpected exception was thrown attempting a %s path between %.2f %.2f and %.2f %.2f",
270  test_name.c_str(), start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
271  return false;
272  }
273  }
274  }
275 
276  if (verbose)
277  ROS_INFO("%d/%d %s plans correctly aborted.", passing_plans, total_plans, test_name.c_str());
278  return passing_plans == total_plans;
279 }
280 
281 
282 bool hasCompleteCoverage(nav_core2::GlobalPlanner& planner, const nav_core2::Costmap& costmap, int max_failure_cases,
283  bool check_exception_type, bool verbose, bool quit_early)
284 {
285  PoseList free_cells, occupied_cells;
286  groupCells(costmap, free_cells, occupied_cells);
287 
288  bool ret = checkValidPathCoverage(planner, free_cells, verbose, quit_early);
289  if (!ret && quit_early)
290  {
291  return ret;
292  }
293  if (max_failure_cases >= 0)
294  {
295  free_cells = subsetPoseList(free_cells, max_failure_cases);
296  occupied_cells = subsetPoseList(occupied_cells, max_failure_cases);
297  }
298 
299  ret = checkOccupiedPathCoverage(planner, free_cells, occupied_cells, "Free->Occupied",
300  check_exception_type, verbose, quit_early, false) && ret;
301  if (!ret && quit_early)
302  {
303  return ret;
304  }
305 
306  ret = checkOccupiedPathCoverage(planner, occupied_cells, free_cells, "Occupied->Free",
307  check_exception_type, verbose, quit_early, true) && ret;
308  if (!ret && quit_early)
309  {
310  return ret;
311  }
312 
313  ret = checkOccupiedPathCoverage(planner, occupied_cells, occupied_cells, "Occupied->Occupied",
314  false, verbose, quit_early) && ret;
315  if (!ret && quit_early)
316  {
317  return ret;
318  }
319 
320  PoseList out_of_bounds = createPosesOutsideCostmap(costmap);
321  ret = checkOutOfBoundsPathCoverage(planner, free_cells, out_of_bounds, "Free->OutOfBounds",
322  check_exception_type, verbose, quit_early, false) && ret;
323  if (!ret && quit_early)
324  {
325  return ret;
326  }
327 
328  ret = checkOutOfBoundsPathCoverage(planner, out_of_bounds, free_cells, "OutOfBounds->Free",
329  check_exception_type, verbose, quit_early, true) && ret;
330  if (!ret && quit_early)
331  {
332  return ret;
333  }
334 
335  ret = checkOutOfBoundsPathCoverage(planner, out_of_bounds, out_of_bounds, "CompletelyOutOfBounds",
336  false, verbose, quit_early) && ret;
337  if (!ret && quit_early)
338  {
339  return ret;
340  }
341 
342  return ret;
343 }
344 
346  bool check_exception_type, bool verbose, bool quit_early)
347 {
348  PoseList free_cells, occupied_cells;
349  groupCells(costmap, free_cells, occupied_cells);
350  int passing_plans = 0, total_plans = 0;
351 
352  unsigned int n = free_cells.size();
353  for (unsigned int i = 0; i < n; i++)
354  {
355  for (unsigned int j = 0; j < n; j++)
356  {
357  if (i == j) continue;
358  total_plans++;
359  bool valid;
360  try
361  {
362  planner.makePlan(free_cells[i], free_cells[j]);
363  // Plan should throw exception. If it doesn't, this test fails
364  if (quit_early)
365  {
366  ROS_INFO("Found an unexpected valid path between %.2f %.2f and %.2f %.2f",
367  free_cells[i].pose.x, free_cells[i].pose.y, free_cells[j].pose.x, free_cells[j].pose.y);
368  return false;
369  }
370  valid = false;
371  }
373  {
374  valid = true;
375  }
376  catch (nav_core2::PlannerException& e)
377  {
378  valid = !check_exception_type;
379  }
380  if (valid)
381  {
382  passing_plans++;
383  }
384  else if (quit_early)
385  {
386  ROS_INFO("An unexpected exception was thrown attempting to plan between %.2f %.2f and %.2f %.2f",
387  free_cells[i].pose.x, free_cells[i].pose.y, free_cells[j].pose.x, free_cells[j].pose.y);
388  return false;
389  }
390  }
391  }
392 
393  if (verbose)
394  ROS_INFO("%d/%d correctly aborted for having no path.", passing_plans, total_plans);
395  return passing_plans == total_plans;
396 }
397 
398 } // namespace global_planner_tests
PoseList createPosesOutsideCostmap(const nav_core2::Costmap &costmap)
Create a list of poses that are outside the costmap&#39;s bounds.
std::vector< nav_2d_msgs::Pose2DStamped > PoseList
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
bool checkValidPathCoverage(nav_core2::GlobalPlanner &planner, const PoseList &free_cells, bool verbose=false, bool quit_early=true)
Check if a path exists between every pair of free_cells.
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
static const unsigned char NO_INFORMATION
PoseList subsetPoseList(const PoseList &cells, unsigned int num_cells)
Create a new list of poses from cells that is num_cells long.
#define ROS_INFO(...)
bool checkOccupiedPathCoverage(nav_core2::GlobalPlanner &planner, const PoseList &start_cells, const PoseList &goal_cells, const std::string &test_name, bool check_exception_type=true, bool verbose=false, bool quit_early=true, bool invalid_starts=true)
Check if the appropriate exception is thrown when attempting to plan to or from an occupied cell...
bool hasCompleteCoverage(nav_core2::GlobalPlanner &planner, const nav_core2::Costmap &costmap, int max_failure_cases=10, bool check_exception_type=true, bool verbose=false, bool quit_early=true)
Run a bunch of tests, assuming there is a valid path from any free cell to any other free cell...
bool checkOutOfBoundsPathCoverage(nav_core2::GlobalPlanner &planner, const PoseList &start_cells, const PoseList &goal_cells, const std::string &test_name, bool check_exception_type=true, bool verbose=false, bool quit_early=true, bool invalid_starts=true)
Check if the appropriate exception is thrown when attempting to plan to or from a pose off the costma...
ROSCPP_DECL bool ok()
bool planExists(nav_core2::GlobalPlanner &planner, nav_2d_msgs::Pose2DStamped start, nav_2d_msgs::Pose2DStamped goal)
Simple check to see if a plan exists. Returns a boolean instead of returning path or throwing an exce...
TFSIMD_FORCE_INLINE const tfScalar & w() const
virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal)=0
void groupCells(const nav_core2::Costmap &costmap, PoseList &free_cells, PoseList &occupied_cells, bool include_edges=true)
Populate two lists of poses from a costmap. one with all the free cells, the other with all the occup...
bool hasNoPaths(nav_core2::GlobalPlanner &planner, const nav_core2::Costmap &costmap, bool check_exception_type=true, bool verbose=false, bool quit_early=true)
Run a bunch of tests, assuming there are no valid paths from any free cell to any other free cell...


global_planner_tests
Author(s):
autogenerated on Wed Jun 26 2019 20:06:09