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