/// <summary>
/// update the robot's map
/// </summary>
/// <param name="state">robot state</param>
private static void Update(ThreadMappingState state)
{
// object used for taking benchmark timings
stopwatch clock = new stopwatch();
clock.Start();
// update all current poses with the observed rays
state.motion.LocalGrid = state.grid;
state.motion.AddObservation(state.stereo_rays, false);
clock.Stop();
state.benchmark_observation_update = clock.time_elapsed_mS;
// what's the relative position of the robot inside the grid ?
pos3D relative_position = new pos3D(state.pose.x - state.grid.x, state.pose.y - state.grid.y, 0);
relative_position.pan = state.pose.pan - state.grid.pan;
clock.Start();
// garbage collect dead occupancy hypotheses
state.grid.GarbageCollect();
clock.Stop();
state.benchmark_garbage_collection = clock.time_elapsed_mS;
}
/// <summary>
/// update the robot's map
/// </summary>
/// <param name="state">robot state</param>
private static void Update(ThreadMappingState state)
{
// object used for taking benchmark timings
stopwatch clock = new stopwatch();
clock.Start();
// update all current poses with the observed rays
state.motion.LocalGrid = state.grid;
state.motion.AddObservation(state.stereo_rays, false);
clock.Stop();
state.benchmark_observation_update = clock.time_elapsed_mS;
// what's the relative position of the robot inside the grid ?
pos3D relative_position = new pos3D(state.pose.x - state.grid.x, state.pose.y - state.grid.y, 0);
relative_position.pan = state.pose.pan - state.grid.pan;
clock.Start();
// garbage collect dead occupancy hypotheses
state.grid.GarbageCollect();
clock.Stop();
state.benchmark_garbage_collection = clock.time_elapsed_mS;
}
/// <summary>
/// ThreadStart delegate
/// </summary>
public void Execute()
{
ThreadMappingState state = (ThreadMappingState)_data;
Update(state);
state.active = false;
_callback(_data);
}
/// <summary>
/// mapping update, which can consist of multiple threads running concurrently
/// </summary>
/// <param name="stereo_rays">rays to be thrown into the grid map</param>
private void MappingUpdate(List<evidenceRay>[] stereo_rays)
{
// list of currently active threads
List<ThreadMappingState> activeThreads = new List<ThreadMappingState>();
// create a set of threads
Thread[] mapping_thread = new Thread[mapping_threads];
for (int th = 0; th < mapping_threads; th++)
{
// create a state for the thread
ThreadMappingState state = new ThreadMappingState();
state.active = true;
state.pose = new pos3D(x, y, z);
state.pose.pan = pan;
state.motion = motion[th];
state.grid = LocalGrid[th];
state.stereo_rays = stereo_rays;
// add this state to the threads to be processed
ThreadMappingUpdate mapupdate = new ThreadMappingUpdate(new WaitCallback(MappingUpdateCallback), state);
mapping_thread[th] = new Thread(new ThreadStart(mapupdate.Execute));
mapping_thread[th].Name = "occupancy grid map " + th.ToString();
mapping_thread[th].Priority = ThreadPriority.AboveNormal;
activeThreads.Add(state);
}
// clear benchmarks
benchmark_observation_update = 0;
benchmark_garbage_collection = 0;
clock.Start();
// start all threads
for (int th = 0; th < mapping_threads; th++)
mapping_thread[th].Start();
// now sit back and wait for all threads to complete
while (activeThreads.Count > 0)
{
for (int th = activeThreads.Count - 1; th >= 0; th--)
{
// is this thread still active?
ThreadMappingState state = (ThreadMappingState)activeThreads[th];
if (!state.active)
{
// remove from the list of active threads
activeThreads.RemoveAt(th);
// update benchmark timings
benchmark_observation_update += state.benchmark_observation_update;
benchmark_garbage_collection += state.benchmark_garbage_collection;
}
}
}
clock.Stop();
long parallel_processing_time = clock.time_elapsed_mS;
// how much time have we gained from multithreading ?
long serial_processing_time = benchmark_observation_update + benchmark_garbage_collection;
long average_update_time = serial_processing_time / mapping_threads;
benchmark_concurrency = serial_processing_time - parallel_processing_time;
// average benchmarks
benchmark_observation_update /= mapping_threads;
benchmark_garbage_collection /= mapping_threads;
// compare the results and see which is the best looking map
pos3D best_pose = null;
float max_path_score = 0;
for (int th = 0; th < mapping_threads; th++)
{
if ((motion[th].current_robot_path_score > max_path_score) ||
(th == 0))
{
max_path_score = motion[th].current_robot_path_score;
best_pose = motion[th].current_robot_pose;
best_grid_index = th;
}
}
// update the robots position and orientation
// with the pose from the highest scoring path
// across all maps
if (best_pose != null)
{
x = best_pose.x;
y = best_pose.y;
z = best_pose.z;
pan = best_pose.pan;
}
}
