//[Test()]
public static void CreateSim()
{
int dimension_cells = 100;
int dimension_cells_vertical = 20;
int cellSize_mm = 50;
int localisationRadius_mm = 2000;
int maxMappingRange_mm = 2000;
float vacancyWeighting = 0.8f;
int map_dim = 14;
byte[] map = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,1,1,1,1,0,0,0,0,0,0,
0,0,0,0,1,0,0,1,0,0,0,0,0,0,
0,0,0,0,1,0,0,1,0,0,0,0,0,0,
0,0,0,0,1,0,0,1,1,1,1,0,0,0,
0,0,0,0,1,0,0,0,0,0,1,0,0,0,
0,0,0,0,1,0,0,0,0,0,1,0,0,0,
0,0,0,0,1,0,0,0,0,0,1,0,0,0,
0,0,0,0,1,1,1,0,1,1,1,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
dpslam sim = CreateSimulation(map_dim, map, 50);
particlePose pose = null;
int img_width = 640;
byte[] img = new byte[img_width * img_width * 3];
sim.Show(0, img, img_width, img_width, pose, true, true);
Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("dpslam_tests_CreateSimulation1.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
robot rob = new robot(1);
rob.WheelBase_mm = 90;
rob.WheelDiameter_mm = 30;
rob.x = 0;
rob.y = 0;
rob.pan = 90 * (float)Math.PI / 180.0f;
rob.head.cameraFOVdegrees[0] = 90;
rob.head.cameraSensorSizeMm[0] = 4.17f;
rob.head.cameraFocalLengthMm[0] = 3.6f;
rob.head.cameraImageWidth[0] = 320;
rob.head.cameraImageHeight[0] = 240;
rayModelLookup sensor_model = new rayModelLookup(10, 10);
sensor_model.InitSurveyorSVS();
rob.head.sensormodel[0] = sensor_model;
float time_elapsed_sec = 1;
float forward_velocity = 50;
float angular_velocity_pan = 0;
float angular_velocity_tilt = 0;
float angular_velocity_roll = 0;
float min_x_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
float min_y_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
float max_x_mm = -min_x_mm;
float max_y_mm = -min_y_mm;
for (float t = 0.0f; t < 4; t += time_elapsed_sec)
{
rob.updateFromVelocities(sim, forward_velocity, angular_velocity_pan, angular_velocity_tilt, angular_velocity_roll, time_elapsed_sec);
Console.WriteLine("xy: " + rob.x.ToString() + " " + rob.y.ToString());
rob.motion.Show(img, img_width, img_width, min_x_mm, min_y_mm, max_x_mm, max_y_mm, true, false, t == 0.0f);
}
rob.SaveGridImage("dpslam_tests_CreateSimulation2.bmp");
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("dpslam_tests_CreateSimulation3.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
}
public void SpeedControl()
{
int image_width = 640;
byte[] img_rays = new byte[image_width*image_width*3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
bool closed_loop = false;
robot rob = new robot(1);
int min_x_mm = 0;
int min_y_mm = 0;
int max_x_mm = 1000;
int max_y_mm = 1200;
int step_size = (max_y_mm - min_y_mm) / 15;
int x = min_x_mm + ((max_x_mm - min_x_mm) / 2);
bool initial = true;
float pan = 0; // (float)Math.PI / 4;
rob.x = x;
rob.y = 0;
for (int y = min_y_mm; y <= max_y_mm; y += step_size)
{
if (closed_loop) surveyPosesDummy(rob);
List<ushort[]> stereo_matches = new List<ushort[]>();
float forward_velocity = step_size;
float angular_velocity_pan = 0 * (float)Math.PI / 180.0f;
rob.updateFromVelocities(stereo_matches, forward_velocity, angular_velocity_pan, 0, 0, 1.0f);
rob.motion.Show(
img_rays, image_width, image_width,
min_x_mm, min_y_mm, max_x_mm, max_y_mm,
true, false,
initial);
initial = false;
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_SpeedControl.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(rob.y > max_y_mm-50, "The robot did not move far enough " + rob.y.ToString());
}