/// <summary>
/// add a new grid hypothesis to this pose
/// </summary>
/// <param name="hypothesis">occupancy hypothesis for a grid cell</param>
/// <param name="radius_cells">local map cache radius in cells</param>
/// <param name="grid_dimension">dimension of the occupancy grid in cells</param>
/// <param name="grid_dimension_vertical">height of the occupancy grid (z axis) in cells</param>
/// <returns>true if the hypothesis was added</returns>
public bool AddHypothesis(particleGridCell hypothesis,
int radius_cells,
int grid_dimension,
int grid_dimension_vertical)
{
bool added = false;
if (path != null)
{
added = path.Add(hypothesis, radius_cells, grid_dimension, grid_dimension_vertical);
if (added)
{
observed_grid_cells.Add(hypothesis);
}
}
return(added);
}
/// <summary>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
{
// calculate noisy velocities
float fwd_velocity = forward_velocity + sample_normal_distribution(
(motion_noise[0] * Math.Abs(forward_velocity)) +
(motion_noise[1] * Math.Abs(angular_velocity)));
float ang_velocity = angular_velocity + sample_normal_distribution(
(motion_noise[2] * Math.Abs(forward_velocity)) +
(motion_noise[3] * Math.Abs(angular_velocity)));
float v = sample_normal_distribution(
(motion_noise[4] * Math.Abs(forward_velocity)) +
(motion_noise[5] * Math.Abs(angular_velocity)));
float fraction = 0;
if (Math.Abs(ang_velocity) > 0.000001f)
{
fraction = fwd_velocity / ang_velocity;
}
float current_pan = path.current_pose.pan;
// if scan matching is active use the current estimated pan angle
if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
{
current_pan = rob.ScanMatchingPanAngleEstimate;
}
float pan2 = current_pan - (ang_velocity * time_elapsed_sec);
float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
(fraction * (float)Math.Sin(pan2));
float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
(fraction * (float)Math.Cos(pan2));
float new_pan = pan2 + (v * time_elapsed_sec);
particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <summary>
/// turns a list of path segments into a list of individual poses
/// </summary>
private void updatePath()
{
particlePose prev_pose = null;
path = new particlePath(999999999);
min_x = 9999;
min_y = 9999;
max_x = -9999;
max_y = -9999;
for (int s = 0; s < pathSegments.Count; s++)
{
simulationPathSegment segment = (simulationPathSegment)pathSegments[s];
// get the last pose
if (s > 0)
{
prev_pose = (particlePose)path.path[path.path.Count - 1];
}
// update the list of poses
List <particlePose> poses = segment.getPoses();
if (prev_pose != null)
{
// is the last pose position the same as the first in this segment?
// if so, remove the last pose added to the path
particlePose firstPose = (particlePose)poses[0];
if (((int)firstPose.x == (int)prev_pose.x) &&
((int)firstPose.y == (int)prev_pose.y) &&
(Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f))
{
path.path.RemoveAt(path.path.Count - 1);
}
}
for (int i = 0; i < poses.Count; i++)
{
particlePose pose = (particlePose)poses[i];
if (pose.x < min_x)
{
min_x = pose.x;
}
if (pose.y < min_y)
{
min_y = pose.y;
}
if (pose.x > max_x)
{
max_x = pose.x;
}
if (pose.y > max_y)
{
max_y = pose.y;
}
path.Add(pose);
}
}
// update the path velocities
velocities = path.getVelocities(0, 0, time_per_index_sec);
}
/// <summary>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
{
// calculate noisy velocities
float fwd_velocity = forward_velocity + sample_normal_distribution(
(motion_noise[0] * Math.Abs(forward_velocity)) +
(motion_noise[1] * Math.Abs(angular_velocity)));
float ang_velocity = angular_velocity + sample_normal_distribution(
(motion_noise[2] * Math.Abs(forward_velocity)) +
(motion_noise[3] * Math.Abs(angular_velocity)));
float v = sample_normal_distribution(
(motion_noise[4] * Math.Abs(forward_velocity)) +
(motion_noise[5] * Math.Abs(angular_velocity)));
float fraction = 0;
if (Math.Abs(ang_velocity) > 0.000001f) fraction = fwd_velocity / ang_velocity;
float current_pan = path.current_pose.pan;
// if scan matching is active use the current estimated pan angle
if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
current_pan = rob.ScanMatchingPanAngleEstimate;
float pan2 = current_pan - (ang_velocity * time_elapsed_sec);
float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
(fraction * (float)Math.Sin(pan2));
float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
(fraction * (float)Math.Cos(pan2));
float new_pan = pan2 + (v * time_elapsed_sec);
particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <summary>
/// plan a route to a given location
/// </summary>
/// <param name="destination_waypoint"></param>
public void PlanRoute(String destination_waypoint)
{
// clear the planned path
planned_path = null;
// retrieve the waypoint from the list of work sites
kmlPlacemarkPoint waypoint = worksites.GetPoint(destination_waypoint);
if (waypoint != null)
{
// get the destination waypoint position in millimetres
// (the KML format stores positions in degrees)
float destination_x = 0, destination_y = 0;
waypoint.GetPositionMillimetres(ref destination_x, ref destination_y);
// the best performing local grid
occupancygridMultiHypothesis grid = LocalGrid[best_grid_index];
// create a planner
createPlanner(grid);
// set variables, in the unlikely case that the centre position
// of the grid has been changed since the planner was initialised
planner.init(grid.navigable_space, grid.x, grid.y);
// update the planner, in order to assign safety scores to the
// navigable space. The efficiency of this could be improved
// by updating only those areas of the map which have changed
planner.Update(0, 0, LocalGridDimension - 1, LocalGridDimension - 1);
// create the plan
List<float> new_plan = planner.CreatePlan(x, y, destination_x, destination_y);
if (new_plan.Count > 0)
{
// convert the plan into a set of poses
planned_path = new particlePath(new_plan.Count / 2);
float prev_xx = 0, prev_yy = 0;
for (int i = 0; i < new_plan.Count; i += 2)
{
float xx = (float)new_plan[i];
float yy = (float)new_plan[i + 1];
if (i > 0)
{
float dx = xx - prev_xx;
float dy = yy - prev_yy;
float dist = (float)Math.Sqrt((dx * dx) + (dy * dy));
if (dist > 0)
{
// TODO: check this pan angle
float pan = (float)Math.Sin(dx / dist);
if (dy < 0) pan = (2 * (float)Math.PI) - pan;
// create a pose and add it to the planned path
particlePose new_pose = new particlePose(prev_xx, prev_yy, pan, planned_path);
planned_path.Add(new_pose);
}
}
prev_xx = xx;
prev_yy = yy;
}
}
}
}
/// <summary>
/// turns a list of path segments into a list of individual poses
/// </summary>
private void updatePath()
{
particlePose prev_pose=null;
path = new particlePath(999999999);
min_x = 9999;
min_y = 9999;
max_x = -9999;
max_y = -9999;
for (int s = 0; s < pathSegments.Count; s++)
{
simulationPathSegment segment = (simulationPathSegment)pathSegments[s];
// get the last pose
if (s > 0)
prev_pose = (particlePose)path.path[path.path.Count - 1];
// update the list of poses
List<particlePose> poses = segment.getPoses();
if (prev_pose != null)
{
// is the last pose position the same as the first in this segment?
// if so, remove the last pose added to the path
particlePose firstPose = (particlePose)poses[0];
if (((int)firstPose.x == (int)prev_pose.x) &&
((int)firstPose.y == (int)prev_pose.y) &&
(Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f))
path.path.RemoveAt(path.path.Count - 1);
}
for (int i = 0; i < poses.Count; i++)
{
particlePose pose = (particlePose)poses[i];
if (pose.x < min_x) min_x = pose.x;
if (pose.y < min_y) min_y = pose.y;
if (pose.x > max_x) max_x = pose.x;
if (pose.y > max_y) max_y = pose.y;
path.Add(pose);
}
}
// update the path velocities
velocities = path.getVelocities(0, 0, time_per_index_sec);
}
