├── .gitignore ├── Assets ├── URG.meta ├── URG │ ├── SCIP_library.cs │ ├── SCIP_library.cs.meta │ ├── UrgDevice.cs │ ├── UrgDevice.cs.meta │ ├── UrgDeviceEthernet.cs │ └── UrgDeviceEthernet.cs.meta ├── URGSample.cs ├── URGSample.cs.meta ├── sample.unity └── sample.unity.meta ├── ProjectSettings ├── AudioManager.asset ├── ClusterInputManager.asset ├── DynamicsManager.asset ├── EditorBuildSettings.asset ├── EditorSettings.asset ├── GraphicsSettings.asset ├── InputManager.asset ├── NavMeshAreas.asset ├── NavMeshLayers.asset ├── NetworkManager.asset ├── Physics2DSettings.asset ├── ProjectSettings.asset ├── ProjectVersion.txt ├── QualitySettings.asset ├── TagManager.asset ├── TimeManager.asset ├── UnityAdsSettings.asset └── UnityConnectSettings.asset └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | [Ll]ibrary/ 2 | [Tt]emp/ 3 | [Oo]bj/ 4 | 5 | # Autogenerated VS/MD solution and project files 6 | /*.csproj 7 | /*.unityproj 8 | /*.sln 9 | /*.suo 10 | /*.user 11 | /*.userprefs 12 | /*.pidb 13 | /*.booproj -------------------------------------------------------------------------------- /Assets/URG.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: db46c6dcc15b643c2b5601b5c91743ef 3 | folderAsset: yes 4 | DefaultImporter: 5 | userData: 6 | -------------------------------------------------------------------------------- /Assets/URG/SCIP_library.cs: -------------------------------------------------------------------------------- 1 | // http://sourceforge.net/p/urgnetwork/wiki/cs_sample_jp/ 2 | 3 | using System; 4 | using System.Collections.Generic; 5 | using System.Text; 6 | 7 | using UnityEngine; 8 | 9 | namespace SCIP_library 10 | { 11 | public class SCIP_Writer 12 | { 13 | public static string END 14 | { 15 | get{ return "\n"; } 16 | } 17 | 18 | /// measurement start step 19 | /// measurement end step 20 | /// grouping step number 21 | /// skip scan number 22 | /// get scan numbar 23 | /// created command 24 | public static string MD(int start, int end, int grouping = 1, int skips = 0, int scans = 0) 25 | { 26 | return UrgDevice.GetCMDString(UrgDevice.CMD.MD) + start.ToString("D4") + end.ToString("D4") + grouping.ToString("D2") + skips.ToString("D1") + scans.ToString("D2") + END; 27 | } 28 | public static string ME(int start, int end, int grouping = 1, int skips = 0, int scans = 0) 29 | { 30 | return UrgDevice.GetCMDString(UrgDevice.CMD.ME) + start.ToString("D4") + end.ToString("D4") + grouping.ToString("D2") + skips.ToString("D1") + scans.ToString("D2") + END; 31 | } 32 | 33 | public static string BM() 34 | { 35 | return UrgDevice.GetCMDString(UrgDevice.CMD.BM) + END; 36 | } 37 | public static string GD(int start, int end, int grouping = 1) 38 | { 39 | return UrgDevice.GetCMDString(UrgDevice.CMD.GD) + start.ToString("D4") + end.ToString("D4") + grouping.ToString("D2") + END; 40 | } 41 | 42 | public static string VV() 43 | { 44 | return UrgDevice.CMD.VV.ToString() + END; 45 | } 46 | 47 | public static string II() 48 | { 49 | return UrgDevice.GetCMDString(UrgDevice.CMD.II) + END; 50 | } 51 | 52 | public static string PP() 53 | { 54 | return UrgDevice.GetCMDString(UrgDevice.CMD.PP) + END; 55 | } 56 | 57 | public static string SCIP2() 58 | { 59 | return "SCIP2.0" + END; 60 | } 61 | 62 | public static string QT() 63 | { 64 | return UrgDevice.GetCMDString(UrgDevice.CMD.QT) + END; 65 | } 66 | } 67 | 68 | public class SCIP_Reader 69 | { 70 | /// 71 | /// read MD command 72 | /// 73 | /// received command 74 | /// timestamp data 75 | /// distance data 76 | /// is successful 77 | public static bool MD(string get_command, ref long time_stamp, ref List distances) 78 | { 79 | //distances.Clear(); 80 | string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries); 81 | 82 | // if (!split_command[0].StartsWith("MD")) { 83 | // return false; 84 | // } 85 | 86 | if (split_command[1].StartsWith("00")) { 87 | return true; 88 | } else if (split_command[1].StartsWith("99")) { 89 | time_stamp = SCIP_Reader.decode(split_command[2], 4); 90 | distance_data(split_command, 3, ref distances); 91 | return true; 92 | } else { 93 | return false; 94 | } 95 | } 96 | 97 | public static bool GD(string get_command, ref long time_stamp, ref List distances) 98 | { 99 | //distances.Clear(); 100 | string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries); 101 | 102 | // if (!split_command[0].StartsWith("GD")) { 103 | // return false; 104 | // } 105 | 106 | if (split_command[1].StartsWith("00")) { 107 | time_stamp = SCIP_Reader.decode(split_command[2], 4); 108 | distance_data(split_command, 3, ref distances); 109 | return true; 110 | } else { 111 | return false; 112 | } 113 | } 114 | 115 | /// 116 | /// read distance data 117 | /// 118 | /// 119 | /// 120 | /// 121 | public static bool distance_data(string[] lines, int start_line, ref List distances) 122 | { 123 | StringBuilder sb = new StringBuilder(); 124 | for (int i = start_line; i < lines.Length; ++i) { 125 | sb.Append(lines[i].Substring(0, lines[i].Length - 1)); 126 | } 127 | return SCIP_Reader.decode_array(sb.ToString(), 3, ref distances); 128 | } 129 | 130 | /// 131 | /// decode part of string 132 | /// 133 | /// encoded string 134 | /// encode size 135 | /// decode start position 136 | /// decode result 137 | public static long decode(string data, int size, int offset = 0) 138 | { 139 | long value = 0; 140 | 141 | for (int i = 0; i < size; ++i) { 142 | value <<= 6; 143 | value |= (long)data[offset + i] - 0x30; 144 | } 145 | 146 | return value; 147 | } 148 | 149 | /// 150 | /// decode multiple data 151 | /// 152 | /// encoded string 153 | /// encode size 154 | /// decode result 155 | public static bool decode_array(string data, int size, ref List decoded_data) 156 | { 157 | for (int pos = 0; pos <= data.Length - size; pos += size) { 158 | decoded_data.Add(decode(data, size, pos)); 159 | } 160 | return true; 161 | } 162 | 163 | 164 | public static bool ME(string get_command, ref long time_stamp, ref List distances, ref List strengths) 165 | { 166 | string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries); 167 | 168 | // if (!split_command[0].StartsWith("ME")) { 169 | // return false; 170 | // } 171 | 172 | if (split_command[1].StartsWith("00")) { 173 | return true; 174 | } else if (split_command[1].StartsWith("99")) { 175 | time_stamp = SCIP_Reader.decode(split_command[2], 4); 176 | distance_strength_data(split_command, 3, ref distances, ref strengths); 177 | return true; 178 | } else { 179 | return false; 180 | } 181 | } 182 | 183 | public static bool distance_strength_data(string[] lines, int start_line, ref List distances, ref List strengths) 184 | { 185 | StringBuilder sb = new StringBuilder(); 186 | for (int i = start_line; i < lines.Length; ++i) { 187 | sb.Append(lines[i].Substring(0, lines[i].Length - 1)); 188 | } 189 | return SCIP_Reader.decode_array(sb.ToString(), 3, ref distances, ref strengths); 190 | } 191 | public static bool decode_array(string data, int size, ref List decoded_data, ref List stdecoded_data) 192 | { 193 | for (int pos = 0; pos <= data.Length - size * 2; pos += size * 2) { 194 | decoded_data.Add(decode(data, size, pos)); 195 | stdecoded_data.Add(decode(data, size, pos + size)); 196 | } 197 | return true; 198 | } 199 | } 200 | } 201 | -------------------------------------------------------------------------------- /Assets/URG/SCIP_library.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8d50cfc603be74e58b8f3fc83b3017d4 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /Assets/URG/UrgDevice.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using System.Collections; 3 | 4 | public class UrgDevice : MonoBehaviour { 5 | 6 | public enum CMD 7 | { 8 | // https://www.hokuyo-aut.jp/02sensor/07scanner/download/pdf/URG_SCIP20.pdf 9 | VV, PP, II, // センサ情報要求コマンド(3 種類) 10 | BM, QT, //計測開始・終了コマンド 11 | MD, GD, // 距離要求コマンド(2 種類) 12 | ME //距離・受光強度要求コマンド 13 | } 14 | 15 | public static string GetCMDString(CMD cmd) 16 | { 17 | return cmd.ToString(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /Assets/URG/UrgDevice.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a7f7117fe2cd2405fa546d3ec7b7bf4f 3 | timeCreated: 1429851445 4 | licenseType: Pro 5 | MonoImporter: 6 | serializedVersion: 2 7 | defaultReferences: [] 8 | executionOrder: 0 9 | icon: {instanceID: 0} 10 | userData: 11 | assetBundleName: 12 | assetBundleVariant: 13 | -------------------------------------------------------------------------------- /Assets/URG/UrgDeviceEthernet.cs: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file 3 | * \brief Get distance data from Ethernet type URG 4 | * \author Jun Fujimoto 5 | * $Id: get_distance_ethernet.cs 403 2013-07-11 05:24:12Z fujimoto $ 6 | */ 7 | using UnityEngine; 8 | using System.Threading; 9 | 10 | using System; 11 | using System.Collections; 12 | using System.Collections.Generic; 13 | using System.Text; 14 | using System.Net.Sockets; 15 | using SCIP_library; 16 | 17 | public class UrgDeviceEthernet : UrgDevice 18 | { 19 | // private Thread listenThread; 20 | private Thread clientThread; 21 | TcpClient tcpClient; 22 | 23 | public List distances; 24 | public List strengths; 25 | 26 | // private Queue messageQueue; 27 | 28 | private string ip_address = "192.168.0.10"; 29 | private int port_number = 10940; 30 | 31 | public void StartTCP(string ip = "192.168.0.10", int port = 10940) 32 | { 33 | // messageQueue = Queue.Synchronized(new Queue()); 34 | 35 | ip_address = ip; 36 | port_number = port; 37 | 38 | distances = new List(); 39 | strengths = new List(); 40 | 41 | try { 42 | tcpClient = new TcpClient(); 43 | tcpClient.Connect(ip_address, port_number); 44 | 45 | Debug.Log("Connect setting = IP Address : " + ip_address + " Port number : " + port_number.ToString()); 46 | 47 | // this.listenThread = new Thread(new ThreadStart(ListenForClients)); 48 | // this.listenThread.Start(); 49 | 50 | ListenForClients(); 51 | } catch (Exception ex) { 52 | Debug.Log(ex.Message); 53 | } finally { 54 | 55 | } 56 | } 57 | 58 | void OnDisable() 59 | { 60 | DeInit(); 61 | } 62 | void OnApplicationQuit() 63 | { 64 | DeInit(); 65 | } 66 | 67 | void DeInit() 68 | { 69 | if(tcpClient != null){ 70 | if( tcpClient.Connected ){ 71 | NetworkStream stream = tcpClient.GetStream(); 72 | if(stream != null){ 73 | stream.Close(); 74 | } 75 | } 76 | tcpClient.Close(); 77 | } 78 | 79 | if(this.clientThread != null){ 80 | this.clientThread.Abort(); 81 | } 82 | } 83 | 84 | public void Write(string scip) 85 | { 86 | NetworkStream stream = tcpClient.GetStream(); 87 | write(stream, scip); 88 | } 89 | 90 | private void ListenForClients() 91 | { 92 | clientThread = new Thread(new ParameterizedThreadStart(HandleClientComm)); 93 | clientThread.Start(tcpClient); 94 | } 95 | private void HandleClientComm(object obj) 96 | { 97 | try 98 | { 99 | using (TcpClient client = (TcpClient)obj) 100 | { 101 | using (NetworkStream stream = client.GetStream()) 102 | { 103 | // NetworkStream clientStream = client.GetStream(); 104 | while (true) 105 | { 106 | long time_stamp = 0; 107 | string receive_data = read_line(stream); 108 | // messageQueue.Enqueue( receive_data ); 109 | 110 | string cmd = GetCommand(receive_data); 111 | if(cmd == GetCMDString(CMD.MD)){ 112 | distances.Clear(); 113 | SCIP_Reader.MD(receive_data, ref time_stamp, ref distances); 114 | }else if(cmd == GetCMDString(CMD.ME)){ 115 | distances.Clear(); 116 | strengths.Clear(); 117 | SCIP_Reader.ME(receive_data, ref time_stamp, ref distances, ref strengths); 118 | }else{ 119 | Debug.Log(">>"+receive_data); 120 | } 121 | } 122 | // client.Close(); 123 | } 124 | } 125 | } catch (System.Exception ex) { 126 | Debug.LogWarning("error: "+ex); 127 | } 128 | } 129 | 130 | string GetCommand(string get_command) 131 | { 132 | string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries); 133 | return split_command[0].Substring(0, 2); 134 | } 135 | 136 | bool CheckCommand(string get_command, string cmd) 137 | { 138 | string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries); 139 | return split_command[0].StartsWith(cmd); 140 | } 141 | 142 | // void Update() 143 | // { 144 | // lock(messageQueue.SyncRoot){ 145 | // if(messageQueue.Count > 0){ 146 | // string receive_data = messageQueue.Dequeue().ToString(); 147 | // long time_stamp; 148 | // if(CheckCommand(receive_data, "MD")){ 149 | // distances.Clear(); 150 | // time_stamp = 0; 151 | // 152 | // SCIP_Reader.MD(receive_data, ref time_stamp, ref distances); 153 | // //Debug.Log("time stamp: " + time_stamp.ToString() + " / count: "+distances.Count); 154 | // }else if(CheckCommand(receive_data, "GD")){ 155 | // distances.Clear(); 156 | // time_stamp = 0; 157 | // 158 | // SCIP_Reader.GD(receive_data, ref time_stamp, ref distances); 159 | // }else{ 160 | // Debug.Log(">>"+receive_data); 161 | // } 162 | // } 163 | // } 164 | // 165 | // } 166 | 167 | 168 | /// 169 | /// Read to "\n\n" from NetworkStream 170 | /// 171 | /// receive data 172 | static string read_line(NetworkStream stream) 173 | { 174 | if (stream.CanRead) { 175 | StringBuilder sb = new StringBuilder(); 176 | bool is_NL2 = false; 177 | bool is_NL = false; 178 | do { 179 | char buf = (char)stream.ReadByte(); 180 | if (buf == '\n') { 181 | if (is_NL) { 182 | is_NL2 = true; 183 | } else { 184 | is_NL = true; 185 | } 186 | } else { 187 | is_NL = false; 188 | } 189 | sb.Append(buf); 190 | } while (!is_NL2); 191 | 192 | return sb.ToString(); 193 | } else { 194 | return null; 195 | } 196 | } 197 | 198 | /// 199 | /// write data 200 | /// 201 | static bool write(NetworkStream stream, string data) 202 | { 203 | if (stream.CanWrite) { 204 | byte[] buffer = Encoding.ASCII.GetBytes(data); 205 | stream.Write(buffer, 0, buffer.Length); 206 | return true; 207 | } else { 208 | return false; 209 | } 210 | } 211 | } -------------------------------------------------------------------------------- /Assets/URG/UrgDeviceEthernet.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f02061e0ddc7b4a46bc5b538311fe064 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /Assets/URGSample.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | 5 | // http://sourceforge.net/p/urgnetwork/wiki/top_jp/ 6 | // https://www.hokuyo-aut.co.jp/02sensor/07scanner/download/pdf/URG_SCIP20.pdf 7 | public class URGSample : MonoBehaviour { 8 | 9 | class DetectObject 10 | { 11 | public List distList; 12 | public List idList; 13 | 14 | public long startDist; 15 | 16 | public DetectObject() 17 | { 18 | distList = new List(); 19 | idList = new List(); 20 | } 21 | } 22 | 23 | [SerializeField] 24 | string ip_address = "192.168.0.10"; 25 | [SerializeField] 26 | int port_number = 10940; 27 | 28 | List detectObjects; 29 | List detectIdList; 30 | 31 | private Vector3[] directions; 32 | private bool cached = false; 33 | 34 | UrgDeviceEthernet urg; 35 | public float scale = 0.1f; 36 | public float limit = 300.0f;//mm 37 | public int noiseLimit = 5; 38 | 39 | public Color distanceColor = Color.white; 40 | // public Color detectColor = Color.white; 41 | public Color strengthColor = Color.white; 42 | 43 | public Color[] groupColors; 44 | 45 | List distances; 46 | List strengths; 47 | 48 | public Rect areaRect; 49 | 50 | public bool debugDraw = false; 51 | 52 | int drawCount; 53 | 54 | 55 | // Use this for initialization 56 | void Start () { 57 | distances = new List(); 58 | strengths = new List(); 59 | 60 | urg = this.gameObject.AddComponent(); 61 | urg.StartTCP(ip_address, port_number); 62 | } 63 | 64 | // Update is called once per frame 65 | void Update () { 66 | 67 | if(gd_loop){ 68 | urg.Write(SCIP_library.SCIP_Writer.GD(0, 1080)); 69 | } 70 | 71 | // center offset rect 72 | Rect detectAreaRect = areaRect; 73 | detectAreaRect.x *= scale; 74 | detectAreaRect.y *= scale; 75 | detectAreaRect.width *= scale; 76 | detectAreaRect.height *= scale; 77 | 78 | detectAreaRect.x = -detectAreaRect.width / 2; 79 | // 80 | 81 | float d = Mathf.PI * 2 / 1440; 82 | float offset = d * 540; 83 | 84 | // cache directions 85 | if(urg.distances.Count > 0 ){ 86 | if(!cached){ 87 | directions = new Vector3[urg.distances.Count]; 88 | for(int i = 0; i < directions.Length; i++){ 89 | float a = d * i + offset; 90 | directions[i] = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 91 | } 92 | cached = true; 93 | } 94 | } 95 | 96 | // strengths 97 | try{ 98 | if(urg.strengths.Count > 0){ 99 | strengths.Clear(); 100 | strengths.AddRange(urg.strengths); 101 | } 102 | }catch{ 103 | } 104 | // distances 105 | try{ 106 | if(urg.distances.Count > 0){ 107 | distances.Clear(); 108 | distances.AddRange(urg.distances); 109 | } 110 | }catch{ 111 | } 112 | // List distances = urg.distances; 113 | 114 | if(debugDraw){ 115 | // strengths 116 | for(int i = 0; i < strengths.Count; i++){ 117 | //float a = d * i + offset; 118 | //Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 119 | Vector3 dir = directions[i]; 120 | long dist = strengths[i]; 121 | Debug.DrawRay(Vector3.zero, Mathf.Abs( dist ) * dir * scale, strengthColor); 122 | } 123 | 124 | // distances 125 | // float colorD = 1.0f / 1440; 126 | for(int i = 0; i < distances.Count; i++){ 127 | //float a = d * i + offset; 128 | //Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 129 | Vector3 dir = directions[i]; 130 | long dist = distances[i]; 131 | //color = (dist < limit && dir.y > 0) ? detectColor : new Color(colorD * i, 0,0,1.0f); 132 | //Color color = (dist < limit && dir.y > 0) ? detectColor : distanceColor; 133 | // Debug.DrawRay(Vector3.zero, dist * dir * scale, color); 134 | Debug.DrawRay(Vector3.zero, dist * dir * scale, distanceColor); 135 | } 136 | } 137 | 138 | //----------------- 139 | // group 140 | detectObjects = new List(); 141 | // 142 | //------ 143 | // bool endGroup = true; 144 | // for(int i = 0; i < distances.Count; i++){ 145 | // int id = i; 146 | // long dist = distances[id]; 147 | // 148 | // float a = d * i + offset; 149 | // Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 150 | // 151 | // if(dist < limit && dir.y > 0){ 152 | // DetectObject detect; 153 | // if(endGroup){ 154 | // detect = new DetectObject(); 155 | // detect.idList.Add(id); 156 | // detect.distList.Add(dist); 157 | // 158 | // detect.startDist = dist; 159 | // detectObjects.Add(detect); 160 | // 161 | // endGroup = false; 162 | // }else{ 163 | // detect = detectObjects[detectObjects.Count-1]; 164 | // detect.idList.Add(id); 165 | // detect.distList.Add(dist); 166 | // 167 | // if(dist > detect.startDist){ 168 | // endGroup = true; 169 | // } 170 | // } 171 | // }else{ 172 | // endGroup = true; 173 | // } 174 | // } 175 | 176 | //------ 177 | // bool endGroup = true; 178 | // for(int i = 1; i < distances.Count-1; i++){ 179 | // long dist = distances[i]; 180 | // float delta = Mathf.Abs((float)(distances[i] - distances[i-1])); 181 | // float delta1 = Mathf.Abs((float)(distances[i+1] - distances[i])); 182 | // 183 | // float a = d * i + offset; 184 | // Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 185 | // 186 | // if(dir.y > 0){ 187 | // DetectObject detect; 188 | // if(endGroup){ 189 | // if(dist < limit && delta > 50){ 190 | // detect = new DetectObject(); 191 | // detect.idList.Add(i); 192 | // detect.distList.Add(dist); 193 | // 194 | // detect.startDist = dist; 195 | // detectObjects.Add(detect); 196 | // 197 | // endGroup = false; 198 | // } 199 | // }else{ 200 | // if(delta < 50){ 201 | // detect = detectObjects[detectObjects.Count-1]; 202 | // detect.idList.Add(i); 203 | // detect.distList.Add(dist); 204 | // }else{ 205 | // endGroup = true; 206 | // } 207 | // } 208 | // } 209 | // } 210 | 211 | 212 | //------ 213 | bool endGroup = true; 214 | float deltaLimit = 100; // 認識の閾値　連続したもののみを取得するため (mm) 215 | for(int i = 1; i < distances.Count-1; i++){ 216 | //float a = d * i + offset; 217 | //Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 218 | Vector3 dir = directions[i]; 219 | long dist = distances[i]; 220 | float delta = Mathf.Abs((float)(distances[i] - distances[i-1])); 221 | float delta1 = Mathf.Abs((float)(distances[i+1] - distances[i])); 222 | 223 | if(dir.y > 0){ 224 | DetectObject detect; 225 | if(endGroup){ 226 | Vector3 pt = dist * dir * scale; 227 | if(dist < limit && (delta < deltaLimit && delta1 < deltaLimit)){ 228 | // bool isArea = detectAreaRect.Contains(pt); 229 | // if(isArea && (delta < deltaLimit && delta1 < deltaLimit)){ 230 | detect = new DetectObject(); 231 | detect.idList.Add(i); 232 | detect.distList.Add(dist); 233 | 234 | detect.startDist = dist; 235 | detectObjects.Add(detect); 236 | 237 | endGroup = false; 238 | } 239 | }else{ 240 | if(delta1 >= deltaLimit || delta >= deltaLimit){ 241 | endGroup = true; 242 | }else{ 243 | detect = detectObjects[detectObjects.Count-1]; 244 | detect.idList.Add(i); 245 | detect.distList.Add(dist); 246 | } 247 | } 248 | } 249 | } 250 | 251 | //----------------- 252 | // draw 253 | drawCount = 0; 254 | for(int i = 0; i < detectObjects.Count; i++){ 255 | DetectObject detect = detectObjects[i]; 256 | 257 | // noise 258 | if(detect.idList.Count < noiseLimit){ 259 | continue; 260 | } 261 | 262 | int offsetCount = detect.idList.Count / 3; 263 | int avgId = 0; 264 | for(int n = 0; n < detect.idList.Count; n++){ 265 | avgId += detect.idList[n]; 266 | } 267 | avgId = avgId / (detect.idList.Count); 268 | 269 | long avgDist = 0; 270 | for(int n = offsetCount; n < detect.distList.Count - offsetCount; n++){ 271 | avgDist += detect.distList[n]; 272 | } 273 | avgDist = avgDist / (detect.distList.Count - offsetCount * 2); 274 | 275 | //float a = d * avgId + offset; 276 | //Vector3 dir = new Vector3(-Mathf.Cos(a), -Mathf.Sin(a), 0); 277 | Vector3 dir = directions[avgId]; 278 | long dist = avgDist; 279 | 280 | 281 | //float a0 = d * detect.idList[offsetCount] + offset; 282 | //Vector3 dir0 = new Vector3(-Mathf.Cos(a0), -Mathf.Sin(a0), 0); 283 | int id0 = detect.idList[offsetCount]; 284 | Vector3 dir0 = directions[id0]; 285 | long dist0 = detect.distList[offsetCount]; 286 | 287 | //float a1 = d * detect.idList[detect.idList.Count-1 - offsetCount] + offset; 288 | //Vector3 dir1 = new Vector3(-Mathf.Cos(a1), -Mathf.Sin(a1), 0); 289 | int id1 = detect.idList[detect.idList.Count-1 - offsetCount]; 290 | Vector3 dir1 = directions[id1]; 291 | long dist1 = detect.distList[detect.distList.Count-1 - offsetCount]; 292 | 293 | Color gColor; 294 | if(drawCount < groupColors.Length){ 295 | gColor = groupColors[drawCount]; 296 | }else{ 297 | gColor = Color.green; 298 | } 299 | for(int j = offsetCount; j < detect.idList.Count - offsetCount; j++){ 300 | //float _a = d * detect.idList[j] + offset; 301 | //Vector3 _dir = new Vector3(-Mathf.Cos(_a), -Mathf.Sin(_a), 0); 302 | int _id = detect.idList[j]; 303 | Vector3 _dir = directions[_id]; 304 | long _dist = detect.distList[j]; 305 | Debug.DrawRay(Vector3.zero, _dist * _dir * scale, gColor); 306 | } 307 | 308 | Debug.DrawLine(dist0 * dir0 * scale, dist1 * dir1 * scale, gColor); 309 | Debug.DrawRay(Vector3.zero, dist * dir * scale, Color.green); 310 | 311 | drawCount++; 312 | } 313 | 314 | DrawRect(detectAreaRect, Color.green); 315 | } 316 | void DrawRect(Rect rect, Color color) 317 | { 318 | Vector3 p0 = new Vector3(rect.x, rect.y, 0); 319 | Vector3 p1 = new Vector3(rect.x + rect.width, rect.y, 0); 320 | Vector3 p2 = new Vector3(rect.x + rect.width, rect.y + rect.height, 0); 321 | Vector3 p3 = new Vector3(rect.x, rect.y + rect.height, 0); 322 | Debug.DrawLine(p0, p1, color); 323 | Debug.DrawLine(p1, p2, color); 324 | Debug.DrawLine(p2, p3, color); 325 | Debug.DrawLine(p3, p0, color); 326 | } 327 | 328 | private bool gd_loop = false; 329 | 330 | // PP 331 | // MODL ... センサ型式情報 332 | // DMIN ... 最小計測可能距離 (mm) 333 | // DMAX ... 最大計測可能距離 (mm) 334 | // ARES ... 角度分解能(360度の分割数) 335 | // AMIN ... 最小計測可能方向値 336 | // AMAX ... 最大計測可能方向値 337 | // AFRT ... 正面方向値 338 | // SCAN ... 標準操作角速度 339 | 340 | void OnGUI() 341 | { 342 | // https://sourceforge.net/p/urgnetwork/wiki/scip_jp/ 343 | if(GUILayout.Button("VV: (バージョン情報の取得)")){ 344 | urg.Write(SCIP_library.SCIP_Writer.VV()); 345 | } 346 | // if(GUILayout.Button("SCIP2")){ 347 | // urg.Write(SCIP_library.SCIP_Writer.SCIP2()); 348 | // } 349 | if(GUILayout.Button("PP: (パラメータ情報の取得)")){ 350 | urg.Write(SCIP_library.SCIP_Writer.PP()); 351 | } 352 | if(GUILayout.Button("MD: (計測＆送信要求)")){ 353 | urg.Write(SCIP_library.SCIP_Writer.MD(0, 1080, 1, 0, 0)); 354 | } 355 | if(GUILayout.Button("ME: (計測＆距離データ・受光強度値送信要求)")){ 356 | urg.Write(SCIP_library.SCIP_Writer.ME(0, 1080, 1, 1, 0)); 357 | } 358 | if(GUILayout.Button("BM: (レーザの発光)")){ 359 | urg.Write(SCIP_library.SCIP_Writer.BM()); 360 | } 361 | if(GUILayout.Button("GD: (計測済み距離データ送信要求)")){ 362 | urg.Write(SCIP_library.SCIP_Writer.GD(0, 1080)); 363 | } 364 | if(GUILayout.Button("GD_loop")){ 365 | gd_loop = !gd_loop; 366 | } 367 | if(GUILayout.Button("QUIT")){ 368 | urg.Write(SCIP_library.SCIP_Writer.QT()); 369 | } 370 | 371 | GUILayout.Label("distances.Count: "+distances.Count + " / strengths.Count: "+strengths.Count); 372 | GUILayout.Label("drawCount: "+drawCount + " / detectObjects: "+detectObjects.Count); 373 | } 374 | 375 | 376 | } 377 | -------------------------------------------------------------------------------- /Assets/URGSample.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 307b3ffa9488c41619dec48bc346675c 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /Assets/sample.unity: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/Assets/sample.unity -------------------------------------------------------------------------------- /Assets/sample.unity.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 6dc614cfba0d649f6bc685b5c0b91ecb 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ProjectSettings/AudioManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/AudioManager.asset -------------------------------------------------------------------------------- /ProjectSettings/ClusterInputManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/ClusterInputManager.asset -------------------------------------------------------------------------------- /ProjectSettings/DynamicsManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/DynamicsManager.asset -------------------------------------------------------------------------------- /ProjectSettings/EditorBuildSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/EditorBuildSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/EditorSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/EditorSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/GraphicsSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/GraphicsSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/InputManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/InputManager.asset -------------------------------------------------------------------------------- /ProjectSettings/NavMeshAreas.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/NavMeshAreas.asset -------------------------------------------------------------------------------- /ProjectSettings/NavMeshLayers.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/NavMeshLayers.asset -------------------------------------------------------------------------------- /ProjectSettings/NetworkManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/NetworkManager.asset -------------------------------------------------------------------------------- /ProjectSettings/Physics2DSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/Physics2DSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/ProjectSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/ProjectSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/ProjectVersion.txt: -------------------------------------------------------------------------------- 1 | m_EditorVersion: 5.3.2p3 2 | m_StandardAssetsVersion: 0 3 | -------------------------------------------------------------------------------- /ProjectSettings/QualitySettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/QualitySettings.asset -------------------------------------------------------------------------------- /ProjectSettings/TagManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/TagManager.asset -------------------------------------------------------------------------------- /ProjectSettings/TimeManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/TimeManager.asset -------------------------------------------------------------------------------- /ProjectSettings/UnityAdsSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/UnityAdsSettings.asset -------------------------------------------------------------------------------- /ProjectSettings/UnityConnectSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/inoook/UnityURG/d8d4811905e6a9b7a4fe9cef457050c4fc27a8f1/ProjectSettings/UnityConnectSettings.asset -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UnityURG 2 | URG for unity3d 3 | 4 | http://sourceforge.net/p/urgnetwork/wiki/top_jp/ 5 | 6 | https://www.hokuyo-aut.jp/02sensor/07scanner/download/pdf/URG_SCIP20.pdf 7 | 8 | 9 | その他 10 | 11 | https://github.com/yusuketomoto/ofxUrgDevice 12 | 13 | https://github.com/kendemu/urg_node 14 | --------------------------------------------------------------------------------