/// <summary>
/// Adds a path to paths list.
/// </summary>
/// <param name="path">Path.</param>
public static void AddPath(BotPath path)
{
if (!paths.Contains(path))
{
paths.Add(path);
}
CamController.AddAreaOfInterest(path);
}
/// <summary>
/// Reset the robot, disable sensors
/// </summary>
public void Reset()
{
_path = new BotPath();
_positions = new Vector3[30];
foreach (Sensor s in _sensors)
{
s.Disable();
}
}
private int ValidateOutstandingPath(BotPath botPath, int simulationStep)
{
var invalidCount = 0;
var distanceToTravel = botPath.Bot.Speed;
if (botPath.HasCollided)
{
distanceToTravel -= simulationStep;
if (distanceToTravel < 0)
{
distanceToTravel = 0;
}
var endpoint = vectorCalculatorService.GetPointFrom(botPath.Bot.Position, distanceToTravel, botPath.Bot.CurrentHeading);
botPath.CollisionDetectionPoints = vectorCalculatorService.CollectCollisionDetectionPointsAlongPath(
botPath.Bot.Position,
endpoint,
botPath.Bot.CurrentHeading);
return(invalidCount);
}
var finalPointIsValid = false;
var distanceFromStart = vectorCalculatorService.GetDistanceBetween(botPath.MovementStartPoint, botPath.Bot.Position);
while (!finalPointIsValid)
{
if (!botPath.CollisionDetectionPoints.Any())
{
finalPointIsValid = true;
break;
}
var distanceToEndpoint = vectorCalculatorService.GetDistanceBetween(
botPath.Bot.Position,
botPath.CollisionDetectionPoints.Last());
if (distanceFromStart + distanceToEndpoint > distanceToTravel)
{
invalidCount++;
botPath.CollisionDetectionPoints.RemoveAt(botPath.CollisionDetectionPoints.Count - 1);
}
else
{
finalPointIsValid = true;
}
}
return(invalidCount);
}
// private methods
// ~-~-~-~-~-~-~-~-
// called once at the start
private void Awake()
{
_sensors = GetComponentsInChildren <Sensor>();
StartCoroutine(StuckDetector());
cameraMount = transform.Find("CameraMount");
if (!cameraMount)
{
cameraMount = transform;
Debug.LogWarning("CameraMount object not found on Robot.");
}
// calculate size bounding box for renderers
Bounds b = new Bounds(Vector3.zero, Vector3.zero);
foreach (Renderer r in GetComponentsInChildren <Renderer>())
{
b.Encapsulate(r.bounds);
}
_size = b.size;
// add center of mass offset
rigidbody.centerOfMass += centerOfMassOffset;
// initialise bath plotter
_path = new BotPath();
StartCoroutine(RecordPath());
}
/// <summary>
/// Removes the path.
/// </summary>
/// <param name="path">Path.</param>
public static void RemovePath(BotPath path)
{
paths.Remove(path);
CamController.RemoveAreaOfInterest(path);
}
/// <summary>
/// Main routine for loading a CSV log file.
/// </summary>
/// <param name="csvFilePath">Csv file path.</param>
static IEnumerator LoadCsvRoutine(string csvFilePath)
{
loading = true;
string csvPath = Path.GetDirectoryName(csvFilePath);
string csvFileName = Path.GetFileName(csvFilePath);
// try opening the file with StreamReader
StreamReader sr;
try {
sr = new StreamReader(csvFilePath);
}
catch (Exception e) {
// log exception
Debug.LogException(e);
UI_Toolbar.I.additionalWindows.Add(
new UI_Prompt(
PromptResponse,
UI_Prompt.Type.Close,
"File Load Exception!",
"See log for details"
)
);
// stop loading
loading = false;
yield break;
}
// inspect CSV header
string line;
string header = "";
string xmlFileName = null;
while ((line = sr.ReadLine()) != null)
{
// stop inspecting when comments are no longer found
if (!line.StartsWith(Strings.csvComment))
{
break;
}
header += line;
// find XML filename stored in CSV
if (line.Contains(Strings.csvXmlCommentTag))
{
xmlFileName = line.Substring(
line.IndexOf(Strings.csvXmlCommentTag) +
Strings.csvXmlCommentTag.Length);
Debug.Log("XML filename from CSV is: " + xmlFileName);
}
}
// temporary settings object for deserialization
Simulation.Settings settings;
// if xml filename was not found in csv...
if (xmlFileName == null)
{
settings = new Simulation.Settings();
// prompt user whether to select environment
_waitingForResponse = true;
UI_Toolbar.I.additionalWindows.Add(
new UI_Prompt(
PromptResponse,
UI_Prompt.Type.YesNo,
"XML filename not found in CSV header!",
header + "\n Select environment to load?"
)
);
while (_waitingForResponse)
{
yield return(new WaitForSeconds(0.1f));
}
if (_response)
{
/// not yet implemented
// browse environments and load selection
Debug.Log("Not yet implemented: browse and load environment");
}
}
else
{
// try loading environment
settings = ObjectSerializer.DeSerializeObject <Simulation.Settings>(csvPath + "\\" + xmlFileName);
// if environment is different to the currently loaded environment
// prompt user for action (discard other paths, or load new env and paths?)
// (not yet implemented)
if (environment)
{
if (environment.name != settings.environmentName)
{
_waitingForResponse = true;
UI_Toolbar.I.additionalWindows.Add(
new UI_Prompt(
PromptResponse,
UI_Prompt.Type.OkCancel,
"Load new environment and paths?",
"CSV log is for a different environment. Load new environment and paths instead?"
)
);
while (_waitingForResponse)
{
yield return(new WaitForSeconds(0.1f));
}
if (_response)
{
// load environment and clear paths if YES
paths.Clear();
CamController.ClearAreaList();
LoadEnvironment(settings.environmentName);
}
else
{
// stop loading if NO
loading = false;
yield break;
}
}
}
else
{
LoadEnvironment(settings.environmentName);
}
}
// load paths from CSV and display them
// go to line with SimulationTime as first string
while ((line = sr.ReadLine()) != null)
{
if (line.StartsWith(Log.Parameters.SimulationTime.ToString()))
{
break;
}
}
// extract headings and store column indexes for path types
int robotPositionIndex;
string[] row = line.Split(Strings.csvDelimiter);
for (robotPositionIndex = 0; robotPositionIndex < row.Length; robotPositionIndex++)
{
if (row[robotPositionIndex] == Log.Parameters.RobotPosition.ToString())
{
break;
}
}
BotPath botpos = new BotPath();
botpos.csvName = csvFileName;
// Build BotPath objects for each path type columns found
while ((line = sr.ReadLine()) != null)
{
row = line.Split(Strings.csvDelimiter);
if (robotPositionIndex > row.Length)
{
Debug.LogWarning("LogLoader: row length too short?");
continue;
}
// try deserializing this vector3 data
// (catch parsing errors not yet implemented)
Vector3 pos = ParseVector3(row[robotPositionIndex]);
// remove " chars from "12.3", for example
// (catch parsing errors not yet implemented)
float time = float.Parse(row[0].Substring(1, row[0].Length - 2));
botpos.AddNode(pos, time);
}
AddPath(botpos);
UpdatePathColors();
loading = false;
}
