/// <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++;
}
}
}
