/// <summary>
/// add a path segment
/// </summary>
/// <param name="x">x coordinate in millimetres</param>
/// <param name="y">y coordinate in millimetres</param>
/// <param name="pan">heading in radians</param>
public void Add(float x, float y, float pan,
int no_of_steps, float dist_per_step_mm,
float pan_per_step)
{
simulationPathSegment segment = new simulationPathSegment(x, y, pan, no_of_steps, dist_per_step_mm, pan_per_step);
pathSegments.Add(segment);
updatePath();
}
public XmlElement getXml(XmlDocument doc, XmlElement parent)
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
XmlElement nodeSimulation = doc.CreateElement("Simulation");
parent.AppendChild(nodeSimulation);
xml.AddComment(doc, nodeSimulation, "Name or title of the simulation");
xml.AddTextElement(doc, nodeSimulation, "SimulationName", Name);
xml.AddComment(doc, nodeSimulation, "The path and filename for the xml file which contains the robot design definition");
xml.AddTextElement(doc, nodeSimulation, "RobotDesignFile", RobotDesignFile);
xml.AddComment(doc, nodeSimulation, "Path where the stereo images can be found");
xml.AddTextElement(doc, nodeSimulation, "ImagesPath", ImagesPath);
xml.AddComment(doc, nodeSimulation, "The time which elapses for each step along the path in seconds");
xml.AddTextElement(doc, nodeSimulation, "SimulationTimeStepSeconds", Convert.ToString(time_per_index_sec));
if (pathSegments != null)
{
XmlElement nodePath = doc.CreateElement("RobotPath");
nodeSimulation.AppendChild(nodePath);
for (int i = 0; i < pathSegments.Count; i++)
{
simulationPathSegment segment = (simulationPathSegment)pathSegments[i];
XmlElement nodePathSegment = doc.CreateElement("PathSegment");
nodePath.AppendChild(nodePathSegment);
xml.AddComment(doc, nodePathSegment, "The initial pose of the robot at the beginning of this path segment");
xml.AddComment(doc, nodePathSegment, "X,Y position in millimetres, followed by heading in degrees");
xml.AddTextElement(doc, nodePathSegment, "InitialPose", Convert.ToString(segment.x, format) + "," +
Convert.ToString(segment.y, format) + "," +
Convert.ToString(segment.pan * 180.0f / Math.PI, format));
xml.AddComment(doc, nodePathSegment, "The number of steps which this segment consists of");
xml.AddTextElement(doc, nodePathSegment, "NumberOfSteps", Convert.ToString(segment.no_of_steps));
xml.AddComment(doc, nodePathSegment, "The distance of each step in millimetres");
xml.AddTextElement(doc, nodePathSegment, "StepSizeMillimetres", Convert.ToString(segment.distance_per_step_mm));
xml.AddComment(doc, nodePathSegment, "The change in heading per step in degrees");
xml.AddTextElement(doc, nodePathSegment, "HeadingChangePerStep", Convert.ToString(segment.pan_per_step * 180.0f / Math.PI));
}
}
return(nodeSimulation);
}
/// <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>
/// add a path segment
/// </summary>
/// <param name="x">x coordinate in millimetres</param>
/// <param name="y">y coordinate in millimetres</param>
/// <param name="pan">heading in radians</param>
public void Add(float x, float y, float pan,
int no_of_steps, float dist_per_step_mm,
float pan_per_step)
{
simulationPathSegment segment = new simulationPathSegment(x, y, pan, no_of_steps, dist_per_step_mm, pan_per_step);
pathSegments.Add(segment);
updatePath();
}
