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; }