/// <summary>
/// creates some new poses
/// </summary>
private void createNewPoses(float x, float y, float pan)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x position in millimetres</param>
/// <param name="y">y position in millimetres</param>
/// <param name="pan">pan angle in radians</param>
/// <param name="path">the path which this pose belongs to</param>
public particlePose(float x, float y,
float pan,
particlePath path)
{
this.x = x;
this.y = y;
this.pan = pan;
this.path = path;
observed_grid_cells = new List<particleGridCell>();
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x position in millimetres</param>
/// <param name="y">y position in millimetres</param>
/// <param name="pan">pan angle in radians</param>
/// <param name="path">the path which this pose belongs to</param>
public particlePose(float x, float y,
float pan,
particlePath path)
{
this.x = x;
this.y = y;
this.pan = pan;
this.path = path;
observed_grid_cells = new List <particleGridCell>();
}
/// <summary>
/// increment or decrement the total number of child branches supported by this path
/// </summary>
/// <param name="path"></param>
/// <param name="increment">increment of 1 or -1</param>
private void incrementPathChildren(particlePath path,
int increment)
{
path.total_children += increment;
particlePath p = path;
while (p.branch_pose != null)
{
p = p.branch_pose.path;
p.total_children += increment;
}
}
/// <summary>
/// create poses distributed over an area, suitable for
/// monte carlo localisation
/// </summary>
/// <param name="tx">top left of the area in millimetres</param>
/// <param name="ty">top left of the area in millimetres</param>
/// <param name="bx">bottom right of the area in millimetres</param>
/// <param name="by">bottom right of the area in millimetres</param>
private void createNewPosesMonteCarlo(float tx, float ty, float bx, float by)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
float x = tx + rnd.Next((int)(bx - tx));
float y = ty + rnd.Next((int)(by - ty));
float pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// add a new path to the list
/// </summary>
/// <param name="path"></param>
private void createNewPose(particlePath path)
{
Poses.Add(path);
ActivePoses.Add(path);
// increment the path ID
path_ID++;
if (path_ID > UInt32.MaxValue - 3)
{
path_ID = 0; // rollover
}
}
/// <summary>
/// increment or decrement the total number of child branches supported by this path
/// </summary>
/// <param name="path"></param>
/// <param name="increment">increment of 1 or -1</param>
private void incrementPathChildren(particlePath path,
int increment)
{
path.total_children += increment;
particlePath p = path;
while (p.branch_pose != null)
{
p = p.branch_pose.path;
p.total_children += increment;
}
}
/// <summary>
/// show the tree of possible paths
/// </summary>
/// <param name="img"></param>
/// <param name="width"></param>
/// <param name="height"></param>
/// <param name="r"></param>
/// <param name="g"></param>
/// <param name="b"></param>
/// <param name="line_thickness"></param>
public void ShowTree(Byte[] img, int width, int height,
int r, int g, int b, int line_thickness)
{
for (int i = 0; i < ActivePoses.Count; i++)
{
bool clearBackground = false;
if (i == 0)
{
clearBackground = true;
}
particlePath path = ActivePoses[i];
path.Show(img, width, height, r, g, b, line_thickness,
min_tree_x, min_tree_y, max_tree_x, max_tree_y,
clearBackground, root_time_step);
}
}
/// <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>
/// update all current poses with the current observation
/// </summary>
/// <param name="stereo_rays">list of evidence rays to be inserted into the grid</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
public void AddObservation(List <evidenceRay>[] stereo_rays, bool localiseOnly)
{
for (int p = 0; p < Poses.Count; p++)
{
particlePath path = Poses[p];
float logodds_localisation_score =
path.current_pose.AddObservation(stereo_rays,
rob, LocalGrid,
localiseOnly);
if (logodds_localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
updatePoseScore(path, logodds_localisation_score);
}
}
// adding an observation is sufficient to update the
// pose localisation scores. This is, after all, SLAM.
PosesEvaluated = true;
}
/// <summary>
/// constructor
/// </summary>
/// <param name="parent">parent path from which this originates</param>
/// <param name="path_ID">unique ID for this path</param>
/// <param name="grid_dimension_cells">dimension of the grid</param>
public particlePath(particlePath parent,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
parent.current_pose.no_of_children++;
current_pose = parent.current_pose;
branch_pose = parent.current_pose;
incrementPathChildren(parent, 1);
this.max_length = parent.max_length;
path = new List <particlePose>();
total_score = parent.total_score;
total_poses = parent.total_poses;
Enabled = true;
// map cache for this path
//map_cache = new ArrayList[grid_dimension_cells][][];
}
/// <summary>
/// constructor
/// </summary>
/// <param name="parent">parent path from which this originates</param>
/// <param name="path_ID">unique ID for this path</param>
/// <param name="grid_dimension_cells">dimension of the grid</param>
public particlePath(particlePath parent,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
parent.current_pose.no_of_children++;
current_pose = parent.current_pose;
branch_pose = parent.current_pose;
incrementPathChildren(parent, 1);
this.max_length = parent.max_length;
path = new List<particlePose>();
total_score = parent.total_score;
total_poses = parent.total_poses;
Enabled = true;
// map cache for this path
//map_cache = new ArrayList[grid_dimension_cells][][];
}
/// <summary>
/// removes low probability poses
/// Note that a maturity threshold is used, so that poses which may initially
/// be unfairly judged as improbable have time to prove their worth
/// </summary>
private void Prune()
{
float max_score = float.MinValue;
best_path = null;
// sort poses by score
for (int i = 0; i < Poses.Count-1; i++)
{
particlePath p1 = Poses[i];
// keep track of the bounding region within which the path tree occurs
if (p1.current_pose.x < min_tree_x) min_tree_x = p1.current_pose.x;
if (p1.current_pose.x > max_tree_x) max_tree_x = p1.current_pose.x;
if (p1.current_pose.y < min_tree_y) min_tree_y = p1.current_pose.y;
if (p1.current_pose.y > max_tree_y) max_tree_y = p1.current_pose.y;
max_score = p1.total_score;
particlePath winner = null;
int winner_index = 0;
for (int j = i + 1; j < Poses.Count; j++)
{
particlePath p2 = Poses[i];
float sc = p2.total_score;
if ((sc > max_score) ||
((max_score == 0) && (sc != 0)))
{
max_score = sc;
winner = p2;
winner_index = j;
}
}
if (winner != null)
{
Poses[i] = winner;
Poses[winner_index] = p1;
}
}
// the best path is at the top
best_path = Poses[0];
// It's culling season
int cull_index = (100 - cull_threshold) * Poses.Count / 100;
if (cull_index > Poses.Count - 2) cull_index = Poses.Count - 2;
for (int i = Poses.Count - 1; i > cull_index; i--)
{
particlePath path = Poses[i];
if (path.path.Count >= pose_maturation)
{
// remove mapping hypotheses for this path
path.Remove(LocalGrid);
// now remove the path itself
Poses.RemoveAt(i);
}
}
// garbage collect any dead paths (R.I.P.)
List<particlePath> possible_roots = new List<particlePath>(); // stores paths where all branches have coinverged to a single possibility
for (int i = ActivePoses.Count - 1; i >= 0; i--)
{
particlePath path = ActivePoses[i];
if ((!path.Enabled) ||
((path.total_children == 0) && (path.branch_pose == null) && (path.current_pose.time_step < time_step - pose_maturation - 5)))
{
ActivePoses.RemoveAt(i);
}
else
{
// record any fully collapsed paths
if (!path.Collapsed)
if (path.branch_pose == null)
possible_roots.Add(path);
else
{
if (path.branch_pose.path.Collapsed)
possible_roots.Add(path);
}
}
}
if (possible_roots.Count == 1)
{
// collapse tha psth
particlePath path = possible_roots[0];
if (path.branch_pose != null)
{
particlePath previous_path = path.branch_pose.path;
previous_path.Distill(LocalGrid);
path.branch_pose.parent = null;
path.branch_pose = null;
}
path.Collapsed = true;
// take note of the time step. This is for use with the display functions only
root_time_step = path.current_pose.time_step;
}
if (best_path != null)
{
// update the current robot position with the best available pose
if (current_robot_pose == null) current_robot_pose =
new pos3D(best_path.current_pose.x,
best_path.current_pose.y,
0);
current_robot_pose.pan = best_path.current_pose.pan;
current_robot_path_score = best_path.total_score;
// generate new poses from the ones which have survived culling
int new_poses_required = survey_trial_poses; // -Poses.Count;
int max = Poses.Count;
int n = 0, added = 0;
while ((max > 0) &&
(n < new_poses_required*4) &&
(added < new_poses_required))
{
// identify a potential parent at random,
// from one of the surviving paths
int random_parent_index = rnd.Next(max-1);
particlePath parent_path = Poses[random_parent_index];
// only mature parents can have children
if (parent_path.path.Count >= pose_maturation)
{
// generate a child branching from the parent path
particlePath child_path = new particlePath(parent_path, path_ID, rob.LocalGridDimension);
createNewPose(child_path);
added++;
}
n++;
}
// like salmon, after parents spawn they die
if (added > 0)
for (int i = max - 1; i >= 0; i--)
Poses.RemoveAt(i);
// if the robot has rotated through a large angle since
// the previous time step then reset the scan matching estimate
//if (Math.Abs(best_path.current_pose.pan - rob.pan) > rob.ScanMatchingMaxPanAngleChange * Math.PI / 180)
rob.ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED;
}
PosesEvaluated = false;
}
/// <summary>
/// updates the given path with the score obtained from the current pose
/// Note that scores are always expressed as log odds matching probability
/// from grid localisation
/// </summary>
/// <param name="path">path whose score is to be updated</param>
/// <param name="new_score">the new score to be added to the path</param>
public void updatePoseScore(particlePath path, float new_score)
{
path.total_score += new_score;
path.local_score += new_score;
}
/// <summary>
/// create poses distributed over an area, suitable for
/// monte carlo localisation
/// </summary>
/// <param name="tx">top left of the area in millimetres</param>
/// <param name="ty">top left of the area in millimetres</param>
/// <param name="bx">bottom right of the area in millimetres</param>
/// <param name="by">bottom right of the area in millimetres</param>
private void createNewPosesMonteCarlo(float tx, float ty, float bx, float by)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
float x = tx + rnd.Next((int)(bx - tx));
float y = ty + rnd.Next((int)(by - ty));
float pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <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>
/// creates some new poses
/// </summary>
private void createNewPoses(float x, float y, float pan)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// add a new path to the list
/// </summary>
/// <param name="path"></param>
private void createNewPose(particlePath path)
{
Poses.Add(path);
ActivePoses.Add(path);
// increment the path ID
path_ID++;
if (path_ID > UInt32.MaxValue - 3) path_ID = 0; // rollover
}
/// <summary>
/// removes low probability poses
/// Note that a maturity threshold is used, so that poses which may initially
/// be unfairly judged as improbable have time to prove their worth
/// </summary>
private void Prune()
{
float max_score = float.MinValue;
best_path = null;
// sort poses by score
for (int i = 0; i < Poses.Count - 1; i++)
{
particlePath p1 = Poses[i];
// keep track of the bounding region within which the path tree occurs
if (p1.current_pose.x < min_tree_x)
{
min_tree_x = p1.current_pose.x;
}
if (p1.current_pose.x > max_tree_x)
{
max_tree_x = p1.current_pose.x;
}
if (p1.current_pose.y < min_tree_y)
{
min_tree_y = p1.current_pose.y;
}
if (p1.current_pose.y > max_tree_y)
{
max_tree_y = p1.current_pose.y;
}
max_score = p1.total_score;
particlePath winner = null;
int winner_index = 0;
for (int j = i + 1; j < Poses.Count; j++)
{
particlePath p2 = Poses[i];
float sc = p2.total_score;
if ((sc > max_score) ||
((max_score == 0) && (sc != 0)))
{
max_score = sc;
winner = p2;
winner_index = j;
}
}
if (winner != null)
{
Poses[i] = winner;
Poses[winner_index] = p1;
}
}
// the best path is at the top
best_path = Poses[0];
// It's culling season
int cull_index = (100 - cull_threshold) * Poses.Count / 100;
if (cull_index > Poses.Count - 2)
{
cull_index = Poses.Count - 2;
}
for (int i = Poses.Count - 1; i > cull_index; i--)
{
particlePath path = Poses[i];
if (path.path.Count >= pose_maturation)
{
// remove mapping hypotheses for this path
path.Remove(LocalGrid);
// now remove the path itself
Poses.RemoveAt(i);
}
}
// garbage collect any dead paths (R.I.P.)
List <particlePath> possible_roots = new List <particlePath>(); // stores paths where all branches have coinverged to a single possibility
for (int i = ActivePoses.Count - 1; i >= 0; i--)
{
particlePath path = ActivePoses[i];
if ((!path.Enabled) ||
((path.total_children == 0) && (path.branch_pose == null) && (path.current_pose.time_step < time_step - pose_maturation - 5)))
{
ActivePoses.RemoveAt(i);
}
else
{
// record any fully collapsed paths
if (!path.Collapsed)
{
if (path.branch_pose == null)
{
possible_roots.Add(path);
}
else
{
if (path.branch_pose.path.Collapsed)
{
possible_roots.Add(path);
}
}
}
}
}
if (possible_roots.Count == 1)
{
// collapse tha psth
particlePath path = possible_roots[0];
if (path.branch_pose != null)
{
particlePath previous_path = path.branch_pose.path;
previous_path.Distill(LocalGrid);
path.branch_pose.parent = null;
path.branch_pose = null;
}
path.Collapsed = true;
// take note of the time step. This is for use with the display functions only
root_time_step = path.current_pose.time_step;
}
if (best_path != null)
{
// update the current robot position with the best available pose
if (current_robot_pose == null)
{
current_robot_pose =
new pos3D(best_path.current_pose.x,
best_path.current_pose.y,
0);
}
current_robot_pose.pan = best_path.current_pose.pan;
current_robot_path_score = best_path.total_score;
// generate new poses from the ones which have survived culling
int new_poses_required = survey_trial_poses; // -Poses.Count;
int max = Poses.Count;
int n = 0, added = 0;
while ((max > 0) &&
(n < new_poses_required * 4) &&
(added < new_poses_required))
{
// identify a potential parent at random,
// from one of the surviving paths
int random_parent_index = rnd.Next(max - 1);
particlePath parent_path = Poses[random_parent_index];
// only mature parents can have children
if (parent_path.path.Count >= pose_maturation)
{
// generate a child branching from the parent path
particlePath child_path = new particlePath(parent_path, path_ID, rob.LocalGridDimension);
createNewPose(child_path);
added++;
}
n++;
}
// like salmon, after parents spawn they die
if (added > 0)
{
for (int i = max - 1; i >= 0; i--)
{
Poses.RemoveAt(i);
}
}
// if the robot has rotated through a large angle since
// the previous time step then reset the scan matching estimate
//if (Math.Abs(best_path.current_pose.pan - rob.pan) > rob.ScanMatchingMaxPanAngleChange * Math.PI / 180)
rob.ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED;
}
PosesEvaluated = false;
}
/// <summary>
/// updates the given path with the score obtained from the current pose
/// Note that scores are always expressed as log odds matching probability
/// from grid localisation
/// </summary>
/// <param name="path">path whose score is to be updated</param>
/// <param name="new_score">the new score to be added to the path</param>
public void updatePoseScore(particlePath path, float new_score)
{
path.total_score += new_score;
path.local_score += new_score;
}
/// <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>
/// 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>
/// 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;
}
}
}
}
