/// <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(); }