global_planner_tests.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2017, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <global_planner_tests/global_planner_tests.h>
00036 #include <nav_grid/coordinate_conversion.h>
00037 #include <nav_core2/exceptions.h>
00038 #include <string>
00039 
00040 namespace global_planner_tests
00041 {
00042 void groupCells(const nav_core2::Costmap& costmap, PoseList& free_cells, PoseList& occupied_cells, bool include_edges)
00043 {
00044   const nav_grid::NavGridInfo& info = costmap.getInfo();
00045   int start = include_edges ? 0 : 1;
00046   unsigned int x_max = include_edges ? info.width : info.width - 1;
00047   unsigned int y_max = include_edges ? info.height : info.height - 1;
00048   nav_2d_msgs::Pose2DStamped pose;
00049   pose.header.frame_id = info.frame_id;
00050 
00051   for (unsigned int i = start; i < x_max; i++)
00052   {
00053     for (unsigned int j = start; j < y_max; j++)
00054     {
00055       gridToWorld(info, i, j, pose.pose.x, pose.pose.y);
00056       unsigned char cost = costmap(i, j);
00057       if (cost < costmap.INSCRIBED_INFLATED_OBSTACLE)
00058       {
00059         free_cells.push_back(pose);
00060       }
00061       else if (cost != costmap.NO_INFORMATION)
00062       {
00063         occupied_cells.push_back(pose);
00064       }
00065     }
00066   }
00067 }
00068 
00069 PoseList createPosesOutsideCostmap(const nav_core2::Costmap& costmap)
00070 {
00071   PoseList poses;
00072   double ox = costmap.getOriginX(), oy = costmap.getOriginY();
00073   double res = costmap.getResolution();
00074   double w = costmap.getWidth() * res, h = costmap.getHeight() * res;
00075   double offset = 3.0 * res;  // arbitrary distance outside costmap
00076   nav_2d_msgs::Pose2DStamped pose;
00077   pose.header.frame_id = costmap.getFrameId();
00078   pose.pose.x = ox - offset;
00079   pose.pose.y = oy - offset;
00080   poses.push_back(pose);
00081   pose.pose.y = oy + h + offset;
00082   poses.push_back(pose);
00083   pose.pose.x = ox + w + offset;
00084   poses.push_back(pose);
00085   pose.pose.y = oy - offset;
00086   poses.push_back(pose);
00087   return poses;
00088 }
00089 
00090 PoseList subsetPoseList(const PoseList& cells, unsigned int num_cells)
00091 {
00092   PoseList subset;
00093   if (cells.size() == 0 || num_cells == 0)
00094   {
00095     return subset;
00096   }
00097 
00098   if (cells.size() <= num_cells)
00099   {
00100     unsigned int i = 0;
00101     while (subset.size() < num_cells)
00102     {
00103       subset.push_back(cells[i++]);
00104       if (i >= cells.size())
00105       {
00106         i = 0;
00107       }
00108     }
00109   }
00110   else
00111   {
00112     double inc = cells.size() / static_cast<double>(num_cells);
00113     double i = 0.0;
00114     while (subset.size() < num_cells)
00115     {
00116       subset.push_back(cells[static_cast<int>(i)]);
00117       i += inc;
00118     }
00119   }
00120 
00121   return subset;
00122 }
00123 
00124 bool planExists(nav_core2::GlobalPlanner& planner, nav_2d_msgs::Pose2DStamped start, nav_2d_msgs::Pose2DStamped goal)
00125 {
00126   try
00127   {
00128     planner.makePlan(start, goal);
00129     return true;
00130   }
00131   catch (nav_core2::PlannerException& e)
00132   {
00133     return false;
00134   }
00135 }
00136 
00137 bool checkValidPathCoverage(nav_core2::GlobalPlanner& planner, const PoseList& free_cells,
00138                             bool verbose, bool quit_early)
00139 {
00140   int passing_plans = 0, total_plans = 0;
00141   for (nav_2d_msgs::Pose2DStamped start_pose : free_cells)
00142   {
00143     for (nav_2d_msgs::Pose2DStamped goal_pose : free_cells)
00144     {
00145       total_plans++;
00146 
00147       if (planExists(planner, start_pose, goal_pose))
00148       {
00149         passing_plans++;
00150       }
00151       else if (quit_early)
00152       {
00153         ROS_INFO("Failed to find a path between %.2f %.2f and %.2f %.2f",
00154                  start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
00155         return false;
00156       }
00157       if (!ros::ok()) return false;
00158     }
00159   }
00160 
00161   if (verbose)
00162     ROS_INFO("%d/%d valid plans found.", passing_plans, total_plans);
00163 
00164   return passing_plans == total_plans;
00165 }
00166 
00167 bool checkOccupiedPathCoverage(nav_core2::GlobalPlanner& planner,
00168                                const PoseList& start_cells, const PoseList& goal_cells,
00169                                const std::string& test_name,
00170                                bool check_exception_type, bool verbose, bool quit_early, bool invalid_starts)
00171 {
00172   int passing_plans = 0, total_plans = 0;
00173 
00174   for (nav_2d_msgs::Pose2DStamped start_pose : start_cells)
00175   {
00176     for (nav_2d_msgs::Pose2DStamped goal_pose : goal_cells)
00177     {
00178       total_plans++;
00179       bool valid = true;
00180       try
00181       {
00182         planner.makePlan(start_pose, goal_pose);
00183         // Plan should throw exception. If it doesn't, this test fails
00184         if (quit_early)
00185         {
00186           ROS_INFO("Found an unexpected valid %s path between %.2f %.2f and %.2f %.2f", test_name.c_str(),
00187                    start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
00188           return false;
00189         }
00190         valid = false;
00191       }
00192       catch (nav_core2::OccupiedStartException& e)
00193       {
00194         if (check_exception_type)
00195           valid = invalid_starts;
00196       }
00197       catch (nav_core2::OccupiedGoalException& e)
00198       {
00199         if (check_exception_type)
00200           valid = !invalid_starts;
00201       }
00202       catch (nav_core2::PlannerException& e)
00203       {
00204         valid = !check_exception_type;
00205       }
00206       if (valid)
00207       {
00208         passing_plans++;
00209       }
00210       else if (quit_early)
00211       {
00212         ROS_INFO("An unexpected exception was thrown attempting a %s path between %.2f %.2f and %.2f %.2f",
00213                  test_name.c_str(), start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
00214         return false;
00215       }
00216     }
00217   }
00218 
00219   if (verbose)
00220     ROS_INFO("%d/%d %s plans correctly aborted.", passing_plans, total_plans, test_name.c_str());
00221   return passing_plans == total_plans;
00222 }
00223 
00224 bool checkOutOfBoundsPathCoverage(nav_core2::GlobalPlanner& planner,
00225                                  const PoseList& start_cells, const PoseList& goal_cells,
00226                                  const std::string& test_name,
00227                                  bool check_exception_type, bool verbose, bool quit_early, bool invalid_starts)
00228 {
00229   int passing_plans = 0, total_plans = 0;
00230 
00231   for (nav_2d_msgs::Pose2DStamped start_pose : start_cells)
00232   {
00233     for (nav_2d_msgs::Pose2DStamped goal_pose : goal_cells)
00234     {
00235       total_plans++;
00236       bool valid = true;
00237       try
00238       {
00239         planner.makePlan(start_pose, goal_pose);
00240         // Plan should throw exception. If it doesn't, this test fails
00241         if (quit_early)
00242         {
00243           ROS_INFO("Found an unexpected valid %s path between %.2f %.2f and %.2f %.2f", test_name.c_str(),
00244                    start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
00245           return false;
00246         }
00247         valid = false;
00248       }
00249       catch (nav_core2::StartBoundsException& e)
00250       {
00251         if (check_exception_type)
00252           valid = invalid_starts;
00253       }
00254       catch (nav_core2::GoalBoundsException& e)
00255       {
00256         if (check_exception_type)
00257           valid = !invalid_starts;
00258       }
00259       catch (nav_core2::PlannerException& e)
00260       {
00261         valid = !check_exception_type;
00262       }
00263       if (valid)
00264       {
00265         passing_plans++;
00266       }
00267       else if (quit_early)
00268       {
00269         ROS_INFO("An unexpected exception was thrown attempting a %s path between %.2f %.2f and %.2f %.2f",
00270                  test_name.c_str(), start_pose.pose.x, start_pose.pose.y, goal_pose.pose.x, goal_pose.pose.y);
00271         return false;
00272       }
00273     }
00274   }
00275 
00276   if (verbose)
00277     ROS_INFO("%d/%d %s plans correctly aborted.", passing_plans, total_plans, test_name.c_str());
00278   return passing_plans == total_plans;
00279 }
00280 
00281 
00282 bool hasCompleteCoverage(nav_core2::GlobalPlanner& planner, const nav_core2::Costmap& costmap, int max_failure_cases,
00283                          bool check_exception_type, bool verbose, bool quit_early)
00284 {
00285   PoseList free_cells, occupied_cells;
00286   groupCells(costmap, free_cells, occupied_cells);
00287 
00288   bool ret = checkValidPathCoverage(planner, free_cells, verbose, quit_early);
00289   if (!ret && quit_early)
00290   {
00291     return ret;
00292   }
00293   if (max_failure_cases >= 0)
00294   {
00295     free_cells = subsetPoseList(free_cells, max_failure_cases);
00296     occupied_cells = subsetPoseList(occupied_cells, max_failure_cases);
00297   }
00298 
00299   ret = checkOccupiedPathCoverage(planner, free_cells, occupied_cells, "Free->Occupied",
00300                                   check_exception_type, verbose, quit_early, false) && ret;
00301   if (!ret && quit_early)
00302   {
00303     return ret;
00304   }
00305 
00306   ret = checkOccupiedPathCoverage(planner, occupied_cells, free_cells, "Occupied->Free",
00307                                   check_exception_type, verbose, quit_early, true) && ret;
00308   if (!ret && quit_early)
00309   {
00310     return ret;
00311   }
00312 
00313   ret = checkOccupiedPathCoverage(planner, occupied_cells, occupied_cells, "Occupied->Occupied",
00314                                   false, verbose, quit_early) && ret;
00315   if (!ret && quit_early)
00316   {
00317     return ret;
00318   }
00319 
00320   PoseList out_of_bounds = createPosesOutsideCostmap(costmap);
00321   ret = checkOutOfBoundsPathCoverage(planner, free_cells, out_of_bounds, "Free->OutOfBounds",
00322                                      check_exception_type, verbose, quit_early, false) && ret;
00323   if (!ret && quit_early)
00324   {
00325     return ret;
00326   }
00327 
00328   ret = checkOutOfBoundsPathCoverage(planner, out_of_bounds, free_cells, "OutOfBounds->Free",
00329                                      check_exception_type, verbose, quit_early, true) && ret;
00330   if (!ret && quit_early)
00331   {
00332     return ret;
00333   }
00334 
00335   ret = checkOutOfBoundsPathCoverage(planner, out_of_bounds, out_of_bounds, "CompletelyOutOfBounds",
00336                                      false, verbose, quit_early) && ret;
00337   if (!ret && quit_early)
00338   {
00339     return ret;
00340   }
00341 
00342   return ret;
00343 }
00344 
00345 bool hasNoPaths(nav_core2::GlobalPlanner& planner, const nav_core2::Costmap& costmap,
00346                 bool check_exception_type, bool verbose, bool quit_early)
00347 {
00348   PoseList free_cells, occupied_cells;
00349   groupCells(costmap, free_cells, occupied_cells);
00350   int passing_plans = 0, total_plans = 0;
00351 
00352   unsigned int n = free_cells.size();
00353   for (unsigned int i = 0; i < n; i++)
00354   {
00355     for (unsigned int j = 0; j < n; j++)
00356     {
00357       if (i == j) continue;
00358       total_plans++;
00359       bool valid;
00360       try
00361       {
00362         planner.makePlan(free_cells[i], free_cells[j]);
00363         // Plan should throw exception. If it doesn't, this test fails
00364         if (quit_early)
00365         {
00366           ROS_INFO("Found an unexpected valid path between %.2f %.2f and %.2f %.2f",
00367                    free_cells[i].pose.x, free_cells[i].pose.y, free_cells[j].pose.x, free_cells[j].pose.y);
00368           return false;
00369         }
00370         valid = false;
00371       }
00372       catch (nav_core2::NoGlobalPathException& e)
00373       {
00374         valid = true;
00375       }
00376       catch (nav_core2::PlannerException& e)
00377       {
00378         valid = !check_exception_type;
00379       }
00380       if (valid)
00381       {
00382         passing_plans++;
00383       }
00384       else if (quit_early)
00385       {
00386         ROS_INFO("An unexpected exception was thrown attempting to plan between %.2f %.2f and %.2f %.2f",
00387                  free_cells[i].pose.x, free_cells[i].pose.y, free_cells[j].pose.x, free_cells[j].pose.y);
00388         return false;
00389       }
00390     }
00391   }
00392 
00393   if (verbose)
00394     ROS_INFO("%d/%d correctly aborted for having no path.", passing_plans, total_plans);
00395   return passing_plans == total_plans;
00396 }
00397 
00398 }  // namespace global_planner_tests


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