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