//[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()); }