/// <summary>
/// run the simulation, one step at a time
/// </summary>
/// <param name="images">stereo image bitmaps for this time step</param>
public void RunOneStep(List <byte[]> images)
{
if (path != null)
{
// position the grid so that the path fits inside it
rob.SetLocalGridPosition(min_x - ((max_x - min_x) / 2),
min_y + ((max_y - min_y) / 2),
0);
if (images.Count > 1)
{
float forward_velocity = (float)velocities[current_time_step * 2];
float angular_velocity = (float)velocities[(current_time_step * 2) + 1];
// get the robots position according to the path data
float actual_x = ((particlePose)path.path[current_time_step]).x;
float actual_y = ((particlePose)path.path[current_time_step]).y;
rob.updateFromVelocities(images, forward_velocity, angular_velocity, time_per_index_sec);
// calculate the position error
float err_x = actual_x - rob.x;
float err_y = actual_y - rob.y;
position_error_mm = (float)Math.Sqrt((err_x * err_x) + (err_y * err_y));
}
// increment the simulation time step
if (current_time_step < path.path.Count - 2)
{
current_time_step++;
}
}
}
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());
}
//[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);
}
