public void MoveToNextLocalGrid() { int no_of_grids = 2; int grid_type = metagrid.TYPE_SIMPLE; int dimension_mm = 3000; int dimension_vertical_mm = 2000; int cellSize_mm = 32; int localisationRadius_mm = 2000; int maxMappingRange_mm = 2000; float vacancyWeighting = 0.5f; int overall_img_width = 640; int overall_img_height = 480; int current_grid_index = 0; int current_disparity_index = 0; pos3D robot_pose = new pos3D(0, 0, 0); metagrid[] buffer = new metagrid[2]; int current_buffer_index = 0; List <float> grid_centres = new List <float>(); bool update_map = false; float grid_centre_x_mm; float grid_centre_y_mm; float grid_centre_z_mm; // create some grid centres along a straight line path float path_length_mm = 10000; int steps = (int)(path_length_mm / (dimension_mm / 2)); for (int i = 0; i < steps; i++) { grid_centre_x_mm = 0; grid_centre_y_mm = i * path_length_mm / steps; grid_centre_z_mm = 0; grid_centres.Add(grid_centre_x_mm); grid_centres.Add(grid_centre_y_mm); grid_centres.Add(grid_centre_z_mm); } // create the buffer for (int i = 0; i < 2; i++) { buffer[i] = new metagrid( no_of_grids, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); } // move along a straight line path int transitions = 0; for (int y = 0; y < path_length_mm; y += 100) { robot_pose.y = y; byte[] overall_map_img = null; if (metagridBuffer.MoveToNextLocalGrid( ref current_grid_index, ref current_disparity_index, robot_pose, buffer, ref current_buffer_index, grid_centres, ref update_map, null, null, ref overall_map_img, overall_img_width, overall_img_height, 0, 0, 0)) { ; } { transitions++; } current_disparity_index = 1; } Assert.AreEqual(steps - 1, transitions, "Incorrect number of local grid transitions"); Assert.AreEqual(steps - 1, current_grid_index, "Did not reach the final grid"); }
public void CreateSimulatedEnvironment() { int no_of_grids = 2; int grid_type = metagrid.TYPE_SIMPLE; int dimension_mm = 8000; int room_height_mm = 2500; int cellSize_mm = 32; int localisationRadius_mm = 8000; int maxMappingRange_mm = 16000; float vacancyWeighting = 0.5f; metagrid grid = new metagrid( no_of_grids, grid_type, dimension_mm, room_height_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); int room_tx_mm = -(1050 / 2); int room_ty_mm = -(dimension_mm / 2) * 80 / 100; int room_bx_mm = 1050 / 2; int room_by_mm = room_ty_mm + 6000; int doorway_height_mm = 2000; int doorway_width_mm = 740; int wall_thickness_mm = 100; List <int> left_wall_doorways = new List <int>(); List <int> top_wall_doorways = new List <int>(); List <int> right_wall_doorways = new List <int>(); List <int> bottom_wall_doorways = new List <int>(); int floor_r = 0; int floor_g = 0; int floor_b = 0; int walls_r = 0; int walls_g = 0; int walls_b = 255; float probability_variance = 0.1f; left_wall_doorways.Add(900); left_wall_doorways.Add(4000); left_wall_doorways.Add(5200); right_wall_doorways.Add(450); right_wall_doorways.Add(4000); grid.InsertRoom( room_tx_mm, room_ty_mm, room_bx_mm, room_by_mm, room_height_mm, wall_thickness_mm, probability_variance, floor_r, floor_g, floor_b, walls_r, walls_g, walls_b, left_wall_doorways, top_wall_doorways, right_wall_doorways, bottom_wall_doorways, doorway_width_mm, doorway_height_mm); int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); bool show_localisation = false; for (int i = 0; i < no_of_grids; i++) { grid.Show(i, debug_img, debug_img_width, debug_img_height, false, show_localisation); BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_meta_CreateSimulatedEnvironment_grid" + i.ToString() + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp); } grid.ExportToIFrIT("tests_occupancygrid_meta_CreateSimulatedEnvironment.txt"); }