/// <summary>
/// create a map using all available track data
/// </summary>
/// <param name="grid"></param>
public void testMapping(occupancygridMultiResolution grid)
{
robotPath pathLocalisation = getLocalisationTrack(0);
robotPath pathMap = getMappingTrack(0);
grid.Clear();
grid.insert(pathMap);
grid.insert(pathLocalisation);
}
/// <summary>
/// add a track to the list of test tracks
/// </summary>
/// <param name="track"></param>
/// <param name="use_for_mapping"></param>
public void Add(robotPath track, Boolean use_for_mapping)
{
if (use_for_mapping)
{
mapping_tracks.Add(track);
noOfMappingTracks = mapping_tracks.Count;
}
else
{
localisation_tracks.Add(track);
noOfLocalisationTracks = localisation_tracks.Count;
}
}
/// <summary>
/// add a track to the list of test tracks
/// </summary>
/// <param name="track"></param>
/// <param name="use_for_mapping"></param>
public void Add(robotPath track, Boolean use_for_mapping)
{
if (use_for_mapping)
{
mapping_tracks.Add(track);
noOfMappingTracks = mapping_tracks.Count;
}
else
{
localisation_tracks.Add(track);
noOfLocalisationTracks = localisation_tracks.Count;
}
}
public void testLocalisation(robot rob,
int localisation_track_index,
int localisation_test_index,
occupancygridMultiResolution grid,
int ray_thickness,
int no_of_trial_poses,
bool pruneSurvey,
int survey_diameter_mm,
int randomSeed, int pruneThreshold, float survey_angular_offset,
float momentum,
ref float error_x, ref float error_y, ref float error_pan,
pos3D estimatedPos)
{
robotPath pathLocalisation = getLocalisationTrack(localisation_track_index);
robotPath pathMap = getMappingTrack(localisation_track_index);
viewpoint view_localisation = pathLocalisation.getViewpoint(localisation_test_index);
viewpoint view_map = pathMap.getViewpoint(localisation_test_index);
float max_score = 0;
// position estimate from odometry, which positions the robot within the grid
// as an initial estimate from which to search
pos3D local_odometry_position = view_map.odometry_position.subtract(pathMap.pathCentre());
ArrayList survey_results = rob.sensorModelLocalisation.surveyXYP(view_localisation, grid, survey_diameter_mm, survey_diameter_mm,
no_of_trial_poses, ray_thickness, pruneSurvey, randomSeed,
pruneThreshold, survey_angular_offset,
local_odometry_position, momentum,
ref max_score);
float peak_x = 0;
float peak_y = 0;
float peak_pan = 0;
rob.sensorModelLocalisation.SurveyPeak(survey_results, ref peak_x, ref peak_y);
//float[] peak = rob.sensorModel.surveyPan(view_map, view_localisation, separation_tollerance, ray_thickness, peak_x, peak_y);
float[] peak = rob.sensorModelLocalisation.surveyPan(view_localisation, grid, ray_thickness, peak_x, peak_y);
peak_pan = rob.sensorModelLocalisation.SurveyPeakPan(peak);
float half_track_length = 1000;
float dx = view_localisation.odometry_position.x - view_map.odometry_position.x;
float dy = view_localisation.odometry_position.y - (view_map.odometry_position.y - half_track_length);
float dp = view_localisation.odometry_position.pan - view_map.odometry_position.pan;
error_x = dx + peak_x;
error_y = dy + peak_y;
error_pan = dp + peak_pan;
error_pan = error_pan / (float)Math.PI * 180;
estimatedPos.x = view_map.odometry_position.x - peak_x;
estimatedPos.y = view_map.odometry_position.y - peak_y;
estimatedPos.pan = view_map.odometry_position.pan - peak_pan;
}
private robotPath getPath(robot rob, String path, String track, String index, int starting_index, String TrackName, int trackType)
{
robotPath rpath = new robotPath();
robotOdometry odometry = new robotOdometry();
odometry.readPathFileSentience(path + track + "\\" + TrackName + ".path", index, rob);
switch (trackType)
{
case 1:
{
// mapping X track
standardXTrack(2000, 500, odometry, index);
break;
}
case 2:
{
//localisation X track
standardXTrack(2000, -500, odometry, index);
break;
}
case 3:
{
//there and back track
odometry.Clear();
thereAndBack(100, 46, odometry, 110, 0, 0);
break;
}
case 4:
{
//square
odometry.Clear();
squareAndBack(100, 10, 6, odometry, 110);
break;
}
case 5:
{
//special
odometry.Clear();
specialTrack(100, 16, 21, 3, odometry, 110);
break;
}
}
if (odometry.no_of_measurements == 0)
{
errorMessage = path + track + "\\" + TrackName + ".path not found or contains no data. ";
errorMessage += "Check that the path file is included as 'Content' within the project folder";
}
for (int distance_index = 0; distance_index < odometry.no_of_measurements; distance_index++)
{
pos3D pos = odometry.getPosition(distance_index);
rob.x = pos.x;
rob.y = pos.y;
rob.z = pos.z;
rob.pan = pos.pan;
rob.tilt = pos.tilt;
rob.roll = pos.roll;
loadGlimpseSentience(rob, path + track + "\\", TrackName, index, distance_index + starting_index, odometry.no_of_measurements, index);
rob.updatePath(rpath);
}
return (rpath);
}
private robotPath getPath(robot rob, String path, String track, String index, int starting_index, String TrackName, int trackType)
{
robotPath rpath = new robotPath();
robotOdometry odometry = new robotOdometry();
odometry.readPathFileSentience(path + track + "\\" + TrackName + ".path", index, rob);
switch (trackType)
{
case 1:
{
// mapping X track
standardXTrack(2000, 500, odometry, index);
break;
}
case 2:
{
//localisation X track
standardXTrack(2000, -500, odometry, index);
break;
}
case 3:
{
//there and back track
odometry.Clear();
thereAndBack(100, 46, odometry, 110, 0, 0);
break;
}
case 4:
{
//square
odometry.Clear();
squareAndBack(100, 10, 6, odometry, 110);
break;
}
case 5:
{
//special
odometry.Clear();
specialTrack(100, 16, 21, 3, odometry, 110);
break;
}
}
if (odometry.no_of_measurements == 0)
{
errorMessage = path + track + "\\" + TrackName + ".path not found or contains no data. ";
errorMessage += "Check that the path file is included as 'Content' within the project folder";
}
for (int distance_index = 0; distance_index < odometry.no_of_measurements; distance_index++)
{
pos3D pos = odometry.getPosition(distance_index);
rob.x = pos.x;
rob.y = pos.y;
rob.z = pos.z;
rob.pan = pos.pan;
rob.tilt = pos.tilt;
rob.roll = pos.roll;
loadGlimpseSentience(rob, path + track + "\\", TrackName, index, distance_index + starting_index, odometry.no_of_measurements, index);
rob.updatePath(rpath);
}
return(rpath);
}
