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");
}
/// <summary>
/// initialise the buffer
/// </summary>
/// <param name="no_of_grid_levels">The number of sub grids</param>
/// <param name="grid_type">the type of sub grids</param>
/// <param name="dimension_mm">dimension of the smallest sub grid</param>
/// <param name="dimension_vertical_mm">vertical dimension of the smallest sub grid</param>
/// <param name="cellSize_mm">cell size of the smallest sub grid</param>
/// <param name="localisationRadius_mm">localisation radius within the smallest sub grid</param>
/// <param name="maxMappingRange_mm">maximum mapping radius within the smallest sub grid</param>
/// <param name="vacancyWeighting">vacancy model weighting, typically between 0.2 and 2</param>
public void Initialise(
int no_of_grid_levels,
int grid_type,
int dimension_mm,
int dimension_vertical_mm,
int cellSize_mm,
int localisationRadius_mm,
int maxMappingRange_mm,
float vacancyWeighting)
{
this.dimension_mm = dimension_mm;
this.dimension_vertical_mm = dimension_vertical_mm;
this.no_of_grid_levels = no_of_grid_levels;
this.grid_type = grid_type;
this.cellSize_mm = cellSize_mm;
this.localisationRadius_mm = localisationRadius_mm;
this.maxMappingRange_mm = maxMappingRange_mm;
this.vacancyWeighting = vacancyWeighting;
// create the buffer
buffer = new metagrid[2];
for (int i = 0; i < 2; i++)
{
buffer[i] =
new metagrid(
no_of_grid_levels,
grid_type,
dimension_mm,
dimension_vertical_mm,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
}
current_buffer_index = 0;
current_grid_index = 0;
grid_centres = new List<float>();
localisations = new List<float>();
disparities_index = new List<int>();
}
/// <summary>
/// moves to the next grid in the sequence, if necessary
/// </summary>
/// <param name="current_grid_index">index of the current local grid</param>
/// <param name="current_disparity_index">index of teh current disparities set within the disparities file</param>
/// <param name="robot_pose">the current robot pose</param>
/// <param name="buffer">buffer containing two metagrids</param>
/// <param name="current_buffer_index">index of the currently active grid within the buffer (0 or 1)</param>
/// <param name="grid_centres">list of grid centre positions (x,y,z)</param>
/// <param name="update_map">returns whether the map should be updated</param>
/// <param name="debug_mapping_filename">filename to save debugging images</param>
/// <param name="overall_map_filename">filename to save an overall map of the path</param>
/// <param name="overall_map_img">overall map image data</param>
/// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param>
/// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param>
/// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param>
/// <returns>true if we have transitioned from one grid to the next</returns>
public static bool MoveToNextLocalGrid(
ref int current_grid_index,
ref int current_disparity_index,
pos3D robot_pose,
metagrid[] buffer,
ref int current_buffer_index,
List<float> grid_centres,
ref bool update_map,
string debug_mapping_filename,
string overall_map_filename,
ref byte[] overall_map_img,
int overall_img_width,
int overall_img_height,
int overall_map_dimension_mm,
int overall_map_centre_x_mm,
int overall_map_centre_y_mm)
{
bool buffer_transition = false;
update_map = false;
// if this is the first time that localisation
// has been called since loading the path
// then update the map
if ((current_grid_index == 0) &&
(current_disparity_index == 0))
{
update_map = true;
float grid_centre_x_mm = grid_centres[current_grid_index * 3];
float grid_centre_y_mm = grid_centres[(current_grid_index * 3) + 1];
float grid_centre_z_mm = grid_centres[(current_grid_index * 3) + 2];
buffer[current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0);
int next_grid_index = current_grid_index + 1;
if (next_grid_index >= grid_centres.Count / 3) next_grid_index = current_grid_index;
grid_centre_x_mm = grid_centres[next_grid_index * 3];
grid_centre_y_mm = grid_centres[(next_grid_index * 3) + 1];
grid_centre_z_mm = grid_centres[(next_grid_index * 3) + 2];
buffer[1 - current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0);
}
// distance to the centre of the currently active grid
float dx = robot_pose.x - buffer[current_buffer_index].x;
float dy = robot_pose.y - buffer[current_buffer_index].y;
float dz = robot_pose.z - buffer[current_buffer_index].z;
float dist_to_grid_centre_sqr_0 = dx*dx + dy*dy + dz*dz;
dx = robot_pose.x - buffer[1 - current_buffer_index].x;
dy = robot_pose.y - buffer[1 - current_buffer_index].y;
dz = robot_pose.z - buffer[1 - current_buffer_index].z;
float dist_to_grid_centre_sqr_1 = dx*dx + dy*dy + dz*dz;
// if we are closer to the next grid than the current one
// then swap the currently active grid
//if (dist_to_grid_centre_sqr_0 > dimension_mm/2)
if (dist_to_grid_centre_sqr_1 < dist_to_grid_centre_sqr_0)
{
if ((debug_mapping_filename != null) &&
(debug_mapping_filename != ""))
{
bool show_localisation = true;
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
Bitmap debug_bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
buffer[current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false, show_localisation);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp);
if (debug_mapping_filename.ToLower().EndsWith("png"))
debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Png);
if (debug_mapping_filename.ToLower().EndsWith("gif"))
debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Gif);
if (debug_mapping_filename.ToLower().EndsWith("jpg"))
debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (debug_mapping_filename.ToLower().EndsWith("bmp"))
debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Bmp);
/*
string[] str = debug_mapping_filename.Split('.');
string debug_mapping_filename2 = str[0] + "b." + str[1];
buffer[1 - current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp);
if (debug_mapping_filename2.ToLower().EndsWith("png"))
debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Png);
if (debug_mapping_filename2.ToLower().EndsWith("gif"))
debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Gif);
if (debug_mapping_filename2.ToLower().EndsWith("jpg"))
debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Jpeg);
if (debug_mapping_filename2.ToLower().EndsWith("bmp"))
debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Bmp);
*/
}
UpdateOverallMap(
buffer,
current_buffer_index,
overall_map_filename,
ref overall_map_img,
overall_img_width,
overall_img_height,
overall_map_dimension_mm,
overall_map_centre_x_mm,
overall_map_centre_y_mm);
// swap the two metagrids
SwapBuffers(
grid_centres,
ref current_grid_index,
buffer,
ref current_buffer_index,
ref update_map);
buffer_transition = true;
}
return(buffer_transition);
}
/// <summary>
/// updates the image of the overall map
/// </summary>
/// <param name="overall_map_filename">filename to save an overall map of the path</param>
/// <param name="overall_map_img">overall map image data</param>
/// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param>
/// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param>
/// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param>
public static void UpdateOverallMap(
metagrid[] buffer,
int current_buffer_index,
string overall_map_filename,
ref byte[] overall_map_img,
int overall_img_width,
int overall_img_height,
int overall_map_dimension_mm,
int overall_map_centre_x_mm,
int overall_map_centre_y_mm)
{
if ((overall_map_filename != null) &&
(overall_map_filename != ""))
{
bool show_localisation = false;
int overall_map_dimension2_mm = overall_map_dimension_mm * overall_img_height / overall_img_width;
bool clear_image = false;
if (overall_map_img == null)
{
overall_map_img = new byte[overall_img_width * overall_img_height * 3];
clear_image = true;
}
Bitmap overall_bmp = new Bitmap(overall_img_width, overall_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
buffer[current_buffer_index].Show(0, overall_map_img, overall_img_width, overall_img_height, false, overall_map_dimension_mm, overall_map_dimension2_mm, overall_map_centre_x_mm, overall_map_centre_y_mm, clear_image, show_localisation);
BitmapArrayConversions.updatebitmap_unsafe(overall_map_img, overall_bmp);
if (overall_map_filename.ToLower().EndsWith("png"))
overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Png);
if (overall_map_filename.ToLower().EndsWith("gif"))
overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Gif);
if (overall_map_filename.ToLower().EndsWith("jpg"))
overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (overall_map_filename.ToLower().EndsWith("bmp"))
overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
}
/// <summary>
/// swaps the two metagrids, clears the previous grid and assigns it the next centre position
/// </summary>
/// <param name="grid_centres">list of grid centre positions (x,y,z)</param>
/// <param name="current_grid_index">index of the current grid which we are in</param>
/// <param name="buffer">grid buffer, consisting of two metagrids</param>
/// <param name="current_buffer_index">index of the currently active buffer - i.e. which of the two we are currently in</param>
/// <param name="update_map">returns whether or not to update with new mapping rays</param>
public static void SwapBuffers(
List<float> grid_centres,
ref int current_grid_index,
metagrid[] buffer,
ref int current_buffer_index,
ref bool update_map)
{
int curr_buffer = 1 - current_buffer_index;
int next_buffer = current_buffer_index;
int no_of_grids = grid_centres.Count / 3;
current_buffer_index = curr_buffer;
if (current_grid_index < no_of_grids)
{
// move into the next grid
current_grid_index++;
if (current_grid_index < no_of_grids - 1)
{
// clear the grid which we have just passed through
buffer[next_buffer].Clear();
// update the next map
update_map = true;
// retrieve the next grid centre position
int next_grid_index = current_grid_index + 1;
float next_grid_centre_x = grid_centres[(next_grid_index * 3)];
float next_grid_centre_y = grid_centres[(next_grid_index * 3) + 1];
float next_grid_centre_z = grid_centres[(next_grid_index * 3) + 2];
// set the next grid centre
buffer[next_buffer].SetPosition(next_grid_centre_x, next_grid_centre_y, next_grid_centre_z, 0.0f);
}
}
}
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");
}
