Classes | Typedefs | Functions
global_planner_tests Namespace Reference

Classes

class  EasyCostmap
 An instantiation of the Costmap class that simply populates the grid from an image. More...
 

Typedefs

using PoseList = std::vector< nav_2d_msgs::Pose2DStamped >
 

Functions

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. More...
 
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 costmap. More...
 
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. More...
 
PoseList createPosesOutsideCostmap (const nav_core2::Costmap &costmap)
 Create a list of poses that are outside the costmap's bounds. More...
 
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 occupied cells. More...
 
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. More...
 
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. More...
 
bool many_map_test_suite (nav_core2::GlobalPlanner &planner, TFListenerPtr tf, const std::string &planner_name, const std::string &maps_list_filename="package://global_planner_tests/config/standard_tests.yaml", bool check_exception_type=true, bool verbose=false, bool quit_early=true)
 Run a collection of global_planner_tests on the provided maps. More...
 
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 exception. More...
 
std::string resolve_filename (const std::string &filename)
 Replace a package:// prefix with the actual path to the given package. More...
 
PoseList subsetPoseList (const PoseList &cells, unsigned int num_cells)
 Create a new list of poses from cells that is num_cells long. More...
 

Typedef Documentation

using global_planner_tests::PoseList = typedef std::vector<nav_2d_msgs::Pose2DStamped>

Definition at line 46 of file global_planner_tests.h.

Function Documentation

bool global_planner_tests::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.

This attempts plans from every start_cell to every goal_cell. We want to test three different modes

  • Occupied cell to free cell
  • Free cell to occupied cell
  • Occupied cell to occupied cell

In the first case, the start cell is invalid (invalid_starts=true) and we expect OccupiedStartException to be thrown In the second, the goal cell is invalid (invalid_starts=false) and we expect OccupiedGoalException In the third, both are invalid, so we don't check the precise exception type (check_exception_type=false)

Parameters
plannerThe planner
start_cellsposes to try to plan from
goal_cellsposes to try to plan to
test_namename of this test configuration for verbose printing
check_exception_typeIf true, requires that OccupiedStartException/OccupiedGoalException thrown appropriately
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
invalid_startsWhether the start poses are invalid. Otherwise its the goals
Returns
False if there is a single failure

Definition at line 167 of file global_planner_tests.cpp.

bool global_planner_tests::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 costmap.

This attempts plans from every start_cell to every goal_cell. We want to test three different modes

  • OutOfBounds cell to free cell
  • Free cell to OutOfBounds cell
  • OutOfBounds cell to OutOfBounds cell

In the first case, the start cell is invalid (invalid_starts=true) and we expect StartBoundsException to be thrown In the second, the goal cell is invalid (invalid_starts=false) and we expect GoalBoundsException In the third, both are invalid, so we don't check the precise exception type (check_exception_type=false)

Parameters
plannerThe planner
start_cellsposes to try to plan from
goal_cellsposes to try to plan to
test_namename of this test configuration for verbose printing
check_exception_typeIf true, requires that StartBoundsException/GoalBoundsException thrown appropriately
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
invalid_startsWhether the start poses are invalid. Otherwise its the goals
Returns
False if there is a single failure

Definition at line 224 of file global_planner_tests.cpp.

bool global_planner_tests::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.

Parameters
plannerThe planner
free_cellsList of poses to check
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
Returns
False if there is a single failure

Definition at line 137 of file global_planner_tests.cpp.

PoseList global_planner_tests::createPosesOutsideCostmap ( const nav_core2::Costmap costmap)

Create a list of poses that are outside the costmap's bounds.

Parameters
costmapSource of the bounds data
Returns
vector of four poses that are outside of the costmap's bounds

Definition at line 69 of file global_planner_tests.cpp.

void global_planner_tests::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 occupied cells.

Parameters
costmapSource of the cells
free_cellsOutput parameter with one pose in every free cell of the costmap
occupied_cellsOutput parameter with one pose in every occupied cell of the costmap
include_edgesTrue if you want to include the cells on the border of the costmap

For simplicity, the edge cells are ignored, meaning planners need not implement robust boundary checking

Definition at line 42 of file global_planner_tests.cpp.

bool global_planner_tests::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.

Calls checkValidPathCoverage(), checkOccupiedPathCoverage() and checkOutOfBoundsPathCoverage()

The first checks if there is a path between every set of free cells. It's likely a bit of overkill to check for the exceptions getting thrown for every combination of free/occupied/OutOfBounds cells. Hence, for the latter two, we limit our tests to a subset of the poses. If N=max_failure_cases, we check between combinations of N free cells, N occupied cells and 4 OutOfBounds cells.

We also allow for none of the tests to check the exact exception type in case you want to use this to test a nav_core planner with the nav_core_adapter (which just throws the generic PlannerException whenever path planning fails)

Parameters
plannerThe planner
costmapCostmap to get poses from
max_failure_casesNumber of poses to limit our lists to when checking for path planning failures
check_exception_typeIf true, requires the correct exception thrown
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
Returns
False if there is a single failure

Definition at line 282 of file global_planner_tests.cpp.

bool global_planner_tests::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.

Parameters
plannerThe planner
costmapCostmap to get poses from
check_exception_typeIf true, requires the correct exception thrown
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
Returns
False if there is a single failure

Definition at line 345 of file global_planner_tests.cpp.

bool global_planner_tests::many_map_test_suite ( nav_core2::GlobalPlanner planner,
TFListenerPtr  tf,
const std::string &  planner_name,
const std::string &  maps_list_filename = "package://global_planner_tests/config/standard_tests.yaml",
bool  check_exception_type = true,
bool  verbose = false,
bool  quit_early = true 
)

Run a collection of global_planner_tests on the provided maps.

Parameters
plannerThe planner
tfUsed when initializing the planner
planner_nameUsed when initializing the planner
maps_list_filenameFilename to load the list of provided maps from
check_exception_typeIf true, requires the correct exception thrown
verboseIf true, will print statistics at the end
quit_earlyWill stop running tests once there is one failure
Returns
False if there is a single failure

Definition at line 46 of file many_map_test_suite.cpp.

bool global_planner_tests::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 exception.

Parameters
plannerThe planner
startStarting pose
goalGoal pose

Definition at line 124 of file global_planner_tests.cpp.

std::string global_planner_tests::resolve_filename ( const std::string &  filename)

Replace a package:// prefix with the actual path to the given package.

This is a quick dirty way to resolve package names to strings. The alternative is resource_retriever, but that returns a file object as opposed to the updated filename.

Parameters
filenameinput file path
Returns
updated file path

Definition at line 43 of file util.cpp.

PoseList global_planner_tests::subsetPoseList ( const PoseList cells,
unsigned int  num_cells 
)

Create a new list of poses from cells that is num_cells long.

Parameters
cellsSource list of cells
num_cellsNumber of cells that should be in the return list
Returns
vector of poses populated from cells

Definition at line 90 of file global_planner_tests.cpp.



global_planner_tests
Author(s):
autogenerated on Sun Jan 10 2021 04:08:30