// Update is called once per frame
void FixedUpdate()
{
ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame();
if (frameData != null)
{
drone.transform.position = frameData.position;
drone.transform.rotation = Quaternion.Euler(frameData.rotation);
}
//if (droneFlying) {
// if (!anchorsActivated) {
// anchorIDParent.SetActive(true);
// anchorsActivated = true;
// }
// for (int i = 0; i < anchors.Length; i++)
// {
// if (GeometryUtility.TestPlanesAABB(GeometryUtility.CalculateFrustumPlanes(Camera.main), anchors[i].GetComponent<Collider>().bounds))
// {
// anchorIDPos[i] = Camera.main.WorldToScreenPoint(anchors[i].transform.position);
// anchorIDs[i].anchoredPosition = new Vector2(anchorIDPos[i].x - 125, anchorIDPos[i].y + 25);
// }
// else
// {
// anchorIDs[i].anchoredPosition = new Vector2(-400, -400);
// }
// }
//}
}
void Update()
{
//This if are consulted when the user presses the use last session configuration. It will enter if there is positioning and anchors.
if (PozyxModule.positiningIsValid && skipClicked && CalibrationSettings.anchorConfigData[0] != null)
{
PozyxModule.positiningIsValid = false;
if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Mapping)
{
SceneManager.LoadScene("Mapping");
}
else if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Recording)
{
SceneManager.LoadScene("PlanSelection");
}
return;
}
//If an error was received when trying to use the last session config, it will enter here
else if (skipClicked && state == TagsConfigState.ErrorReceived)
{
skipEverythingButton.GetComponent <Button>().interactable = true;
skipEverythingButton.GetChild(0).GetComponent <Text>().color = new Vector4(1, 1, 1, 1f);
discoverAnchorsButton.GetComponent <Button>().interactable = true;
discoverAnchorsButton.GetChild(0).GetComponent <Text>().color = new Vector4(1, 1, 1, 1f);
lookingForTags.gameObject.SetActive(false);
searchTags.gameObject.SetActive(true);
state = TagsConfigState.Idle;
}
//Meanwhile, it will enter here to rotate the throbber
else if (skipClicked)
{
tagsThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
}
switch (state)
{
case TagsConfigState.Idle:
// Caso para comprobación de estado
break;
//Not used for the moment
case TagsConfigState.SearchTagsReturn:
discoverAnchorsButton.GetComponent <Button>().interactable = true;
lookingForTags.gameObject.SetActive(false);
searchTags.gameObject.SetActive(true);
//We leave it idle while the user configs which tag is where
state = TagsConfigState.Idle;
break;
//This case happens once the user clicked on the discover button
case TagsConfigState.SearchingTags:
tagsThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
if (camOffsetReceived && tagInfoReceived && droneFilterReceived)
{
state = TagsConfigState.TagsFound;
}
break;
//Once the app gets the tag list, it enters here and displays them
case TagsConfigState.TagsFound:
if (!panelInstantiated)
{
panelInstantiated = true;
int width = Screen.width;
int height = Screen.height;
string[] tagsFoundStr = PozyxModule.decAnchors.Split('\n');
tagsFoundCount = tagsFoundStr.Length - 1;
tagsText.text = PozyxModule.textMsg;
discoverAnchorsButton.gameObject.SetActive(false);
configButton.gameObject.SetActive(true);
Vector2 inputSize = new Vector2(panelSize.x * 0.3f, panelSize.y / 12);
Vector2 buttonSize = new Vector2(panelSize.x * 0.6f, panelSize.y / 10);
//All of this resizes and positions the tag selection and configurations panel
widthInput.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
heightInput.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputX.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputY.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputZ.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
updatePeriodInput.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
movementFreedomInput.GetComponent <InputField>().placeholder.GetComponent <Text>().fontSize = (int)(width * 0.018f);
widthInput.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
heightInput.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputX.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputY.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
camDistInputZ.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
updatePeriodInput.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
movementFreedomInput.GetComponent <InputField>().textComponent.GetComponent <Text>().fontSize = (int)(width * 0.018f);
widthInput.GetComponent <InputField>().onValueChanged.AddListener(WidthInputText);
heightInput.GetComponent <InputField>().onValueChanged.AddListener(HeightInputText);
camDistInputX.GetComponent <InputField>().onValueChanged.AddListener(CamDistInputXText);
camDistInputY.GetComponent <InputField>().onValueChanged.AddListener(CamDistInputYText);
camDistInputZ.GetComponent <InputField>().onValueChanged.AddListener(CamDistInputZText);
updatePeriodInput.GetComponent <InputField>().onValueChanged.AddListener(UpdatePeriodText);
movementFreedomInput.GetComponent <InputField>().onValueChanged.AddListener(MovementFreedomText);
widthLabel.GetComponent <Text>().fontSize = (int)(width * 0.0225f);
heightLabel.GetComponent <Text>().fontSize = (int)(width * 0.0225f);
camDistLabelX.GetComponent <Text>().fontSize = (int)(width * 0.0225f);
camDistLabelY.GetComponent <Text>().fontSize = (int)(width * 0.0225f);
camDistLabelZ.GetComponent <Text>().fontSize = (int)(width * 0.0225f);
updatePeriodLabel.GetComponent <Text>().fontSize = (int)(width * 0.02f);
movementFreedomLabel.GetComponent <Text>().fontSize = (int)(width * 0.02f);
tagsFoundLabel.anchoredPosition = new Vector2(inputSize.x * 0.1f, inputSize.y * 1.5f);
tagsFoundLabel.sizeDelta = inputSize;
tagsFoundLabel.GetComponent <Text>().fontSize = (int)(width * 0.03f);
widthInput.sizeDelta = inputSize;
heightInput.sizeDelta = inputSize;
camDistInputX.sizeDelta = inputSize;
camDistInputY.sizeDelta = inputSize;
camDistInputZ.sizeDelta = inputSize;
updatePeriodInput.sizeDelta = inputSize;
movementFreedomInput.sizeDelta = inputSize;
widthLabel.sizeDelta = inputSize;
heightLabel.sizeDelta = inputSize;
camDistLabelX.sizeDelta = inputSize;
camDistLabelY.sizeDelta = inputSize;
camDistLabelZ.sizeDelta = inputSize;
updatePeriodLabel.sizeDelta = inputSize;
movementFreedomLabel.sizeDelta = inputSize;
tagsFoundCount = tagsFoundCount > 10 ? 10 : tagsFoundCount;
widthInput.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 0 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
heightInput.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 1 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistInputX.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 2 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistInputY.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 3 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistInputZ.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 4 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
updatePeriodInput.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 5 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
movementFreedomInput.anchoredPosition = new Vector2(panelSize.x * 0.6f, (-panelSize.y / 7) * 6 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
widthLabel.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 0 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
heightLabel.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 1 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistLabelX.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 2 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistLabelY.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 3 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
camDistLabelZ.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 4 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
updatePeriodLabel.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 5 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
movementFreedomLabel.anchoredPosition = new Vector2(panelSize.x * 0.1f, (-panelSize.y / 7) * 6 - ((panelSize.y - (inputSize.y * 7)) / (7 * 2)));
//This creates a button for each tag found
for (int i = 0; i < tagsFoundCount; i++)
{
GameObject inputGO = Instantiate(idGroupPrefab, inputParent.transform);
inputGO.name = tagsFoundStr[i];
//inputGO.name = i.ToString();
inputGO.transform.GetChild(0).GetComponent <RectTransform>().sizeDelta = buttonSize;
inputGO.transform.GetChild(0).GetComponent <RectTransform>().anchoredPosition = new Vector2(panelSize.x * 0.2f, (-panelSize.y / tagsFoundCount) * (i) - ((panelSize.y - (inputSize.y * tagsFoundCount)) / (tagsFoundCount * 2))); // - (inputGO.transform.GetChild(0).GetComponent<RectTransform>().sizeDelta.y * 1.1f)
inputGO.transform.GetChild(0).GetChild(0).GetComponent <Text>().fontSize = (int)(width * 0.025f);
//int hex = -1;
//if (!int.TryParse(inputGO.name, System.Globalization.NumberStyles.HexNumber, System.Globalization.CultureInfo.InvariantCulture, out hex))
//{
// UnityEngine.Debug.LogWarning("Unable to parse anchor Value!");
//}
//inputGO.transform.GetChild(1).GetChild(0).GetComponent<Text>().text = hex.ToString("X");
inputGO.transform.GetChild(0).GetChild(0).GetComponent <Text>().text = "0x" + int.Parse(inputGO.name).ToString("x");
//And assigns a function for each tag button clicked
inputGO.transform.GetChild(0).GetComponent <Button>().onClick.AddListener(() => SelectTagId(inputGO.name, inputGO.transform.parent));
}
//Gets the current offset of the camera
Vector3 camInfo = DroneModule.GetCamOffset();
camDistValueX = camInfo.x;
camDistValueY = camInfo.y;
camDistValueZ = camInfo.z;
camDistInputX.GetComponent <InputField>().text = camDistValueX.ToString();
camDistInputY.GetComponent <InputField>().text = camDistValueY.ToString();
camDistInputZ.GetComponent <InputField>().text = camDistValueZ.ToString();
camDistTextMesh.text = "(" + camDistValueX + ", " + camDistValueY + ", " + camDistValueZ + ")";
//Gets the tag configuration (which tag belongs to which part of the drone, the widht and height ...
ServerMessages.IPSDroneTag tagData = PozyxModule.GetTagFrame();
if (tagData != null)
{
widthInput.GetComponent <InputField>().text = tagData.width.ToString();
widthValue = tagData.width;
heightInput.GetComponent <InputField>().text = tagData.height.ToString();
heightValue = tagData.height;
updatePeriodInput.GetComponent <InputField>().text = PozyxModule.updatePeriod.ToString();
movementFreedomInput.GetComponent <InputField>().text = PozyxModule.movementFreedom.ToString();
//Here it is assigned to which drawing of the tag do they belong
for (int i = 0; i < tagsFoundCount; i++)
{
if (inputParent.transform.GetChild(i).name == tagData.idSW.ToString())
{
swTextMesh.text = "0x" + tagData.idSW.ToString("x");
inputParent.transform.GetChild(i).GetChild(0).GetComponent <Image>().color = new Vector4(0, 1, 0, 1);
swValue = tagData.idSW;
}
if (inputParent.transform.GetChild(i).name == tagData.idNW.ToString())
{
nwTextMesh.text = "0x" + tagData.idNW.ToString("x");
inputParent.transform.GetChild(i).GetChild(0).GetComponent <Image>().color = new Vector4(0, 1, 0, 1);
nwValue = tagData.idNW;
}
if (inputParent.transform.GetChild(i).name == tagData.idNE.ToString())
{
neTextMesh.text = "0x" + tagData.idNE.ToString("x");
inputParent.transform.GetChild(i).GetChild(0).GetComponent <Image>().color = new Vector4(0, 1, 0, 1);
neValue = tagData.idNE;
}
if (inputParent.transform.GetChild(i).name == tagData.idSE.ToString())
{
seTextMesh.text = "0x" + tagData.idSE.ToString("x");
inputParent.transform.GetChild(i).GetChild(0).GetComponent <Image>().color = new Vector4(0, 1, 0, 1);
seValue = tagData.idSE;
}
}
lookingForTags.gameObject.SetActive(false);
}
else
{
return;
}
state = TagsConfigState.ConfiguringTags;
}
break;
case TagsConfigState.ConfiguringTags:
// Caso para comprobación de estado
break;
case TagsConfigState.ConfigurationSent:
//After the user presses the confirm configuration, it enters here
sendingDataThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
if (camOffsetConfirmed && tagInfoConfirmed)
{
state = TagsConfigState.ConfigurationAccepted;
}
break;
case TagsConfigState.ConfigurationAccepted:
// We go here and start anchors calibration once the app sends all configurations
state = TagsConfigState.Idle;
camOffsetReceived = false;
tagInfoReceived = false;
droneFilterReceived = false;
camOffsetConfirmed = false;
tagInfoConfirmed = false;
SceneManager.LoadScene("Calibration");
break;
case TagsConfigState.ErrorReceived:
UnityEngine.Debug.LogWarning(PozyxModule.errorMsg);
if (tagsFound.gameObject != null)
{
tagsText.text = PozyxModule.errorMsg;
state = TagsConfigState.SearchingTags;
}
// ## TODO: Mostrar error recibido en escena?
break;
}
}
// Update is called once per frame
void FixedUpdate()
{
//We get the drone position to reflect it on the screen
ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame();
if (frameData != null)
{
drone.transform.position = frameData.position;
drone.transform.rotation = Quaternion.Euler(frameData.rotation);
//Camera.main.transform.position = new Vector3(drone.transform.position.x, drone.transform.position.y + 10, drone.transform.position.z) ;
}
//We get the anchors to paint them
ServerMessages.IPSFrameAnchorData anchorData = PozyxModule.DequeueIPSAnchorFrame();
if (anchorData != null)
{
anchorParent.transform.Find("Anchor" + anchorID).position = anchorData.position;
anchorID++;
if (anchorID == 9)
{
UnityEngine.Debug.Log("All anchors positioned");
}
}
//Here we should do a takeoff
switch (states)
{
case FlyingStates.IDLE:
break;
case FlyingStates.STARTFLYING:
time += Time.deltaTime;
if (time >= 2.0f)
{
clientUnity.client.SendCommand((byte)Modules.PLAN_EXECUTOR_MODULE, (byte)PlanExecutorCommandType.PLAN_EXEC_TAKEOFF);
states = FlyingStates.FLYING;
}
break;
case FlyingStates.FLYING:
break;
default:
break;
}
//If the drone reached its destination, we activate the button that returns to general
if (flightEnded)
{
flightEnded = false;
droneHasFinishedPanel.gameObject.SetActive(true);
warningRecStartPanel.gameObject.SetActive(false);
recordingPanel.gameObject.SetActive(false);
}
/*
* if (indexWaypoints >= waypoints.Count || TotalindexMiddlePoints >= path.middlePointsTop.Count)
* {
* active = false;
*
* }
* if (active && time < 1)
* {
* drone.transform.position = Vector3.Lerp(actualWaypoint, nextWaypoint, time);
* //camToTravel.transform.position = nextWaypoint * Time.deltaTime * 0.001f;
* if (waypoints[indexWaypoints].GetComponent<PathPoint>().BlockDirection)
* {
* drone.transform.rotation = Quaternion.Euler(waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.z, waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.y - 90, waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.x);
* //blockedDIrection = true;
* }
* else if (!waypoints[indexWaypoints].GetComponent<PathPoint>().BlockDirection)
* {
* drone.transform.LookAt(waypoints[indexWaypoints].GetComponent<PathPoint>().Poi);
* }
*
* time += Time.deltaTime;
* }
* if (active && time >= 1)
* {
* actualWaypoint = nextWaypoint;
* UnityEngine.Debug.Log(waypoints[indexWaypoints].GetComponent<PathPoint>().Segments);
* if (waypoints[indexWaypoints].GetComponent<PathPoint>().Segments <= 3)
* {
* if (waypoints.Count - 1 > indexWaypoints)
* {
* indexWaypoints++;
* TotalindexMiddlePoints++;
* nextWaypoint = waypoints[indexWaypoints].transform.position;
* indexMiddlePoints = 0;
* }
* //blockedDIrection = false;
*
* }
* else
* {
* if (waypoints[indexWaypoints].GetComponent<PathPoint>().Segments == indexMiddlePoints + 1)
* {
* indexWaypoints++;
* TotalindexMiddlePoints++;
* if (TotalindexMiddlePoints < path.middlePointsTop.Count)
* nextWaypoint = path.middlePointsTop[TotalindexMiddlePoints];
* indexMiddlePoints = 0;
* //blockedDIrection = false;
*
* }
* else if (path.middlePointsTop.Count > TotalindexMiddlePoints)
* {
* indexMiddlePoints++;
* TotalindexMiddlePoints++;
* if (TotalindexMiddlePoints < path.middlePointsTop.Count)
* nextWaypoint = path.middlePointsTop[TotalindexMiddlePoints];
* }
* }
*
*
* time = 0;
* }
*/
}
/// <summary>
/// Runs the tasks from the given module
/// </summary>
/// <param name="module">The module.</param>
/// <param name="taskNames">The task names.</param>
/// <param name="env">The env.</param>
/// <returns></returns>
/// <exception cref="System.ArgumentNullException">module
/// or
/// taskNames
/// or
/// config</exception>
public IList<DroneTaskResult> Run(DroneModule module, IEnumerable<string> taskNames, DroneEnv env)
{
if (module == null)
throw new ArgumentNullException("module");
if (taskNames == null)
throw new ArgumentNullException("taskNames");
if(env == null)
throw new ArgumentNullException("env");
var names = taskNames.ToList();
var results = new List<DroneTaskResult>();
var sw = new Stopwatch();
sw.Start();
if (names.Count == 0)
{
this.log.Debug("no task names provided, trying to run default task");
var result = this.TryRunDefaultTask(module, env);
if(result.HasValue)
results.Add(result.Value);
}
else
{
this.log.Debug("checking task names exist...");
this.EnsureTaskNamesExists(module, names);
this.log.Debug("all task names found");
var taskFaulted = false;
this.log.Debug("running tasks...");
foreach(var taskName in names)
{
this.log.Debug("finding task '{0}'", taskName);
var task = module.TryGet(taskName);
if(taskFaulted)
{
this.log.Debug("skipping task '{0}' due to previous task failure", taskName);
results.Add(new DroneTaskResult(task, DroneTaskState.NotRan, TimeSpan.Zero, Option.None<Exception>()));
continue;
}
this.log.Debug("task '{0}' found!, running...", taskName);
var result = this.Run(module, task.Value, env, true);
results.Add(result);
if (!result.IsSuccess)
{
this.log.Debug("task '{0}' has failed, skipping all other tasks", taskName);
taskFaulted = true;
}
this.log.Info(string.Empty);
}
this.log.Debug("all tasks completed");
}
if(results.Count == 0)
return results;
var maxNameLen = results.Max(x => x.Task.Get(t => t.Name.Length, 0));
var totalTime = TimeSpan.FromMilliseconds(results.Select(x => x.TimeElapsed.TotalMilliseconds).Sum());
var summaryTitle = string.Format("tasks: ({0})", HumanTime.Format(totalTime));
this.log.Info(summaryTitle);
this.log.Info(string.Join(string.Empty, Enumerable.Repeat("-", summaryTitle.Length)));
foreach(var result in results)
{
var glyph = this.GetTaskStateGlyph(result.State);
var state = this.GetTaskStateDesc(result.State);
var name = result.Task.Get(x => x.Name, "[null]");
var time = string.Format("({0})", this.GetTaskStateFormatedTime(result.State, result.TimeElapsed));
var fmt = string.Format("{{0}} {{1, -{0}}} {{2, -10}} {{3, -10}}", maxNameLen);
this.log.Info(fmt, glyph, name, state, time);
}
sw.Stop();
this.log.Info(string.Empty);
this.log.Info("total time: ({0})", HumanTime.Format(sw.Elapsed));
return results;
}
private Option<DroneTaskResult> TryRunDefaultTask(DroneModule module, DroneEnv env)
{
var task = module.TryGet(DroneModule.DefaultTaskName);
if(task.HasValue)
{
this.log.Debug("default task found, running...");
return Option.From(this.Run(module, task.Value, env, true));
}
else
{
this.log.Warn("no default task found");
return Option.None<DroneTaskResult>();
}
}
private DroneTaskResult Run(DroneModule module, DroneTask task, DroneEnv env, bool logErrors)
{
var taskLog = DroneLogManager.GetTaskLog(task.Name);
var taskContext = new DroneTaskContext(module, task, env, taskLog, (t, e) =>
{
var childTaskResult = this.Run(module, t, e, false);
if(!childTaskResult.IsSuccess)
{
this.log.Debug("child task '{0}' has failed", t.Name);
throw childTaskResult.Exception.Value;
}
});
this.log.Info("running '{0}'", task.Name);
var handler = this.factory.TryGetHandler(task);
var result = FuncStopwatch.Run(() =>
{
if(handler.HasValue)
{
handler.Value.Handle(taskContext);
}
else
{
throw DroneTaskHandlerNotFoundException.Get(task);
}
});
if (result.IsSuccess)
{
this.log.Info("task '{0}' completed ({1})", task.Name, HumanTime.Format(result.TimeElapsed));
return new DroneTaskResult(Option.From(task), DroneTaskState.Completed, result.TimeElapsed, Option.None<Exception>());
}
else
{
var state = DroneTaskState.Faulted;
var stateName = "failed";
this.log.Info("task '{0}' {1} ({2})", task.Name, stateName, HumanTime.Format(result.TimeElapsed));
if(logErrors)
this.log.ExceptionAndData(result.Exception);
return new DroneTaskResult(Option.From(task), state, result.TimeElapsed, Option.From(result.Exception));
}
}
private void EnsureTaskNamesExists(DroneModule module, IList<string> taskNames)
{
var tasksNotFound = taskNames
.Where(x => !module.TryGet(x).HasValue)
.Distinct()
.ToList();
if (tasksNotFound.Count > 0)
throw DroneTasksNotFoundException.Get(tasksNotFound);
}
// Use this for initialization
void Awake()
{
//If clientunity object is already present on the scene, the new one gets destroyed
if (FindObjectsOfType(GetType()).Length > 1)
{
Destroy(gameObject);
return;
}
//The first ClientUnity is common for every scene
DontDestroyOnLoad(gameObject);
//FileInfo fileInfo = new System.IO.FileInfo(Application.dataPath + "/Resources/Update/airt-project-0.1.1-.deb");
//FileStream fs = File.OpenRead(Application.dataPath + "/Resources/Update/airt-project-0.1.1-.deb");
//ulong fileSize = (ulong)fileInfo.Length;
//UnityEngine.Debug.Log("File size: " + fileSize);
//byte[] fileBytes = new byte[4096];
//
//BinaryReader br = new BinaryReader(fs);
//
//
//ulong bytesRead = 0;
//while (bytesRead < fileSize)
//{
// int readCount = br.Read(fileBytes, (int)0, 4096);
// bytesRead += 4096;
// UnityEngine.Debug.Log("READING");
//}
//UnityEngine.Debug.Log("END");
//Drone ip
if (GlobalSettings.Instance.getIP() == "")
{
//GlobalSettings.Instance.setIP("10.42.0.101");
GlobalSettings.Instance.setIP("192.168.8.1");
}
//GlobalSettings.Instance.setIP("192.168.8.205");
//GlobalSettings.Instance.setIP("158.42.154.131");
//Every module
client = new Client();
stdManager = new StandardModule();
atreyuManager = new AtreyuModule();
osManager = new OSModule(FileManager.debFilePath, FileManager.debFileName + ".deb");
fpvManager = new FPVModule();
FCS_Module = new FCSModule();
libraryManager = new LibraryModule();
missionExecutorManager = new MissionExecutorModule();
pozyxManager = new PozyxModule();
pointCloudManager = new MapperModule();
droneManager = new DroneModule();
recCamModule = new RecCamModule();
//mhandlers.Add(new MsgHandler((byte)Modules.POINTCLOUD_MODULE, (byte)PointcloudNotificationType.PCL_SINGLE, onPclData)); // data
cli_thread = new Thread(() => client.run(mhandlers));
cli_thread.Start();
//We start quering atreyu state to see if the app should jump to mapping or recording, as the drone can't do anything other than mapping or recording if it is
//is those states
if (client.isConnected)
{
tryReconnect = false;
client.SendCommand((byte)Modules.ATREYU_MODULE, (byte)AtreyuCommandType.ATREYU_QUERY_SYSTEM_STATE);
}
else
{
tryReconnect = true;
}
//client.sendCommand((byte)Modules.ATREYU_MODULE, (byte)AtreyuCommandType.ATREYU_QUERY_SERVER_VERSION);
}
// Update is called once per frame
void FixedUpdate()
{
switch (state)
{
case CalibrationState.IDLE:
//The spinning wheel(throbber) rotates
if (autocalibratingThrobber.activeInHierarchy)
{
autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
}
break;
case CalibrationState.DISCOVERING:
searchingLabel.transform.GetChild(0).Rotate(0, 0, -200 * Time.deltaTime);
break;
case CalibrationState.RECEIVING_DISCOVER_ANCHORS:
List <int> anchorsIDList = PozyxModule.GetAnchorList();
{
//UnityEngine.Debug.Log("ReadingAnchorList");
dicoveredAnchors.text = "";
for (int i = 0; i < anchorsIDList.Count; i++)
{
//Writing the anchor list received inside the TextBoxes that can be clicked, with the HorizontalGroup name
//UnityEngine.Debug.Log("Reading Anchor List Element: " + i);
//anchorParent.transform.Find("Cube" + anchorID).position = anchorData.position;
//anchor1Pos = cam.WorldToScreenPoint(anchorData.position);
//transform.Find("Cube" + anchorID + "ID").GetComponent<RectTransform>().anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f);
dicoveredAnchors.text += "Anchor: " + i + " ID: " + "0x" + anchorsIDList[i].ToString("x") + "\n";
anchorList.transform.Find("HorizontalGroup" + (i + 1)).GetChild(0).GetChild(0).GetComponent <Text>().text = "0x" + anchorsIDList[i].ToString("x");
anchorList.transform.Find("HorizontalGroup" + (i + 1)).GetChild(0).name = anchorsIDList[i].ToString();
}
//UnityEngine.Debug.Log("FinishedReadingAnchorList");
//Activate the button that lets the user give the correct position to the anchors
configAnchorsButton.GetComponent <Button>().interactable = true;
configAnchorsButton.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1, 1, 1, 1);
//UnityEngine.Debug.Log("All anchors positioned");
searchingLabel.SetActive(false);
state = CalibrationState.RECEIVED_DISCOVER_ANCHORS;
}
break;
case CalibrationState.RECEIVED_DISCOVER_ANCHORS:
//Having done previously that, is this state necessary?
if (PozyxModule.anchorsReceived)
{
PozyxModule.anchorsReceived = false;
state = CalibrationState.DISCOVERED;
}
break;
case CalibrationState.DISCOVERED:
anchorReadCount = 0;
for (int i = 0; i < 8; i++)
{
ServerMessages.IPSFrameAnchorData anchorData = CalibrationSettings.GetAnchorData(i);
//anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).GetChild(0).GetComponent<Text>().text = anchorData.id.ToString();
//anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).name = anchorData.id.ToString();
//Writing the corresponding id of an anchor to a cube
anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(0).GetComponent <TextMesh>().text = "0x" + anchorData.id.ToString("x");
anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(1).localPosition = anchorData.position;
for (int j = 1; j < 9; j++)
{
//UnityEngine.Debug.Log(anchorData.order);
//Writing the received position of the anchors
if (anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).name == anchorData.id.ToString())
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = anchorData.position.x.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = anchorData.position.y.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().text = anchorData.position.z.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Image>().color = new Vector4(0, 255, 0, 150);
anchorReadCount++;
//Writing the received position of the anchor list when autocalibration is pressed
if (autoCalib && (anchorData.order == 1 || anchorData.order == 5))
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "0";
}
else if (autoCalib && (anchorData.order == 3 || anchorData.order == 7))
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "0";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false;
}
else if (autoCalib && (anchorData.order == 2 || anchorData.order == 6))
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false;
}
else if (autoCalib)
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "0";
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "0";
}
}
}
}
//UnityEngine.Debug.Log("Anclas coincidentes: " + anchorReadCount);
//Activates button to confirm configuration
if (anchorReadCount == 8)
{
UnityEngine.Debug.Log("PozyxModuleAnchorsReady");
confirmManual.interactable = true;
confirmManual.transform.GetChild(0).GetComponent <Text>().color = new Vector4(255, 255, 255, 255);
}
state = CalibrationState.IDLE;
//anchorParent.transform.Find("Cube" + anchorID).position = anchorData.position;
//anchor1Pos = cam.WorldToScreenPoint(anchorData.position);
//transform.Find("Cube" + anchorID + "ID").GetComponent<RectTransform>().anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f);
//dicoveredAnchors.text += "Tag: " + anchorID + " ID: " + anchorData.id + "\n";
//anchorList.transform.Find("Anchor" + anchorID).GetChild(0).GetComponent<Text>().text = "Anchor ID: " + anchorData.id;
//anchorList.transform.Find("Anchor" + anchorID).name = anchorData.id.ToString();
//configAnchorsButton.GetComponent<Button>().interactable = true;
//configAnchorsButton.transform.GetChild(0).GetComponent<Text>().color = new Vector4(1, 1, 1, 1);
//UnityEngine.Debug.Log("All anchor data received and ready to be used");
//searchingLabel.SetActive(false);
//ipsCommandSent = false;
break;
case CalibrationState.MANUAL_ACCEPTED:
//Sending the configuration and wait for positioning
clientUnity.client.SendCommand((byte)Modules.POSITIONING_MODULE, (byte)PositioningCommandType.IPS_UPDATE_SETTINGS);
state = CalibrationState.TRY_START_POSITIONING;
break;
case CalibrationState.AUTOCALIBRATION_RECEIVED:
autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
anchorReadCount = 0;
for (int i = 0; i < 8; i++)
{
ServerMessages.IPSFrameAnchorData anchorData = CalibrationSettings.GetAnchorData(i);
//anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).GetChild(0).GetComponent<Text>().text = anchorData.id.ToString();
//anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).name = anchorData.id.ToString();
anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(0).GetComponent <TextMesh>().text = "0x" + anchorData.id.ToString("x");
anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(1).localPosition = anchorData.position;
//Receiving the anchors position from autocalibration
for (int j = 1; j < 9; j++)
{
if (anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).name == anchorData.id.ToString())
{
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = anchorData.position.x.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = anchorData.position.y.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().text = anchorData.position.z.ToString();
anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Button>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().interactable = false;
anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Image>().color = new Vector4(0, 255, 0, 150);
anchorReadCount++;
//Painting orange the autocalibrated anchors
if (anchorData.order == 1 || anchorData.order == 2 || anchorData.order == 5 || anchorData.order == 6)
{
ColorBlock cb = anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().colors;
cb.disabledColor = new Vector4(1.0f, 0.6f, 0.0f, 0.4f);
cb.normalColor = new Vector4(1.0f, 0.8f, 0.4f, 1.0f);
cb.highlightedColor = new Vector4(1.0f, 0.9f, 0.7f, 1.0f);
anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().colors = cb;
}
if (anchorData.order == 2 || anchorData.order == 3 || anchorData.order == 6 || anchorData.order == 7)
{
ColorBlock cb = anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().colors;
cb.disabledColor = new Vector4(1.0f, 0.6f, 0.0f, 0.4f);
cb.normalColor = new Vector4(1.0f, 0.8f, 0.4f, 1.0f);
cb.highlightedColor = new Vector4(1.0f, 0.9f, 0.7f, 1.0f);
anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().colors = cb;
}
}
}
}
if (anchorReadCount == 8)
{
//Habilitating the three buttons
//UnityEngine.Debug.Log("AnchorsAutocalibratedReady");
// ## Se puede sacar fuera del if todo
confirmManual.gameObject.SetActive(false);
editConfig.gameObject.SetActive(true);
redoAutocalibration.gameObject.SetActive(true);
confirmAuto.gameObject.SetActive(true);
editConfig.interactable = true;
editConfig.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
redoAutocalibration.interactable = true;
redoAutocalibration.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
confirmAuto.interactable = true;
confirmAuto.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
//acceptConfig.interactable = true;
//acceptConfig.transform.GetChild(0).GetComponent<Text>().color = new Vector4(255, 255, 255, 255);
}
state = CalibrationState.AUTOCALIBRATION_FINNISHED;
break;
case CalibrationState.AUTOCALIBRATION_ACCEPTED:
autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
// Used to check the current state
break;
case CalibrationState.AUTOCALIBRATION_FINNISHED:
// Used to check the current state
autocalibratingThrobber.transform.parent.gameObject.SetActive(false);
break;
case CalibrationState.CALIBRATION_PREVIEW:
//Nobody uses this state ??
ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame();
if (frameData != null)
{
//UnityEngine.Debug.Log("Position: " + frameData.position);
//UnityEngine.Debug.Log("Rotation: " + frameData.rotation);
drone.transform.position = frameData.position;
drone.transform.rotation = Quaternion.Euler(frameData.rotation);
}
anchor1Pos = cam.WorldToScreenPoint(anchor1GO.transform.position);
anchor2Pos = cam.WorldToScreenPoint(anchor2GO.transform.position);
anchor3Pos = cam.WorldToScreenPoint(anchor3GO.transform.position);
anchor4Pos = cam.WorldToScreenPoint(anchor4GO.transform.position);
anchor5Pos = cam.WorldToScreenPoint(anchor5GO.transform.position);
anchor6Pos = cam.WorldToScreenPoint(anchor6GO.transform.position);
anchor7Pos = cam.WorldToScreenPoint(anchor7GO.transform.position);
anchor8Pos = cam.WorldToScreenPoint(anchor8GO.transform.position);
// ## TODO: Incluir en el if de arriba buscando cada anchorID según el ID actual y asociar su posición
anchor1ID.anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f);
anchor2ID.anchoredPosition = new Vector2(anchor2Pos.x - 40, anchor2Pos.y - Screen.height * 0.15f);
anchor3ID.anchoredPosition = new Vector2(anchor3Pos.x - 40, anchor3Pos.y - Screen.height * 0.15f);
anchor4ID.anchoredPosition = new Vector2(anchor4Pos.x - 40, anchor4Pos.y - Screen.height * 0.15f);
anchor5ID.anchoredPosition = new Vector2(anchor5Pos.x - 40, anchor5Pos.y - Screen.height * 0.15f);
anchor6ID.anchoredPosition = new Vector2(anchor6Pos.x - 40, anchor6Pos.y - Screen.height * 0.15f);
anchor7ID.anchoredPosition = new Vector2(anchor7Pos.x - 40, anchor7Pos.y - Screen.height * 0.15f);
anchor8ID.anchoredPosition = new Vector2(anchor8Pos.x - 40, anchor8Pos.y - Screen.height * 0.15f);
break;
case CalibrationState.TRY_START_POSITIONING:
//Check for positioning
if (PozyxModule.positiningIsValid)
{
firstPositionTry = true;
state = CalibrationState.POSITIONING_CONFIRMED;
}
else
{
if (firstPositionTry)
{
autocalibratingThrobber.transform.parent.GetComponent <Text>().text = "Waiting for positioning";
autocalibratingThrobber.transform.parent.gameObject.SetActive(true);
firstPositionTry = false;
}
autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime);
}
break;
case CalibrationState.POSITIONING_CONFIRMED:
//Change scene, positioning received
state = CalibrationState.IDLE;
autocalibratingThrobber.transform.parent.gameObject.SetActive(false);
//UnityEngine.Debug.Log(GeneralSceneManager.appState);
//You can access the scene from both buttons, as calibrating is always necessary, that is way there is a button to use last calibration and skip the process
if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Mapping)
{
SceneManager.LoadScene("Mapping");
}
else if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Recording)
{
SceneManager.LoadScene("PlanSelection");
}
break;
case CalibrationState.LAST_REQ_FAILED:
UnityEngine.Debug.Log("LastReqFailed");
if (discoverPanel.activeInHierarchy)
{ // Something failed discovering anchors, retry
UnityEngine.Debug.Log("DiscoverFailed");
dicoveredAnchors.text = PozyxModule.errorMsg;
state = CalibrationState.DISCOVERING;
}
else
{ // Something failed with anchors configuration, let the user change the config and try again
UnityEngine.Debug.Log("ConfigFailed");
state = CalibrationState.IDLE;
autocalibratingThrobber.transform.parent.gameObject.SetActive(false);
if (confirmManual.gameObject.activeInHierarchy)
{
confirmManual.interactable = true;
confirmManual.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
}
else if (confirmAuto.gameObject.activeInHierarchy)
{
confirmAuto.interactable = true;
confirmAuto.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
editConfig.interactable = true;
editConfig.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
redoAutocalibration.interactable = true;
redoAutocalibration.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f);
}
}
//discoverButton.SetActive(true);
//configAnchorsButton.SetActive(false);
//startingLabel.SetActive(true);
//searchingLabel.SetActive(false);
break;
}
}
