public PozyxModule()
{
positiningIsValid = false;
clientUnity = UnityEngine.GameObject.Find("ClientUnity").GetComponent <ClientUnity>();
ips_tag_frames = new ServerMessages.IPSDroneTag();
// Notifications from pozyxsource
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_POSITIONING_STARTED, onIPSStarted));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_POSITIONING_STOPPED, onIPSStopped));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)NotificationType.QUITTING, onIPSQuitting));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_LOCALTAG_CONNECTED, onIPSConnected));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DISCOVERED, onIPSDiscovered));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_AUTOCALIBRATED, onIPSAutocalibrated));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_UPDATED_SETTINGS, onIPSUpdated));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DATA_ANCHOR, onIPSAnchorData)); // anchor data
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DATA_ANCHORS_LIST, onIPSAnchorList));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_NOTENOUGH_TAGSFOUND, onIPSNotenoughTags));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_NOTENOUGH_ANCHORSFOUND, onIPSNotenoughAnchors));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_LAST_REQUEST_HAS_FAILED, onIPSLastRequestFailed));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_ANCHORS_MANUAL_CONFIG_ACCEPTED, onIPSManualConfigAccepted));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_ANCHORS_TOBE_AUTOCALIBRATED_ACCEPTED, onIPSAnchorsToBeAutocalibratedAccepted));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DRONETAGS_LIST, onDroneTagsList));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DRONETAGS, onDroneLastTagData));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_DRONETAGS_MANUAL_CONFIG_ACCEPTED, onConfirmedManualConfig));
clientUnity.mhandlers.Add(new MsgHandler((byte)Modules.POSITIONING_NOTIFICATIONS_MODULE, (byte)PositioningNotificationType.IPS_GET_DRONEFILTER_NOTIFICATION, onGetDroneFilter));
}
// Confirm tag config button listener
void ConfirmTagConfiguration()
{
if ((clientUnity != null) && (clientUnity.client != null) && (clientUnity.client.isConnected))
{
if (swValue >= 0 && nwValue >= 0 && neValue >= 0 && seValue >= 0 && widthValue >= 0 && heightValue >= 0)
{
state = TagsConfigState.ConfigurationSent;
confirmButton.GetComponent <Button>().interactable = false;
ServerMessages.DroneCamInfo camInfo = new ServerMessages.DroneCamInfo(camDistValueX, camDistValueY, camDistValueZ);
ServerMessages.IPSDroneTag tagData = new ServerMessages.IPSDroneTag(swValue, nwValue, neValue, seValue, widthValue, heightValue, (int)camDistValueY);
clientUnity.client.sendCommand((byte)Modules.POSITIONING_MODULE, (byte)PositioningCommandType.IPS_DRONETAGS_MANUAL_CONFIG, tagData);
clientUnity.client.sendCommand((byte)Modules.DRONE_MODULE, (byte)DroneCommandType.DRONE_SET_ZCAMERA_OFFSET, camInfo);
clientUnity.client.sendSetDroneFilter((byte)Modules.POSITIONING_MODULE, (byte)PositioningCommandType.IPS_SET_DRONEFILTER, 0, PozyxModule.updatePeriod, PozyxModule.movementFreedom);
}
else
{
UnityEngine.Debug.Log("Some invalid values");
PozyxModule.errorMsg = "Some invald values";
state = TagsConfigState.ErrorReceived;
}
}
}
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;
}
}
/// <summary>
/// Send the tags configuration
/// </summary>
/// <param name="module"></param>
/// <param name="action"></param>
/// <param name="data"></param>
/// <returns></returns>
public bool sendCommand(byte module, byte action, ServerMessages.IPSDroneTag data)
{
byte[] data_bytes = new byte[31]; // { (byte)'A', module, action, data.id, data.order, data.position };
data_bytes[0] = (byte)'A';
data_bytes[1] = module;
data_bytes[2] = action;
byte[] converted = BitConverter.GetBytes(data.width);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 3] = converted[i];
}
converted = BitConverter.GetBytes(data.height);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 7] = converted[i];
}
converted = BitConverter.GetBytes(data.idSW);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 11] = converted[i];
}
converted = BitConverter.GetBytes(data.idNW);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 15] = converted[i];
}
converted = BitConverter.GetBytes(data.idNE);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 19] = converted[i];
}
converted = BitConverter.GetBytes(data.idSE);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 23] = converted[i];
}
converted = BitConverter.GetBytes(data.camDist);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 27] = converted[i];
}
NetMQMessage req_msg = new NetMQMessage();
req_msg.Append(data_bytes);
NetMQ.Msg resp_msg = new StdMessage(0x00, 0x00).to_Msg(); // it will be filled when receiving the response
reqSrv = new ReliableExternalClient(
GlobalSettings.Instance.getRequestPort(), TimeSpan.FromMilliseconds(1000), 3);
if (!reqSrv.sendAndReceive(ref req_msg, ref resp_msg))
{
UnityEngine.Debug.Log("Client: server not respoding");
return(false);
}
// Just after the request the atreyu server must respond with a notification
// (ACK, UNKNOWN_MODULE, or UNDEFINED_MODULE) through the same 'requests' socket.
if (StdMessage.isStdNotification(ref resp_msg) && StdMessage.getMessageAction(ref resp_msg) != (byte)NotificationType.ACK)
{
if (module == (byte)Modules.STD_COMMANDS_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: STD command was not accepted {0}", new StdMessage(module, action).to_string()));
}
else if (module == (byte)Modules.POSITIONING_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POSITIONING_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
else if (module == (byte)Modules.POINTCLOUD_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POINTCLOUD_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
return(false);
}
return(true);
}
