// Update is called once per frame void Update() { time += Time.deltaTime; if (time > 0.2f) { time = 0.0f; wsc.Connect(); wsc.ServiceCallerDB(srvName, srvReq); } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); ServiceResponseDB responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); //位置を取得&変換 Vector3 marker_position = new Vector3((float)responce.values.tmsdb[0].x, (float)responce.values.tmsdb[0].y, (float)responce.values.tmsdb[0].z); marker_position = Ros2UnityPosition(marker_position); marker_position.z += 0.2f; Debug.Log("Marker Pos: " + marker_position); //回転を取得&変換 Vector3 marker_euler = new Vector3(Rad2Euler((float)responce.values.tmsdb[0].rr), Rad2Euler((float)responce.values.tmsdb[0].rp), Rad2Euler((float)responce.values.tmsdb[0].ry)); VICONDataText.text = "Database: " + marker_euler.ToString(); marker_euler = Ros2UnityRotation(marker_euler); //Debug.Log("Marker rot raw: " + marker_euler); Quaternion marker_rot = Quaternion.Euler(marker_euler); //marker_rot *= Quaternion.Euler(0, 0, 180); marker_rot *= Quaternion.Euler(0, 180, 0); marker_euler = marker_rot.eulerAngles; marker_euler.x = 0.0f; marker_euler.z = 0.0f; Debug.Log("Marker rot: " + marker_euler); //回転をモデルに適用 transform.eulerAngles = marker_euler; Debug.Log("Model Euler: " + marker_euler); ModelDataText.text = "Model: " + marker_euler.ToString(); //位置をモデル上のマーカーに適用 /* * GameObject prefab = (GameObject)Resources.Load("Coordinates Adapter"); * irvs_marker = Instantiate(prefab); * irvs_marker.transform.parent = GameObject.Find("rostms/world_link").transform; * irvs_marker.transform.localPosition = marker_position; */ } }
/******************************************************* * データベースにある初期位置を取得 ******************************************************/ void InitPosition() { time += Time.deltaTime; if (time > 1.0f) { time = 0.0f; srvReq.tmsdb = new tmsdb("PLACE", 2009); wsc.ServiceCallerDB(srvName, srvReq); } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { string srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); ServiceResponseDB responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); foreach (tmsdb data in responce.values.tmsdb) { if (data.sensor == 3018) { foreach (Image image in image_list) { if (image.name.IndexOf(data.name) != -1) { if (data.state == 1) { Vector2 position = new Vector2((float)data.x, (float)data.y); Vector2 offset = new Vector2((float)offset_x_refrigerator, (float)offset_y_refrigerator); position -= offset; Debug.Log(data.name + ": " + position.ToString("f2")); recog.touch_position_of_refrigerator = position; //recog.ChangeImagePosition(image, position); recog.ChangeImagePosition(image); } else { Debug.Log(data.name + ": not exist"); recog.ChangeImagePosition(image, false); } } } } } init_flag = true; Debug.Log("Init Position Complete"); } }
/***************************************************************** * DBからVICONのデータを取得してポジショントラッキング *****************************************************************/ private void PositionTracking() { time_pos += Time.deltaTime; if (!DBAdapter.CheckWaitAnything() && time_pos > 1.0f) { time_pos = 0.0f; IEnumerator coroutine = DBAdapter.ReadSmartPalPos(); StartCoroutine(coroutine); } if (DBAdapter.CheckReadSmartPalPos()) { if (DBAdapter.CheckAbort()) { DBAdapter.ConfirmAbort(); } if (DBAdapter.CheckSuccess()) { ServiceResponseDB responce = DBAdapter.GetResponce(); DBAdapter.FinishReadData(); Vector3 sp5_pos = new Vector3((float)responce.values.tmsdb[0].x, (float)responce.values.tmsdb[0].y, (float)responce.values.tmsdb[0].z); sp5_pos = Ros2UnityPosition(sp5_pos); sp5_pos.y = 0.0f; sp5_pos += mainSystem.GetConfig().vicon_offset_pos; sp5_pos += mainSystem.GetConfig().robot_offset_pos; Vector3 sp5_euler = new Vector3(Rad2Euler((float)responce.values.tmsdb[0].rr), Rad2Euler((float)responce.values.tmsdb[0].rp), Rad2Euler((float)responce.values.tmsdb[0].ry)); sp5_euler = Ros2UnityRotation(sp5_euler); sp5_euler.x = 0.0f; sp5_euler.z = 0.0f; sp5_euler.y += mainSystem.GetConfig().robot_offset_yaw; transform.localPosition = sp5_pos; transform.localEulerAngles = sp5_euler; Debug.Log(responce.values.tmsdb[0].name + " pos: " + sp5_pos); mainSystem.MyConsole_Add(responce.values.tmsdb[0].name + " pos: " + sp5_pos); Debug.Log(responce.values.tmsdb[0].name + " eul: " + sp5_euler); mainSystem.MyConsole_Add(responce.values.tmsdb[0].name + " eul: " + sp5_euler); mainSystem.UpdateDatabaseInfoViconSmartPal(sp5_pos, sp5_euler); } } }
// Update is called once per frame //ずっと繰り返し呼び出されるよ~ void Update() { if (!mainSystem.finish_read_config) { return; } switch (calibration_state) { case 0: mainSystem.UpdateMainCanvasInfoText("Fail to Start"); break; case 1: mainSystem.UpdateMainCanvasInfoText("Can NOT Connect [" + mainSystem.GetConfig().ros_ip + "]"); break; case 2: mainSystem.UpdateMainCanvasInfoText("Access to Database"); break; case 3: mainSystem.UpdateMainCanvasInfoText("Please Look [IRVS Marker]"); break; case 4: mainSystem.UpdateMainCanvasInfoText("Ready to AR B-sen"); break; default: mainSystem.UpdateMainCanvasInfoText("Error : " + calibration_state.ToString()); break; } //phase 0 //毎回すること //AugmentedImageの更新 if (!Application.isEditor) { Session.GetTrackables <AugmentedImage>(m_AugmentedImages, TrackableQueryFilter.Updated); } //CameraとB-senのポジション表示 mainSystem.UpdateCalibrationInfoCamera(Camera.main.transform.position, Camera.main.transform.eulerAngles); if (mainSystem.GetConfig().old_calibration) { mainSystem.UpdateCalibrationInfoBsen(bsen_model.transform.position, bsen_model.transform.eulerAngles); } else { mainSystem.UpdateCalibrationInfoDevice(arcore_device.transform.position, arcore_device.transform.eulerAngles); } //どれだけ手動キャリブしてるか表示 if (mainSystem.GetConfig().old_calibration) { Vector3 offset_pos = bsen_model.transform.position - not_offset_pos; Vector3 offset_rot = bsen_model.transform.eulerAngles - not_offset_rot; mainSystem.UpdateCalibrationInfoOffset(offset_pos, offset_rot); } else { Vector3 offset_pos = arcore_device.transform.position - not_offset_pos; Vector3 offset_rot = arcore_device.transform.eulerAngles - not_offset_rot; mainSystem.UpdateCalibrationInfoOffset(offset_pos, offset_rot); } //自動キャリブ終了前 if (!CheckFinishCalibration()) { switch (calibration_state) { //DBにアクセス開始 case 1: if (DBAdapter.IsConnected() && !DBAdapter.CheckWaitAnything()) { IEnumerator coroutine = DBAdapter.ReadMarkerPos(); StartCoroutine(coroutine); calibration_state = 2; } break; //DBのデータをもとにモデルの位置&回転を変更 case 2: if (DBAdapter.CheckSuccess()) { ServiceResponseDB responce = DBAdapter.GetResponce(); DBAdapter.FinishReadData(); //位置を取得&変換 Vector3 marker_position = new Vector3((float)responce.values.tmsdb[0].x, (float)responce.values.tmsdb[0].y, (float)responce.values.tmsdb[0].z); marker_position = Ros2UnityPosition(marker_position); marker_position += mainSystem.GetConfig().vicon_offset_pos; marker_position += mainSystem.GetConfig().calibration_offset_pos; Debug.Log("Marker Pos: " + marker_position); mainSystem.MyConsole_Add("Marker Pos: " + marker_position); //回転を取得&変換 Vector3 marker_euler = new Vector3(Rad2Euler((float)responce.values.tmsdb[0].rr), Rad2Euler((float)responce.values.tmsdb[0].rp), Rad2Euler((float)responce.values.tmsdb[0].ry)); marker_euler = Ros2UnityRotation(marker_euler); if (mainSystem.GetConfig().old_calibration) { marker_euler *= -1.0f; } marker_euler.x = 0.0f; marker_euler.z = 0.0f; marker_euler.y += mainSystem.GetConfig().calibration_offset_yaw; Debug.Log("Marker Rot: " + marker_euler); mainSystem.MyConsole_Add("Marker Rot: " + marker_euler); mainSystem.UpdateDatabaseInfoViconIRVSMarker(marker_position, marker_euler); //回転をモデルに適用 if (mainSystem.GetConfig().old_calibration) { bsen_model.transform.eulerAngles = marker_euler; } //位置と回転をモデル上のマーカーに適用 irvs_marker = Instantiate(new GameObject()); irvs_marker.name = "IRVS Marker"; irvs_marker.transform.SetParent(GameObject.Find("rostms/world_link").transform, false); irvs_marker.transform.localPosition = marker_position; irvs_marker.transform.localEulerAngles = marker_euler; //回転軸をマーカーの位置に合わせる if (mainSystem.GetConfig().old_calibration) { GameObject world_link = GameObject.Find("rostms/world_link"); world_link.transform.localPosition = marker_position * -1; } calibration_state = 3; } break; //画像認識したらキャリブレーションしてモデルを表示 //UnityEditor上ではここはスキップ case 3: if (Application.isEditor) { rostms_shader.alpha = 0.6f; rostms_shader.ChangeColors(); calibration_state = 4; finish_calibration = true; return; } if (!detected_marker) { foreach (var image in m_AugmentedImages) { if (image.TrackingState == TrackingState.Tracking) { detected_marker = true; marker_image = image; autoPositioning(); rostms_shader.alpha = 0.6f; rostms_shader.ChangeColors(); calibration_state = 4; finish_calibration = true; } } } //自動キャリブ終了時の位置と回転を保存 if (mainSystem.GetConfig().old_calibration) { not_offset_pos = bsen_model.transform.position; not_offset_rot = bsen_model.transform.eulerAngles; } else { not_offset_pos = arcore_device.transform.position; not_offset_rot = arcore_device.transform.eulerAngles; } break; } } else //手動キャリブ { manualCalibration(); } }
// Update is called once per frame void Update() { if (!mainSystem.finish_read_config) { return; } //CoordinatesAdapterの位置を調整してカメラとの距離を計算 coordinates_adapter.transform.localPosition = new Vector3(-0.23f, 0.0f, -0.3f); distance_old = distance; distance = CalcDistance(coordinates_adapter, ar_camera); //最初の1回Shaderを変更する if (!change_goods_shader) { foreach (GoodsData goods in goods_data_dictionary.Values) { ShaderChange shaderchange = goods.obj.GetComponent <ShaderChange>(); shaderchange.ChangeShader(Shader.Find("Custom/Transparent")); } change_goods_shader = true; } //距離が閾値以下でデータベースのstateが1だったら表示,違ったら非表示 if (calib_system.CheckFinishCalibration() && finish_coroutine) { //近づいたとき if (distance < mainSystem.GetConfig().refrigerator_distance&& distance_old >= mainSystem.GetConfig().refrigerator_distance) { refrigerator_shaderchange.ChangeShader(Shader.Find("Custom/Transparent")); refrigerator_shaderchange.alpha = 0.4f; refrigerator_shaderchange.ChangeColors(); IEnumerator coroutine = AppearSlowly(); StartCoroutine(coroutine); } //遠くにいるとき else if (distance >= mainSystem.GetConfig().refrigerator_distance) { foreach (GoodsData goods in goods_data_dictionary.Values) { ShaderChange goods_shaderchange = goods.obj.GetComponent <ShaderChange>(); goods_shaderchange.alpha = 0.0f; goods_shaderchange.ChangeColors(); goods.text3d.SetActive(false); } if (refrigerator_shaderchange.shader_now != Shader.Find("Custom/ARTransparent")) { refrigerator_shaderchange.ChangeShader(Shader.Find("Custom/ARTransparent")); refrigerator_shaderchange.alpha = rostms_shaderchange.alpha; refrigerator_shaderchange.ChangeColors(); } } //ずっと近くにいるとき else if (distance < mainSystem.GetConfig().refrigerator_distance&& distance_old < mainSystem.GetConfig().refrigerator_distance) { foreach (GoodsData goods in goods_data_dictionary.Values) { ShaderChange goods_shaderchange = goods.obj.GetComponent <ShaderChange>(); if (goods.state_bool) { goods_shaderchange.alpha = 0.4f; goods.text3d.SetActive(true); } else { goods_shaderchange.alpha = 0.0f; goods.text3d.SetActive(false); } goods_shaderchange.ChangeColors(); } } } //画像認識による自動キャリブレーションが終わった後に実行 if (calib_system.CheckFinishCalibration()) { //冷蔵庫に入っている物品のデータを取得 time_1 += Time.deltaTime; if (!DBAdapter.CheckWaitAnything() && time_1 > 1.0f) { time_1 = 0.0f; IEnumerator coroutine = DBAdapter.GetRefrigeratorItem(); StartCoroutine(coroutine); } if (DBAdapter.CheckGetRefrigeratorItem()) { if (DBAdapter.CheckAbort()) { DBAdapter.ConfirmAbort(); } if (DBAdapter.CheckSuccess()) { id_list = new List <int>(); ServiceResponseDB responce = DBAdapter.GetResponce(); DBAdapter.FinishReadData(); foreach (tmsdb data in responce.values.tmsdb) { //Debug.Log(data.name); //Debug.Log(data.x + ", " + data.y + ", " + data.z); if (data.sensor == 3018 && goods_data_dictionary.ContainsKey(data.id)) { GoodsData goods = goods_data_dictionary[data.id]; Vector3 place = new Vector3((float)data.x, (float)data.y, (float)data.z); place = Ros2UnityPosition(place); if (data.state == 1) { goods.state_bool = true; Debug.Log(data.name + " pos: " + place.ToString("f2")); mainSystem.MyConsole_Add(data.name + " pos: " + place.ToString("f2")); goods.obj.transform.localPosition = place; id_list.Add(data.id); } else { goods.state_bool = false; } if (goods.name == null) { goods.name = data.name; } if (goods.expiration == null) { goods.expiration = "don't know"; } goods.state = data.state; goods.pos = place; } } } } //冷蔵庫に入っている物品の消費期限を取得 time_2 += Time.deltaTime; if (!DBAdapter.CheckWaitAnything() && time_2 > 5.0f) { time_2 = 0.0f; DBAdapter.GiveItemIDList(id_list); IEnumerator coroutine = DBAdapter.ReadExpiration(); StartCoroutine(coroutine); } if (DBAdapter.CheckReadExpiration()) { if (DBAdapter.CheckAbort()) { DBAdapter.ConfirmAbort(); } if (DBAdapter.CheckSuccess()) { Dictionary <int, string> expiration_dictionary = DBAdapter.ReadExpirationData(); DBAdapter.FinishReadData(); foreach (KeyValuePair <int, string> goods in expiration_dictionary) { ExpirationData expiration_data = JsonUtility.FromJson <ExpirationData>(goods.Value); string expiration = expiration_data.expiration; Debug.Log("id: " + goods.Key + ", name: " + goods_data_dictionary[goods.Key].name + ", expiration: " + expiration); mainSystem.MyConsole_Add("id: " + goods.Key + ", name: " + goods_data_dictionary[goods.Key].name + ", expiration: " + expiration); goods_data_dictionary[goods.Key].text3d.GetComponent <TextMeshPro>().text = expiration; goods_data_dictionary[goods.Key].expiration = expiration; } } } //UIを更新する Dictionary <int, string> goods_info_string_dictionary = new Dictionary <int, string>(); foreach (KeyValuePair <int, GoodsData> goods in goods_data_dictionary) { string info = goods.Value.name + ", " + goods.Key.ToString() + ", " + goods.Value.state.ToString() + ", " + goods.Value.pos.ToString("f3") + ", " + goods.Value.expiration; goods_info_string_dictionary.Add(goods.Key, info); } mainSystem.UpdateDatabaseInfoRefrigerator(goods_info_string_dictionary); } }
// Update is called once per frame void Update() { if (wsc.conneciton_state == wscCONST.STATE_DISCONNECTED) { time += Time.deltaTime; if (time > 5.0f) { time = 0.0f; wsc.Connect(); } } if (wsc.conneciton_state == wscCONST.STATE_CONNECTED) { if (!success_access && !abort_access) { if (access_db) { if (read_marker_pos) { time += Time.deltaTime; if (time > 1.0f) { time = 0.0f; srvReq.tmsdb = new tmsdb("ID_SENSOR", 7030, 3001); wsc.ServiceCallerDB(srvName, srvReq); } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); success_access = true; access_db = false; } } if (get_refrigerator_item) { time += Time.deltaTime; if (time > 1.0f) { time = 0.0f; abort_access = true; access_db = false; } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); success_access = true; access_db = false; } } if (read_smartpal_pos) { time += Time.deltaTime; if (time > 0.5f) { time = 0.0f; abort_access = true; access_db = false; } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); success_access = true; access_db = false; } } if (read_whs1) { time += Time.deltaTime; if (time > 0.5f) { time = 0.0f; abort_access = true; access_db = false; } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); success_access = true; access_db = false; } } if (read_expiration) { time += Time.deltaTime; if (time > 0.5f) { time = 0.0f; abort_access = true; access_db = false; } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); access_db = false; } } if (read_battery) { time += Time.deltaTime; if (time > 0.5f) { time = 0.0f; abort_access = true; access_db = false; } if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == srvName) { srvRes = wsc.GetSrvResMsg(); Debug.Log("ROS: " + srvRes); responce = JsonUtility.FromJson <ServiceResponseDB>(srvRes); success_access = true; access_db = false; } } } } } }
/***************************************************************** * DBからバッテリー情報を取得して表示 *****************************************************************/ private void UpdateBatteryInformation() { time_bat += Time.deltaTime; if (!DBAdapter.CheckWaitAnything() && time_bat > 1.0f) { time_bat = 0.0f; IEnumerator coroutine = DBAdapter.ReadBattery(); StartCoroutine(coroutine); } if (DBAdapter.CheckReadBattery()) { if (DBAdapter.CheckAbort()) { DBAdapter.ConfirmAbort(); } if (DBAdapter.CheckSuccess()) { ServiceResponseDB responce = DBAdapter.GetResponce(); DBAdapter.FinishReadData(); BatteryData battery_data = JsonUtility.FromJson <BatteryData>(responce.values.tmsdb[0].etcdata); float battery_per = battery_data.battery * 100; Debug.Log("SmartPal Battery: " + battery_per + "[%]"); mainSystem.MyConsole_Add("SmartPal Battery: " + battery_per + "[%]"); if (!finish_battery_text) { Battery_3DText = (GameObject)Instantiate(Resources.Load("TextMeshPro")); Battery_3DText.transform.SetParent(transform, false); Battery_3DText.transform.localPosition = new Vector3(0.0f, 1.5f, 0.0f); TextMeshPro TMP = Battery_3DText.GetComponent <TextMeshPro>(); TMP.fontSize = 1.0f; TMP.text = "Battery: " + battery_per.ToString() + "[%]"; finish_battery_text = true; } else { Battery_3DText.GetComponent <TextMeshPro>().text = "Battery: " + battery_per.ToString() + "[%]"; } mainSystem.UpdateDatabaseInfoSmartPalBattery(battery_per); } } //カメラとSmartPalの距離が近づいたら表示 if (Battery_3DText != null) { //if(CalcDistance(Camera.main.gameObject, transform.gameObject) < 2.0f) { if (CalcDistance(Camera.main.gameObject, transform.gameObject) < mainSystem.GetConfig().robot_battery_distance) { Battery_3DText.SetActive(true); } else { Battery_3DText.SetActive(false); } } }
/***************************************************************** * 表示する文字を更新 *****************************************************************/ private void WHS1DataUpdate() { time += Time.deltaTime; if (!DBAdapter.CheckWaitAnything() && time > 0.2f) { time = 0.0f; IEnumerator coroutine = DBAdapter.ReadWHS1(); StartCoroutine(coroutine); } if (DBAdapter.CheckReadWHS1()) { if (DBAdapter.CheckAbort()) { DBAdapter.ConfirmAbort(); } if (DBAdapter.CheckSuccess()) { ServiceResponseDB responce = DBAdapter.GetResponce(); DBAdapter.FinishReadData(); Debug.Log(responce.values.tmsdb[0].note); //mainSystem.MyConsole_Add(responce.values.tmsdb[0].note); WHS1Data whs1_data = JsonUtility.FromJson <WHS1Data>(responce.values.tmsdb[0].note); Debug.Log("Temp: " + whs1_data.temp.ToString("f1")); mainSystem.MyConsole_Add("Temp: " + whs1_data.temp.ToString("f1")); Debug.Log("Rate: " + whs1_data.rate); mainSystem.MyConsole_Add("Rate: " + whs1_data.rate); string debug_string = null; foreach (int count in whs1_data.wave) { debug_string += count + ","; } Debug.Log("Wave: " + debug_string); //mainSystem.MyConsole_Add("Wave: " + debug_string); WHS1_3D_TextMeshPro.text = "Temp: " + whs1_data.temp.ToString("f1") + "[degC]\n"; WHS1_3D_TextMeshPro.text += "Rate: " + whs1_data.rate.ToString() + "[bpm]"; UpdateWaveGraph(whs1_data.wave); mainSystem.UpdateDatabaseInfoWHS1Info(whs1_data.temp, whs1_data.rate); mainSystem.UpdateDatabaseInfoWHS1Wave(whs1_data.wave); } } //カメラと近いときに表示 if (WHS1_3D_Text != null) { if (CalcDistance(Camera.main.gameObject, WHS1_3D_Text) < mainSystem.GetConfig().whs1_distance) { WHS1_3D_Text.SetActive(true); } else { WHS1_3D_Text.SetActive(false); } } }