fix bugs and add new pcd format support

autocore-ai · Jul 22, 2019 · 95c1c9e · 95c1c9e
1 parent 014173a
commit 95c1c9e
Show file tree

Hide file tree

Showing 49 changed files with 536 additions and 102 deletions.
diff --git a/PcdTools/Editor/EditorMenuExterns.cs b/PcdTools/Editor/EditorMenuExterns.cs
@@ -20,7 +20,7 @@
 using UnityEditor;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.PcdTools.Editor
+namespace Packages.MapToolbox.PcdTools.Editor
 {
     class EditorMenuExterns
     {

diff --git a/PcdTools/Editor/PcdImporter.cs b/PcdTools/Editor/PcdImporter.cs
@@ -21,7 +21,7 @@
 using UnityEditorInternal;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.PcdTools.Editor
+namespace Packages.MapToolbox.PcdTools.Editor
 {
     [ScriptedImporter(1, "pcd")]
     class PcdImporter : ScriptedImporter

diff --git a/PcdTools/HeightMeshBuild.cs b/PcdTools/HeightMeshBuild.cs
@@ -24,7 +24,7 @@
 using UnityEngine;
 using UnityEngine.Rendering;
 
-namespace Packages.AutowareUnityTools.PcdTools
+namespace Packages.MapToolbox.PcdTools
 {
     public static class HeightMeshBuild
     {

diff --git a/PcdTools/PcdReader.cs b/PcdTools/PcdReader.cs
@@ -22,14 +22,15 @@
 using Unity.Mathematics;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.PcdTools
+namespace Packages.MapToolbox.PcdTools
 {
     public class PcdReader : StreamReader
     {
         public string FilePath { get; private set; }
         public NativeArray<float3byte4> PcdData_xyz_rgb { get; set; }
         public NativeArray<float4> PcdData_xyz_intensity { get; set; }
         public NativeArray<float3> PcdData_xyz { get; private set; }
+        public NativeArray<xyz_intensity_ring> PcdData_xyz_intensity_ring { get; set; }
         public PcdReader(string filePath) : base(filePath)
         {
             FilePath = filePath;
@@ -87,14 +88,24 @@ public PcdReader(string filePath) : base(filePath)
                                 }
                             }
                             break;
+                        case "x y z _ intensity ring _":
+                            PcdData_xyz_intensity_ring = new NativeArray<xyz_intensity_ring>(pointsCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory);
+                            unsafe
+                            {
+                                fixed (byte* data = new BinaryReader(BaseStream).ReadBytes(pointsCount * sizeof(xyz_intensity_ring)))
+                                {
+                                    UnsafeUtility.MemCpy(PcdData_xyz_intensity_ring.GetUnsafePtr(), data, pointsCount * sizeof(xyz_intensity_ring));
+                                }
+                            }
+                            break;
                         default:
                             Debug.LogError($"Unknow field type {fields}");
                             break;
                     }
                 }
                 else
                 {
-                    Debug.LogError("Binary format pcd surported only currently !");
+                    Debug.LogError("Binary format pcd supported only currently !");
                 }
             }
             else
@@ -110,6 +121,8 @@ protected override void Dispose(bool disposing)
                 PcdData_xyz_rgb.Dispose();
             if (PcdData_xyz_intensity.IsCreated)
                 PcdData_xyz_intensity.Dispose();
+            if (PcdData_xyz_intensity_ring.IsCreated)
+                PcdData_xyz_intensity_ring.Dispose();
             base.Dispose(disposing);
         }
     }

diff --git a/PcdTools/PcdWriter.cs b/PcdTools/PcdWriter.cs
@@ -21,7 +21,7 @@
 using Unity.Collections.LowLevel.Unsafe;
 using Unity.Mathematics;
 
-namespace Packages.AutowareUnityTools.PcdTools
+namespace Packages.MapToolbox.PcdTools
 {
     public class PcdWriter : StreamWriter
     {

diff --git a/PcdTools/PointsMeshBuild.cs b/PcdTools/PointsMeshBuild.cs
@@ -24,10 +24,11 @@
 using UnityEngine;
 using UnityEngine.Rendering;
 
-namespace Packages.AutowareUnityTools.PcdTools
+namespace Packages.MapToolbox.PcdTools
 {
     public static class PointsMeshBuild
     {
+        const float max_intensity = 100;
         public static Mesh PointsMesh(this PcdReader pcdReader)
         {
             if (pcdReader.PcdData_xyz.IsCreated)
@@ -80,6 +81,24 @@ public static Mesh PointsMesh(this PcdReader pcdReader)
                     }
                 }
             }
+            else if (pcdReader.PcdData_xyz_intensity_ring.IsCreated)
+            {
+                using (var vertices = new NativeArray<Vector3>(pcdReader.PcdData_xyz_intensity_ring.Length, Allocator.TempJob, NativeArrayOptions.UninitializedMemory))
+                {
+                    using (var colors = new NativeArray<Color32>(pcdReader.PcdData_xyz_intensity_ring.Length, Allocator.TempJob, NativeArrayOptions.UninitializedMemory))
+                    {
+                        new Ros2Unity_xyz_intensity_ring() { pcd = pcdReader.PcdData_xyz_intensity_ring, vertices = vertices, colors = colors }.Schedule(vertices.Length, 128).Complete();
+                        Mesh mesh = new Mesh()
+                        {
+                            indexFormat = vertices.Length > 65535 ? IndexFormat.UInt32 : IndexFormat.UInt16,
+                            vertices = vertices.ToArray(),
+                            colors32 = colors.ToArray()
+                        };
+                        mesh.SetIndices(Enumerable.Range(0, mesh.vertexCount).ToArray(), MeshTopology.Points, 0);
+                        return mesh;
+                    }
+                }
+            }
             else
             {
                 return new Mesh();
@@ -113,7 +132,19 @@ struct Ros2Unity_xyz_intensity : IJobParallelFor
             public void Execute(int index)
             {
                 vertices[index] = new Vector3(-pcd[index].y, pcd[index].z, pcd[index].x);
-                colors[index] = Color32.Lerp(Color.red, Color.green, pcd[index].w / 100);
+                colors[index] = Color32.Lerp(Color.red, Color.green, pcd[index].w / max_intensity);
+            }
+        }
+        [BurstCompile]
+        struct Ros2Unity_xyz_intensity_ring : IJobParallelFor
+        {
+            [ReadOnly] internal NativeArray<xyz_intensity_ring> pcd;
+            [WriteOnly] internal NativeArray<Vector3> vertices;
+            [WriteOnly] internal NativeArray<Color32> colors;
+            public void Execute(int index)
+            {
+                vertices[index] = new Vector3(-pcd[index].point.y, pcd[index].point.z, pcd[index].point.x);
+                colors[index] = Color32.Lerp(Color.red, Color.green, pcd[index].intensity / max_intensity);
             }
         }
     }

diff --git a/PcdTools/float3byte4.cs b/PcdTools/float3byte4.cs
@@ -18,7 +18,7 @@
 
 using Unity.Mathematics;
 
-namespace Packages.AutowareUnityTools.PcdTools
+namespace Packages.MapToolbox.PcdTools
 {
     public struct float3byte4
     {

diff --git a/PcdTools/xyz_intensity_ring.cs b/PcdTools/xyz_intensity_ring.cs
@@ -0,0 +1,34 @@
+#region License
+/******************************************************************************
+* Copyright 2019 The AutoCore Authors. All Rights Reserved.
+* 
+* Licensed under the GNU Lesser General Public License, Version 3.0 (the "License"); 
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+* 
+* https://www.gnu.org/licenses/lgpl-3.0.html
+* 
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*****************************************************************************/
+#endregion
+
+using Unity.Mathematics;
+
+namespace Packages.MapToolbox.PcdTools
+{
+    /// <summary>
+    /// Pcd format https://github.com/autocore-ai/MapToolbox/issues/6
+    /// </summary>
+    public struct xyz_intensity_ring
+    {
+        public float3 point;
+        public byte _0;
+        public float intensity;
+        public ushort ring;
+        public byte _1;
+    }
+}
diff --git a/PcdTools/xyz_intensity_ring.cs.meta b/PcdTools/xyz_intensity_ring.cs.meta
diff --git a/README.md b/README.md
@@ -12,8 +12,10 @@ This a unity plugin helps user create vector maps for [Autoware](https://github.
 
 * pcd import
   * binary pcd
-    * ✔️ x y z intensity
+    * ✔️ x y z
     * ✔️ x y z color
+    * ✔️ x y z intensity
+    * ✔️ x y z intensity ring
 * vector maps export
   * Autoware
     * ✔️ lane merge and branch
@@ -43,6 +45,7 @@ This a unity plugin helps user create vector maps for [Autoware](https://github.
 "com.autocore.map-toolbox": "https://github.com/autocore-ai/MapToolbox.git",
 "com.nition.unity-octree": "https://github.com/autocore-ai/UnityOctree.git#upm",
 ```
-## user manual
+## User manual
 * please check video. docs will be provided soon.
+
 [![Watch the video](https://img.youtube.com/vi/WTRHPs8pN04/0.jpg)](https://youtu.be/WTRHPs8pN04)
diff --git a/VectorMapTools/AutoHeightBezier.cs b/VectorMapTools/AutoHeightBezier.cs
@@ -18,7 +18,7 @@
 
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools
+namespace Packages.MapToolbox.VectorMapTools
 {
     public class AutoHeightBezier : Bezier
     {

diff --git a/VectorMapTools/AutoHeightLine.cs b/VectorMapTools/AutoHeightLine.cs
@@ -18,7 +18,7 @@
 
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools
+namespace Packages.MapToolbox.VectorMapTools
 {
     public class AutoHeightLine : Line
     {

diff --git a/VectorMapTools/Bezier.cs b/VectorMapTools/Bezier.cs
@@ -18,7 +18,7 @@
 
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools
+namespace Packages.MapToolbox.VectorMapTools
 {
     public class Bezier : LRMapElement
     {
@@ -50,7 +50,7 @@ public virtual Vector3 EndPosition
                 endTangent += endPositionMove;
             }
         }
-        internal override Vector3? Pivot
+        public override Vector3? Pivot
         {
             set
             {

diff --git a/VectorMapTools/Curb.cs b/VectorMapTools/Curb.cs
@@ -19,15 +19,14 @@
 using System.Collections.Generic;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools
+namespace Packages.MapToolbox.VectorMapTools
 {
     public class Curb : DoubleConnectBezier
     {
         public static List<Curb> List { get; set; } = new List<Curb>();
         protected override void Awake()
         {
             base.Awake();
-            List.Add(this);
             LineRenderer.sharedMaterial = new Material(Shader.Find("Unlit/Color"))
             {
                 color = Color.red

diff --git a/VectorMapTools/DoubleConnectBezier.cs b/VectorMapTools/DoubleConnectBezier.cs
@@ -19,7 +19,7 @@
 using System.Linq;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools
+namespace Packages.MapToolbox.VectorMapTools
 {
     public class DoubleConnectBezier : AutoHeightBezier
     {

diff --git a/VectorMapTools/Editor/BezierEditor.cs b/VectorMapTools/Editor/BezierEditor.cs
@@ -19,7 +19,7 @@
 using UnityEditor;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(Bezier))]
     class BezierEditor : LRMapElementEditor

diff --git a/VectorMapTools/Editor/CurbEditor.cs b/VectorMapTools/Editor/CurbEditor.cs
@@ -18,7 +18,7 @@
 
 using UnityEditor;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(Curb))]
     class CurbEditor : BezierEditor { }

diff --git a/VectorMapTools/Editor/EditorMenuExterns.cs b/VectorMapTools/Editor/EditorMenuExterns.cs
@@ -16,12 +16,12 @@
 *****************************************************************************/
 #endregion
 
-using Packages.AutowareUnityTools.VectorMapTools.Export;
+using Packages.MapToolbox.VectorMapTools.Export;
 using System.IO;
 using UnityEditor;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     class EditorMenuExterns
     {
@@ -76,7 +76,7 @@ private static void ExportVectorMap()
             if (!string.IsNullOrEmpty(folder))
             {
                 PlayerPrefs.SetString("ExportVectorMap", folder);
-                VectorMapExporter.ExportMaps(folder);
+                new VectorMapCollection().WriteFiles(folder);
             }
         }
     }

diff --git a/VectorMapTools/Editor/LRMapElementEditor.cs b/VectorMapTools/Editor/LRMapElementEditor.cs
@@ -18,7 +18,7 @@
 
 using UnityEditor;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(LRMapElement))]
     class LRMapElementEditor : UnityEditor.Editor

diff --git a/VectorMapTools/Editor/LaneEditor.cs b/VectorMapTools/Editor/LaneEditor.cs
@@ -18,7 +18,7 @@
 
 using UnityEditor;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(Lane))]
     class LaneEditor : BezierEditor { }

diff --git a/VectorMapTools/Editor/LineEditor.cs b/VectorMapTools/Editor/LineEditor.cs
@@ -19,7 +19,7 @@
 using UnityEditor;
 using UnityEngine;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(Line))]
     class LineEditor : UnityEditor.Editor

diff --git a/VectorMapTools/Editor/Packages.AutowareUnityTools.VectorMapTools.Editor.asmdef b/VectorMapTools/Editor/Packages.AutowareUnityTools.VectorMapTools.Editor.asmdef
@@ -0,0 +1,17 @@
+{
+    "name": "Packages.AutowareUnityTools.VectorMapTools.Editor",
+    "references": [
+        "GUID:8eabb44705a93db47bc5b2b6feda08e3"
+    ],
+    "optionalUnityReferences": [],
+    "includePlatforms": [
+        "Editor"
+    ],
+    "excludePlatforms": [],
+    "allowUnsafeCode": false,
+    "overrideReferences": false,
+    "precompiledReferences": [],
+    "autoReferenced": true,
+    "defineConstraints": [],
+    "versionDefines": []
+}
diff --git a/VectorMapTools/Editor/Packages.AutowareUnityTools.VectorMapTools.Editor.asmdef.meta b/VectorMapTools/Editor/Packages.AutowareUnityTools.VectorMapTools.Editor.asmdef.meta
diff --git a/VectorMapTools/Editor/RoadEdgeEditor.cs b/VectorMapTools/Editor/RoadEdgeEditor.cs
@@ -18,7 +18,7 @@
 
 using UnityEditor;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(RoadEdge))]
     class RoadEdgeEditor : BezierEditor { }

diff --git a/VectorMapTools/Editor/SignalLightEditor.cs b/VectorMapTools/Editor/SignalLightEditor.cs
@@ -18,7 +18,7 @@
 
 using UnityEditor;
 
-namespace Packages.AutowareUnityTools.VectorMapTools.Editor
+namespace Packages.MapToolbox.VectorMapTools.Editor
 {
     [CustomEditor(typeof(SignalLight))]
     class SignalLightEditor : UnityEditor.Editor