void Update()
{
if (!Enabled)
{
regionsFromRosparam = null;
nowLoadingFromRosparam = false;
loadedFromRosparamAndWaitingProcessed = false;
loadedAndProcessed = false;
if (regionGameObjects != null && regionGameObjects.Length > 0)
{
foreach (var go in regionGameObjects)
{
Destroy(go);
}
regionGameObjects = null;
}
return;
}
// ====
// まずはROSPARM側の情報を読んでViewerに反映する
// ====
if (regionsFromRosparam == null && !nowLoadingFromRosparam)
{
// 読み込みリクエスト
GetRegionsFromRosparam();
nowLoadingFromRosparam = true;
return;
}
else if (regionsFromRosparam != null && nowLoadingFromRosparam)
{
// 読み込み完了・処理リクエスト
nowLoadingFromRosparam = false;
loadedFromRosparamAndWaitingProcessed = true;
return;
}
else if (loadedFromRosparamAndWaitingProcessed)
{
loadedFromRosparamAndWaitingProcessed = false;
regionGameObjects = new GameObject[regionsFromRosparam.Length];
Debug.Log($"ROSPARM LENGTH: {regionsFromRosparam.Length}");
for (int index = 0; index < regionsFromRosparam.Length; index++)
{
Debug.Log($"Region {index} convert to Unity GameObj");
Region r = regionsFromRosparam[index];
GameObject go = CreateGameObject.CubeRegion();
go.tag = "RegionForeground";
Vector3 maxPtRos = new Vector3(r.maxPt.x, r.maxPt.y, r.maxPt.z);
Vector3 minPtRos = new Vector3(r.minPt.x, r.minPt.y, r.minPt.z);
// 変換によって必ずしもmax, min pointでなくなる点に注意
Vector3 p1 = CoordinateConvert.RosToUnity(maxPtRos);
Vector3 p2 = CoordinateConvert.RosToUnity(minPtRos);
Vector3 pos = (p1 + p2) / 2;
Vector3 scale = new Vector3(Mathf.Abs(p1.x - p2.x), Mathf.Abs(p1.y - p2.y), Mathf.Abs(p1.z - p2.z));
go.transform.position = pos;
go.transform.localScale = scale;
regionGameObjects[index] = go;
}
loadedAndProcessed = true;
return;
}
// ====
// 現状のUnityサイドの情報を定期的にROSPARMに反映する
// ====
if (loadedAndProcessed)
{
if (!UpdateRegions)
{
return;
}
Debug.Log("Update Regions");
/*
* if (CheckCount > 0)
* {
* CheckCount -= 1;
* return;
* }
*/
UpdateRegions = false;
CheckCount = UpdateInterval;
SetRegionsToRosparam();
}
}