private void Update()
    {
        if (!mainSystem.finish_read_config)
        {
            return;
        }
        else
        {
            if (!finish_set_address)
            {
                ws = new WebSocket(mainSystem.GetConfig().ros_ip);
                //open message
                ws.OnOpen += (sender, e) => {
                    Debug.Log("*********** Websocket connected ***********");
                    conneciton_state = wscCONST.STATE_CONNECTED;
                };
                //close message
                ws.OnClose += (sender, e) => {
                    Debug.Log("*********** Websocket disconnected ***********");
                    conneciton_state = wscCONST.STATE_DISCONNECTED;
                };
                //error message
                ws.OnError += (sender, e) => {
                    Debug.Log("Error : " + e.Message);
                    conneciton_state = wscCONST.STATE_ERROR;
                };
                OnMessage();

                finish_set_address = true;
                Debug.Log("OK ROS_IP");

                Connect();
            }
        }
    }
Example #2
0
    /*****************************************************************
    * 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);
            }
        }
    }
    IEnumerator SendReset()
    {
        while (cm.CheckWaitAnything())
        {
            yield return(null);
        }

        IEnumerator coroutine = cm.LeftGripperMove(mainSystem.GetConfig().left_gripper_home_pos *Mathf.Deg2Rad);

        StartCoroutine(coroutine);

        while (cm.CheckWaitLeftGripperMove())
        {
            if (cm.CheckAbort() || cm.CheckSuccess())
            {
                cm.FinishAccess();
            }
            yield return(null);
        }
    }
Example #4
0
    /**************************************************
    * 最初の設定
    **************************************************/
    void SetupWebSocket()
    {
        WebSocket = new WebSocket(Main.GetConfig().ros_ip);

        //接続したとき
        WebSocket.OnOpen += (sender, e) => {
            Debug.Log("*********** Websocket connected ***********");
            connection_state = ConnectionState.Connected;
        };

        //切断したとき
        WebSocket.OnClose += (sender, e) => {
            Debug.Log("*********** Websocket disconnected ***********");
            connection_state = ConnectionState.Disconnected;
        };

        //エラーが出たとき
        WebSocket.OnError += (sender, e) => {
            Debug.Log("Error : " + e.Message);
            connection_state = ConnectionState.Error;
        };

        //ROSからメッセージが来たとき
        WebSocket.OnMessage += (sender, e) => {
            //receive_json = e.Data;
            string receive_json = e.Data;
            //Debug.Log("ROS : " + receive_json);

            int index_op    = receive_json.IndexOf("\"op\"");
            int index_colon = receive_json.IndexOf(":", index_op);
            int index_end   = receive_json.IndexOf(",", index_colon);
            if (index_end == -1)
            {
                index_end = receive_json.IndexOf("}", index_colon);
            }
            string op = receive_json.Substring(index_colon + 1, index_end - index_colon - 1);
            op = op.Replace(" ", "");
            op = op.Replace("\"", "");

            switch (op)
            {
            case "publish":
                //topic_json = receive_json;

                /*
                 * string topic_name = JsonUtility.FromJson<Publish>(receive_json).topic;
                 * if (ReceivedTopicDictionary.ContainsKey(topic_name)) { ReceivedTopicDictionary.Remove(topic_name); }
                 * ReceivedTopicDictionary.Add(topic_name, receive_json);
                 */
                ReceivedTopicDictionary[JsonUtility.FromJson <Publish>(receive_json).topic] = receive_json;
                break;

            case "service_response":
                //service_response_json = receive_json;
                ReceivedServiceResponseDictionary[JsonUtility.FromJson <ServiceResponse>(receive_json).service] = receive_json;
                break;

            case "call_service":
                //service_request_json = receive_json;
                ReceivedServiceRequestDictionary[JsonUtility.FromJson <CallService>(receive_json).service] = receive_json;
                break;

            default:
                break;
            }
        };

        finish_setup = true;
        Debug.Log("Finish WebSocket Setting");
    }
Example #5
0
    // 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);
        }
    }
    /**************************************************
    * Right Arm Reset
    **************************************************/
    public IEnumerator RightArmReset()
    {
        wait_anything = access_db = wait_RightArmReset = true;
        time_access   = 0.0f;

        float[] arg = new float[8];
        arg[0] = mainSystem.GetConfig().right_arm_home_pos[0] * Mathf.Deg2Rad;
        arg[1] = mainSystem.GetConfig().right_arm_home_pos[1] * Mathf.Deg2Rad;
        arg[2] = mainSystem.GetConfig().right_arm_home_pos[2] * Mathf.Deg2Rad;
        arg[3] = mainSystem.GetConfig().right_arm_home_pos[3] * Mathf.Deg2Rad;
        arg[4] = mainSystem.GetConfig().right_arm_home_pos[4] * Mathf.Deg2Rad;
        arg[5] = mainSystem.GetConfig().right_arm_home_pos[5] * Mathf.Deg2Rad;
        arg[6] = mainSystem.GetConfig().right_arm_home_pos[6] * Mathf.Deg2Rad;
        arg[7] = mainSystem.GetConfig().right_arm_move_speed *Mathf.Deg2Rad;
        string msg = "";

        foreach (float n in arg)
        {
            msg += n.ToString() + ", ";
        }
        msg = msg.Substring(0, msg.Length - 2);
        Debug.Log("SendMessage arg: " + msg);
        Req_sp5_control srvReq = new Req_sp5_control(2, 15, arg);

        ServiceCaller_sp5_control(srvReq);

        while (access_db)
        {
            yield return(null);
        }

        while (success_access || abort_access)
        {
            yield return(null);
        }

        wait_anything = wait_RightArmReset = false;
    }
Example #8
0
    /*****************************************************************
    * 表示する文字を更新
    *****************************************************************/
    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);
            }
        }
    }