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