private void test_path_planner(int grid_dimension, int cellSize_mm)
{
int grid_centre_x_mm = 0;
int grid_centre_y_mm = 0;
bool[][] navigable_space = new bool[grid_dimension][];
for (int j = 0; j < navigable_space.Length; j++)
navigable_space[j] = new bool[grid_dimension];
pathplanner planner = new pathplanner(navigable_space, cellSize_mm, grid_centre_x_mm, grid_centre_y_mm);
// create a test map
int w = grid_dimension / 20;
planner.AddNavigableSpace((grid_dimension / 2)-w, grid_dimension / 50, w + (grid_dimension / 2), grid_dimension - (grid_dimension / 50));
planner.AddNavigableSpace((grid_dimension / 2) - w, (grid_dimension * 70 / 100), grid_dimension - (grid_dimension / 50), (grid_dimension * 70 / 100) + (w * 2));
planner.AddNavigableSpace((grid_dimension / 5), (grid_dimension * 30 / 100), grid_dimension / 2, (grid_dimension * 30 / 100) + (w * 2));
planner.AddNavigableSpace((grid_dimension / 20), (grid_dimension / 10), grid_dimension * 44 / 100, (grid_dimension * 50 / 100));
planner.AddNavigableSpace((grid_dimension / 2), (grid_dimension * 20 / 100), grid_dimension - (grid_dimension / 50), (grid_dimension * 20 / 100) + (w * 2));
planner.AddNavigableSpace((grid_dimension * 80 / 100), (grid_dimension * 50 / 100), (grid_dimension * 80 / 100) + (w * 2), (grid_dimension * 70 / 100));
planner.AddNavigableSpace((grid_dimension * 57 / 100), (grid_dimension * 31 / 100), (grid_dimension * 95 / 100), (grid_dimension * 69 / 100));
planner.AddNavigableSpace((grid_dimension / 20), (grid_dimension * 52 / 100), grid_dimension * 44 / 100, (grid_dimension * 95 / 100));
planner.AddNavigableSpace((grid_dimension * 20 / 100), (grid_dimension * 80 / 100), (grid_dimension * 50 / 100), (grid_dimension * 80 / 100) + (w * 2));
planner.Update(0, 0, grid_dimension - 1, grid_dimension - 1);
ArrayList plan = new ArrayList();
int start_x=0, start_y=0, finish_x=0, finish_y=0;
int i = 0;
bool found = false;
while ((i < 10000) && (!found))
{
start_x = 1 + rnd.Next(grid_dimension - 3);
start_y = 1 + rnd.Next(grid_dimension - 3);
if ((navigable_space[start_x][start_y]) &&
(navigable_space[start_x-1][start_y-1]) &&
(navigable_space[start_x - 1][start_y]))
found = true;
i++;
}
if (found)
{
found = false;
i = 0;
while ((i < 10000) && (!found))
{
finish_x = 1 + rnd.Next(grid_dimension - 3);
finish_y = 1 + rnd.Next(grid_dimension - 3);
if ((navigable_space[finish_x][finish_y]) &&
(navigable_space[finish_x-1][finish_y-1]) &&
(navigable_space[finish_x - 1][finish_y]))
found = true;
i++;
}
if (found)
{
int start_xx = ((start_x - (grid_dimension / 2)) * cellSize_mm) + grid_centre_x_mm;
int start_yy = ((start_y - (grid_dimension / 2)) * cellSize_mm) + grid_centre_y_mm;
int finish_xx = ((finish_x - (grid_dimension / 2)) * cellSize_mm) + grid_centre_x_mm;
int finish_yy = ((finish_y - (grid_dimension / 2)) * cellSize_mm) + grid_centre_y_mm;
plan = planner.CreatePlan(start_xx, start_yy, finish_xx, finish_yy);
}
}
planner.Show(img_rays, standard_width, standard_height, plan);
drawing.drawCircle(img_rays, standard_width, standard_height,
start_x * standard_width / grid_dimension,
start_y * standard_height / grid_dimension,
standard_width / 100, 255, 0, 0, 1);
drawing.drawCircle(img_rays, standard_width, standard_height,
finish_x * standard_width / grid_dimension,
finish_y * standard_height / grid_dimension,
standard_width / 100, 255, 0, 255, 1);
}
/// <summary>
/// create a path planner for the given grid
/// </summary>
/// <param name="grid">occupancy grid</param>
private void createPlanner(occupancygridMultiHypothesis grid)
{
if (planner == null)
planner = new sentience.pathplanner.pathplanner(grid.navigable_space, (int)LocalGridCellSize_mm, grid.x, grid.y);
else
planner.navigable_space = grid.navigable_space;
}
