Merge pull request #40 from Field-Robotics-Japan/develop

ROS2への対応・TFセンサの追加・VelodyneLiDARへのノイズ追加

Author: Autumn60

.vscode/settings.json

@@ -0,0 +1,55 @@
+{
+    "files.exclude":
+    {
+        "**/.DS_Store":true,
+        "**/.git":true,
+        "**/.gitmodules":true,
+        "**/*.booproj":true,
+        "**/*.pidb":true,
+        "**/*.suo":true,
+        "**/*.user":true,
+        "**/*.userprefs":true,
+        "**/*.unityproj":true,
+        "**/*.dll":true,
+        "**/*.exe":true,
+        "**/*.pdf":true,
+        "**/*.mid":true,
+        "**/*.midi":true,
+        "**/*.wav":true,
+        "**/*.gif":true,
+        "**/*.ico":true,
+        "**/*.jpg":true,
+        "**/*.jpeg":true,
+        "**/*.png":true,
+        "**/*.psd":true,
+        "**/*.tga":true,
+        "**/*.tif":true,
+        "**/*.tiff":true,
+        "**/*.3ds":true,
+        "**/*.3DS":true,
+        "**/*.fbx":true,
+        "**/*.FBX":true,
+        "**/*.lxo":true,
+        "**/*.LXO":true,
+        "**/*.ma":true,
+        "**/*.MA":true,
+        "**/*.obj":true,
+        "**/*.OBJ":true,
+        "**/*.asset":true,
+        "**/*.cubemap":true,
+        "**/*.flare":true,
+        "**/*.mat":true,
+        "**/*.meta":true,
+        "**/*.prefab":true,
+        "**/*.unity":true,
+        "build/":true,
+        "Build/":true,
+        "Library/":true,
+        "library/":true,
+        "obj/":true,
+        "Obj/":true,
+        "ProjectSettings/":true,
+        "temp/":true,
+        "Temp/":true
+    }
+}

Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs

@@ -17,6 +17,12 @@ public class VelodyneSensor : Sensor
         [SerializeField]
         private RotatingLiDARScanPattern _scanPattern;
         [SerializeField]
+        private float _minDistance = 0.0f;
+        [SerializeField]
+        private float _maxDistance = 100.0f;
+        [SerializeField]
+        private float _maxIntensity = 255.0f;
+        [SerializeField]
         private float _gaussianNoiseSigma = 0.0f;
 
         private Transform _transform;
@@ -30,11 +36,16 @@ public class VelodyneSensor : Sensor
         private NativeArray<RaycastHit> _raycastHits;
         private Random _random;
         private NativeArray<float> _noises;
+        
+        public NativeArray<float> distances;
         public NativeArray<Vector3> points;
+        public NativeArray<float> intensities;
 
         private uint _randomSeed;
         private int _pointsNum;
         public uint pointsNum { get => (uint)_pointsNum; }
+        public int layersNum { get => _scanPattern.numOfLayer; }
+        public int azimuthResolution { get => _scanPattern.azimuthResolution; }
 
         protected override void Init()
         {
@@ -56,7 +67,9 @@ private void SetupDirections()
 
         private void SetupJobs()
         {
+            distances = new NativeArray<float>(_pointsNum, Allocator.Persistent);
             points = new NativeArray<Vector3>(_pointsNum, Allocator.Persistent);
+            intensities = new NativeArray<float>(_pointsNum, Allocator.Persistent);
             _raycastCommands = new NativeArray<RaycastCommand>(_pointsNum, Allocator.Persistent);
             _raycastHits = new NativeArray<RaycastHit>(_pointsNum, Allocator.Persistent);
 
@@ -82,10 +95,16 @@ private void SetupJobs()
 
             _raycastHitsToPointsJob = new RaycastHitsToPointsJob()
             {
+                minDistance = _minDistance,
+                minDistance_sqr = _minDistance * _minDistance,
+                maxDistance = _maxDistance,
+                maxIntensity = _maxIntensity,
                 directions = _directions,
                 raycastHits = _raycastHits,
                 noises = _noises,
-                points = points
+                distances = distances,
+                points = points,
+                intensities = intensities
             };
         }
 
@@ -119,7 +138,9 @@ private void OnDestroy()
             _directions.Dispose();
             _raycastCommands.Dispose();
             _raycastHits.Dispose();
+            distances.Dispose();
             points.Dispose();
+            intensities.Dispose();
         }
 
         [BurstCompile]
@@ -161,18 +182,34 @@ public void Execute(int index)
         [BurstCompile]
         private struct RaycastHitsToPointsJob : IJobParallelFor
         {
+            [ReadOnly]
+            public float minDistance;
+            [ReadOnly]
+            public float minDistance_sqr;
+            [ReadOnly]
+            public float maxDistance;
+            [ReadOnly]
+            public float maxIntensity;
             [ReadOnly]
             public NativeArray<Vector3> directions;
             [ReadOnly]
             public NativeArray<RaycastHit> raycastHits;
             [ReadOnly]
             public NativeArray<float> noises;
 
+            public NativeArray<float> distances;
             public NativeArray<Vector3> points;
+            public NativeArray<float> intensities;
 
             public void Execute(int index)
             {
-                points[index] = directions[index] * (raycastHits[index].distance + noises[index]);
+                float distance = raycastHits[index].distance + noises[index];
+                bool isValid = (minDistance <= distance && distance <= maxDistance);
+                if (!isValid) distance = 0;
+                distances[index] = distance;
+                points[index] = directions[index] * distance;
+                float distance_sqr = distance * distance;
+                intensities[index] = isValid ? maxIntensity * minDistance_sqr / distance_sqr : 0;
             }
         }
     }

Assets/UnitySensors/Runtime/Scripts/Sensors/TF.meta

@@ -0,0 +1,14 @@
+using UnityEngine;
+using UnitySensors;
+
+public class TFSensor : Sensor {
+
+    [ReadOnly]
+    private Vector3 _position;
+
+    [ReadOnly]
+    private Quaternion _rotation;
+
+    public Vector3 position { get => _position; }
+    public Quaternion rotation { get => _rotation; }
+}

Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs

@@ -19,7 +19,7 @@ public override void GenerateScanPattern()
             _loadedFile = "";
             _maxAzimuth = _maxZenith = 0;
 
-            if(_file == null)
+            if (_file == null)
             {
                 Debug.LogWarning(this.name + ": CSV file is not set.");
                 return;
@@ -70,8 +70,10 @@ public override void GenerateScanPattern()
             _size = lines.Length - 2;
 
             _generated = true;
+#if UNITY_EDITOR
             EditorUtility.SetDirty(this);
             AssetDatabase.SaveAssets();
+#endif
         }
     }
 }

Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs.meta

@@ -24,6 +24,7 @@ private enum RotationDirection
         private int _azimuthResolution = 360;
 
         public int numOfLayer { get => _zenithAngles.Length; }
+        public int azimuthResolution { get => _azimuthResolution; }
 
         public override void GenerateScanPattern()
         {
@@ -35,11 +36,11 @@ public override void GenerateScanPattern()
             _scans = new Vector3[_size];
 
             int index = 0;
-            for(int azimuth = 0; azimuth < _azimuthResolution; azimuth++)
+            for (int azimuth = 0; azimuth < _azimuthResolution; azimuth++)
             {
                 float azimuthAngle = 360.0f / _azimuthResolution * azimuth;
                 if (_rotationDirection == RotationDirection.CCW) azimuthAngle *= -1;
-                foreach(float zenithAngle in _zenithAngles)
+                foreach (float zenithAngle in _zenithAngles)
                 {
                     _scans[index] = Quaternion.Euler(-zenithAngle, azimuthAngle, 0) * Vector3.forward;
                     index++;
@@ -54,8 +55,10 @@ public override void GenerateScanPattern()
 
             _generated = true;
 
+#if UNITY_EDITOR
             EditorUtility.SetDirty(this);
             AssetDatabase.SaveAssets();
+#endif
         }
     }
 }