From fa9c26c2131426316995b765c53ecb4797cf9f5f Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 10:13:25 +0900 Subject: [PATCH 1/9] Suppress log in Camera device --- Assets/Scripts/Devices/Camera.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs index 7bef6fb7..726ca87b 100644 --- a/Assets/Scripts/Devices/Camera.cs +++ b/Assets/Scripts/Devices/Camera.cs @@ -272,8 +272,6 @@ private IEnumerator CameraWorker() var MaxUpdateRate = (float)Application.targetFrameRate; var messageGenerationTime = 1f / (MaxUpdateRate - UpdateRate); - Debug.Log(messageGenerationTime); - while (_startCameraWork) { _universalCamData.enabled = true; From bed89395880663129df35a88f72824e8adc20637 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 10:18:12 +0900 Subject: [PATCH 2/9] Remove useless code --- Assets/Scripts/Tools/Mesh/MeshUtil.cs | 72 ---------------------- Assets/Scripts/Tools/Mesh/MeshUtil.cs.meta | 11 ---- 2 files changed, 83 deletions(-) delete mode 100644 Assets/Scripts/Tools/Mesh/MeshUtil.cs delete mode 100644 Assets/Scripts/Tools/Mesh/MeshUtil.cs.meta diff --git a/Assets/Scripts/Tools/Mesh/MeshUtil.cs b/Assets/Scripts/Tools/Mesh/MeshUtil.cs deleted file mode 100644 index 7c1c877c..00000000 --- a/Assets/Scripts/Tools/Mesh/MeshUtil.cs +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (c) 2024 LG Electronics Inc. - * - * SPDX-License-Identifier: MIT - */ - -using UnityEngine; - -public static class MeshUtil -{ - public static Mesh GetMesh(this TerrainCollider collider) - { - if (collider == null) - { - return null; - } - - Mesh mesh = new Mesh(); - var terrainData = collider.terrainData; - - var resolution = terrainData.heightmapResolution; - Debug.Log(resolution); - Vector3[] vertices = new Vector3[resolution * resolution]; - - // Populate the vertices array with the corresponding height values - for (int x = 0; x < resolution; x++) - { - for (int z = 0; z < resolution; z++) - { - float height = terrainData.GetHeight(x, z); - vertices[x + z * resolution] = new Vector3(x, height, z); - } - } - - // Define the mesh's triangles - int[] triangles = new int[(resolution - 1) * (resolution - 1) * 6]; - - // Populate the triangles array with the corresponding triangle indices - int triangleIndex = 0; - for (int x = 0; x < resolution - 1; x++) - { - for (int z = 0; z < resolution - 1; z++) - { - int v0 = x + z * resolution; - int v1 = x + (z + 1) * resolution; - int v2 = (x + 1) + z * resolution; - int v3 = (x + 1) + (z + 1) * resolution; - - triangles[triangleIndex] = v0; - triangles[triangleIndex + 1] = v1; - triangles[triangleIndex + 2] = v2; - - triangles[triangleIndex + 3] = v2; - triangles[triangleIndex + 4] = v1; - triangles[triangleIndex + 5] = v3; - - triangleIndex += 6; - } - } - - // Assign the mesh data to the Mesh object - mesh.vertices = vertices; - mesh.triangles = triangles; - - // Optional: Calculate normals and UVs - mesh.Optimize(); - // mesh.RecalculateNormals(); - // mesh.RecalculateTangents(); - - return mesh; - } -} \ No newline at end of file diff --git a/Assets/Scripts/Tools/Mesh/MeshUtil.cs.meta b/Assets/Scripts/Tools/Mesh/MeshUtil.cs.meta deleted file mode 100644 index 3c07ff2f..00000000 --- a/Assets/Scripts/Tools/Mesh/MeshUtil.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: bbaf4dba2455dc85d8fe6d0af5ab3675 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: From 699f75375c676d920e965348f7a48353f9666571 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 11:27:14 +0900 Subject: [PATCH 3/9] modify bouciness of Props PhysicsMaterials --- Assets/Resources/PhysicsMaterials/Props.physicMaterial | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Assets/Resources/PhysicsMaterials/Props.physicMaterial b/Assets/Resources/PhysicsMaterials/Props.physicMaterial index 46ea0017..888bc4bf 100644 --- a/Assets/Resources/PhysicsMaterials/Props.physicMaterial +++ b/Assets/Resources/PhysicsMaterials/Props.physicMaterial @@ -9,6 +9,6 @@ PhysicMaterial: m_Name: Props dynamicFriction: 0.6 staticFriction: 0.6 - bounciness: 0.0001 + bounciness: 0.00001 frictionCombine: 0 bounceCombine: 0 From b9e969480369f18415e15c616e04781de92ff1dd Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 11:27:30 +0900 Subject: [PATCH 4/9] Set pyhsics material for Box/Sphere Props --- Assets/Scripts/Core/ObjectSpawning.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Assets/Scripts/Core/ObjectSpawning.cs b/Assets/Scripts/Core/ObjectSpawning.cs index 81d2ef32..93ceac29 100644 --- a/Assets/Scripts/Core/ObjectSpawning.cs +++ b/Assets/Scripts/Core/ObjectSpawning.cs @@ -237,12 +237,14 @@ private GameObject CreateUnitProps(in PropsType type, in Mesh targetMesh) default: var meshCollider = newObject.AddComponent(); meshCollider.sharedMesh = targetMesh; - meshCollider.sharedMaterial = _propsPhysicalMaterial; meshCollider.convex = true; meshCollider.isTrigger = false; break; } + var collider = newObject.GetComponent(); + collider.sharedMaterial = _propsPhysicalMaterial; + var rigidBody = newObject.AddComponent(); rigidBody.mass = 1; rigidBody.drag = 0.25f; From 7fefed8edae137960503269a53ef290c0b817649 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 12:17:35 +0900 Subject: [PATCH 5/9] Add workaround code for wrong Terrain Height when model importing --- Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs | 5 ++--- Assets/Scripts/UI/ModelImporter.cs | 9 +++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs b/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs index 5828631d..c6f0ffff 100644 --- a/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs +++ b/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs @@ -14,11 +14,10 @@ public static class ProceduralHeightmap private static readonly string terrainShaderName = "Universal Render Pipeline/Terrain/Lit"; private static readonly Shader TerrainShader = Shader.Find(terrainShaderName); - // private static readonly int ResolutionPerPatch = 32; private static readonly int DetailResolutionRate = 2; private static readonly int MinDetailResolution = 8; private static readonly float DefaultSmootheness = 0.3f; - private static readonly float BaseMapDistance = 500f; + private static readonly float BaseMapDistance = 100f; private static void UpdateHeightMap(this TerrainData terrainData, in Texture2D heightmapTexture) { @@ -45,6 +44,7 @@ private static void UpdateHeightMap(this TerrainData terrainData, in Texture2D h } } terrainData.SetHeights(0, 0, heightValues); + terrainData.SyncHeightmap(); } private static byte[] GetBytesFromImage(in string imagePath) @@ -129,7 +129,6 @@ public static void Generate(in SDF.Heightmap property, in GameObject heightmapOb if (isVisualMesh) { - // terrainData.terrainLayers = new TerrainLayer[property.texture.Count]; var terrainLayers = new TerrainLayer[property.texture.Count]; for (var i = 0; i < property.texture.Count; i++) { diff --git a/Assets/Scripts/UI/ModelImporter.cs b/Assets/Scripts/UI/ModelImporter.cs index 98b134cd..08c3b206 100644 --- a/Assets/Scripts/UI/ModelImporter.cs +++ b/Assets/Scripts/UI/ModelImporter.cs @@ -110,6 +110,15 @@ public void SetModelForDeploy(in Transform targetTransform) modelDeployOffset.y = DeployOffsetMargin + ((totalBound.min.y < 0) ? -totalBound.min.y : 0); // Debug.Log("Deploy == " + modelDeployOffset.y + " " + totalBound.min + ", " + totalBound.center + "," + totalBound.extents); + + #region Workaround code for Wrong TerrainHeight issue + var terrain = targetTransform.GetComponentInChildren(); + if (terrain != null) + { + var terrainSize = terrain.terrainData.size; + terrain.terrainData.size = terrainSize; + } + #endregion } private bool GetPointAndNormalOnClick(out Vector3 point, out Vector3 normal) From 23eb6fe902f006d23d80722ddea09539cd4e44e6 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 15:55:10 +0900 Subject: [PATCH 6/9] Enhance Lidar Sensor - consider moment of lidar scanning time - modify structure of queue handling --- Assets/Scripts/Devices/Camera.cs | 6 +- Assets/Scripts/Devices/Lidar.cs | 407 ++++++++++-------- .../Tools/SDF/Implement/Implement.Sensor.cs | 2 +- 3 files changed, 231 insertions(+), 184 deletions(-) diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs index 726ca87b..466d4b70 100644 --- a/Assets/Scripts/Devices/Camera.cs +++ b/Assets/Scripts/Devices/Camera.cs @@ -308,10 +308,10 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request) lock (_asyncWorkList) { - checked + var asyncWorkIndex = _asyncWorkList.FindIndex(x => x.request.Equals(request)); + if (asyncWorkIndex >= 0 && asyncWorkIndex < _asyncWorkList.Count) { - var asyncWorkIndex = _asyncWorkList.FindIndex(x => x.request.Equals(request)); - if (asyncWorkIndex >= 0 && asyncWorkIndex < _asyncWorkList.Count) + checked { asyncWork = _asyncWorkList[asyncWorkIndex]; var readbackData = request.GetData(); diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index 528d09ab..3db4124b 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -7,7 +7,9 @@ using System.Collections; using System.Collections.Generic; +using System.Collections.Concurrent; using System.Threading.Tasks; +using Stopwatch = System.Diagnostics.Stopwatch; using System; using UnityEngine.Rendering.Universal; using UnityEngine.Rendering; @@ -21,7 +23,8 @@ namespace SensorDevices [RequireComponent(typeof(UnityEngine.Camera))] public partial class Lidar : Device { - private messages.LaserScanStamped laserScanStamped = null; + private messages.LaserScan _laserScan = null; + protected BlockingCollection _laserStampedQueue = new BlockingCollection(); private const int BatchSize = 8; private const float DEG180 = Mathf.PI * Mathf.Rad2Deg; @@ -57,8 +60,10 @@ public partial class Lidar : Device private DepthData.CamBuffer[] _depthCamBuffers; private LaserData.LaserCamData[] _laserCamData; private LaserData.LaserDataOutput[] _laserDataOutput; - private LaserFilter laserFilter = null; - public Noise noise = null; + private LaserFilter _laserFilter = null; + private Noise _noise = null; + + public double[] _rangesForVisualize = null; protected override void OnAwake() { @@ -80,7 +85,10 @@ protected override void OnStart() SetupLaserCameraData(); _startLaserWork = true; + StartCoroutine(CaptureLaserCamera()); + + StartCoroutine(LaserProcsssing()); } } @@ -103,12 +111,10 @@ protected override void OnReset() protected override void InitializeMessages() { - laserScanStamped = new messages.LaserScanStamped(); - laserScanStamped.Time = new messages.Time(); - laserScanStamped.Scan = new messages.LaserScan(); - laserScanStamped.Scan.WorldPose = new messages.Pose(); - laserScanStamped.Scan.WorldPose.Position = new messages.Vector3d(); - laserScanStamped.Scan.WorldPose.Orientation = new messages.Quaternion(); + _laserScan = new messages.LaserScan(); + _laserScan.WorldPose = new messages.Pose(); + _laserScan.WorldPose.Position = new messages.Vector3d(); + _laserScan.WorldPose.Orientation = new messages.Quaternion(); if (vertical.Equals(default(LaserData.Scan))) { @@ -118,31 +124,32 @@ protected override void InitializeMessages() protected override void SetupMessages() { - var laserScan = laserScanStamped.Scan; - laserScan.Frame = DeviceName; - laserScan.Count = horizontal.samples; - laserScan.AngleMin = horizontal.angle.min * Mathf.Deg2Rad; - laserScan.AngleMax = horizontal.angle.max * Mathf.Deg2Rad; - laserScan.AngleStep = horizontal.angleStep * Mathf.Deg2Rad; - - laserScan.RangeMin = range.min; - laserScan.RangeMax = range.max; - - laserScan.VerticalCount = vertical.samples; - laserScan.VerticalAngleMin = vertical.angle.min * Mathf.Deg2Rad; - laserScan.VerticalAngleMax = vertical.angle.max * Mathf.Deg2Rad; - laserScan.VerticalAngleStep = vertical.angleStep * Mathf.Deg2Rad; + _laserScan.Frame = DeviceName; + _laserScan.Count = horizontal.samples; + _laserScan.AngleMin = horizontal.angle.min * Mathf.Deg2Rad; + _laserScan.AngleMax = horizontal.angle.max * Mathf.Deg2Rad; + _laserScan.AngleStep = horizontal.angleStep * Mathf.Deg2Rad; + + _laserScan.RangeMin = range.min; + _laserScan.RangeMax = range.max; + + _laserScan.VerticalCount = vertical.samples; + _laserScan.VerticalAngleMin = vertical.angle.min * Mathf.Deg2Rad; + _laserScan.VerticalAngleMax = vertical.angle.max * Mathf.Deg2Rad; + _laserScan.VerticalAngleStep = vertical.angleStep * Mathf.Deg2Rad; // Debug.Log(laserScan.VerticalCount + ", " + laserScan.VerticalAngleMin + ", " + laserScan.VerticalAngleMax + ", " + laserScan.VerticalAngleStep); - var totalSamples = laserScan.Count * laserScan.VerticalCount; + var totalSamples = _laserScan.Count * _laserScan.VerticalCount; // Debug.Log(samples + " x " + vertical.samples + " = " + totalSamples); - laserScan.Ranges = new double[totalSamples]; - laserScan.Intensities = new double[totalSamples]; - Array.Clear(laserScan.Ranges, 0, laserScan.Ranges.Length); - Array.Clear(laserScan.Intensities, 0, laserScan.Intensities.Length); - Parallel.ForEach(laserScan.Ranges, item => item = double.NaN); - Parallel.ForEach(laserScan.Intensities, item => item = double.NaN); + _laserScan.Ranges = new double[totalSamples]; + _laserScan.Intensities = new double[totalSamples]; + Array.Clear(_laserScan.Ranges, 0, _laserScan.Ranges.Length); + Array.Clear(_laserScan.Intensities, 0, _laserScan.Intensities.Length); + Parallel.ForEach(_laserScan.Ranges, item => item = double.NaN); + Parallel.ForEach(_laserScan.Intensities, item => item = double.NaN); + + _rangesForVisualize = new double[totalSamples]; laserAngleResolution = new LaserData.AngleResolution((float)horizontal.angleStep, (float)vertical.angleStep); // Debug.Log("H resolution: " + laserAngleResolution.H + ", V resolution: " + laserAngleResolution.V); @@ -237,10 +244,10 @@ private void SetupLaserCamera() // laserCam.hideFlags |= HideFlags.NotEditable; laserCam.enabled = false; - if (noise != null) + if (_noise != null) { - noise.SetClampMin(range.min); - noise.SetClampMax(range.max); + _noise.SetClampMin(range.min); + _noise.SetClampMax(range.max); } } @@ -270,30 +277,38 @@ private void SetupLaserCameraData() } } + public void SetupNoise(in SDF.Noise param) + { + _noise = new SensorDevices.Noise(param, "lidar"); + } + public void SetupLaserAngleFilter(in double filterAngleLower, in double filterAngleUpper, in bool useIntensity = false) { - if (laserFilter == null) + if (_laserFilter == null) { - laserFilter = new LaserFilter(laserScanStamped.Scan, useIntensity); + _laserFilter = new LaserFilter(_laserScan, useIntensity); } - laserFilter.SetupAngleFilter(filterAngleLower, filterAngleUpper); + _laserFilter.SetupAngleFilter(filterAngleLower, filterAngleUpper); } public void SetupLaserRangeFilter(in double filterRangeMin, in double filterRangeMax, in bool useIntensity = false) { - if (laserFilter == null) + if (_laserFilter == null) { - laserFilter = new LaserFilter(laserScanStamped.Scan, useIntensity); + _laserFilter = new LaserFilter(_laserScan, useIntensity); } - laserFilter.SetupRangeFilter(filterRangeMin, filterRangeMax); + _laserFilter.SetupRangeFilter(filterRangeMin, filterRangeMax); } private IEnumerator CaptureLaserCamera() { + var sw = new Stopwatch(); while (_startLaserWork) { + sw.Restart(); + // Update lidar sensor pose _lidarSensorPose.position = _lidarLink.position; _lidarSensorPose.rotation = _lidarLink.rotation; @@ -320,18 +335,14 @@ private IEnumerator CaptureLaserCamera() } laserCam.enabled = false; - yield return null; } } - var processingTime = 0f; - for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++) - { - processingTime += _laserDataOutput[dataIndex].processingTime; - } + sw.Stop(); - var averageProcessingTime = processingTime / numberOfLaserCamData; - yield return new WaitForSeconds(UpdatePeriod - averageProcessingTime); + var processingTime = (float)sw.Elapsed.TotalSeconds; + // Debug.Log("CaptureLaserCamera processingTime=" + (processingTime) + ", UpdatePeriod=" + UpdatePeriod); + yield return new WaitForSeconds(WaitPeriod(processingTime)); } } @@ -343,179 +354,215 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request) } else if (request.done) { - AsyncWork.Laser asyncWork; + var asyncWorkIndex = -1; lock (_asyncWorkList) { - asyncWork = _asyncWorkList.Find(x => x.request.Equals(request)); + asyncWorkIndex = _asyncWorkList.FindIndex(x => x.request.Equals(request)); } - var dataIndex = asyncWork.dataIndex; - var depthCamBuffer = _depthCamBuffers[dataIndex]; - - checked + if (asyncWorkIndex >= 0 && asyncWorkIndex < _asyncWorkList.Count) { - depthCamBuffer.Allocate(); - depthCamBuffer.raw = request.GetData(); - - if (depthCamBuffer.depth.IsCreated) + checked { - var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize); - jobHandleDepthCamBuffer.Complete(); + var asyncWork = _asyncWorkList[asyncWorkIndex]; + var dataIndex = asyncWork.dataIndex; + var depthCamBuffer = _depthCamBuffers[dataIndex]; - var laserCamData = _laserCamData[dataIndex]; - laserCamData.depthBuffer = depthCamBuffer.depth; - laserCamData.Allocate(); + depthCamBuffer.Allocate(); + depthCamBuffer.raw = request.GetData(); - var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize); - jobHandle.Complete(); + if (depthCamBuffer.depth.IsCreated) + { + var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize); + jobHandleDepthCamBuffer.Complete(); - _laserDataOutput[dataIndex].data = laserCamData.GetLaserData(); - _laserDataOutput[dataIndex].capturedTime = asyncWork.capturedTime; - _laserDataOutput[dataIndex].processingTime = (float)DeviceHelper.GlobalClock.SimTime - asyncWork.capturedTime; + var laserCamData = _laserCamData[dataIndex]; + laserCamData.depthBuffer = depthCamBuffer.depth; + laserCamData.Allocate(); - if (noise != null) - { - noise.Apply(ref _laserDataOutput[dataIndex].data); + var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize); + jobHandle.Complete(); + + var laserDataOutput = new LaserData.LaserDataOutput(); + laserDataOutput.data = laserCamData.GetLaserData(); + laserDataOutput.capturedTime = asyncWork.capturedTime; + laserDataOutput.processingTime = (float)DeviceHelper.GlobalClock.SimTime - asyncWork.capturedTime; + + _laserDataOutput[dataIndex] = laserDataOutput; + + laserCamData.Deallocate(); } - laserCamData.Deallocate(); + depthCamBuffer.Deallocate(); } - depthCamBuffer.Deallocate(); - } - - lock (_asyncWorkList) - { - _asyncWorkList.Remove(asyncWork); + lock (_asyncWorkList) + { + _asyncWorkList.RemoveAt(asyncWorkIndex); + } } } } - protected override void GenerateMessage() - { - var lidarPosition = _lidarSensorPose.position + _lidarSensorInitPose.position; - var lidarRotation = _lidarSensorPose.rotation * _lidarSensorInitPose.rotation; - var laserScan = laserScanStamped.Scan; - - laserScan.WorldPose.Position.Set(lidarPosition); - laserScan.WorldPose.Orientation.Set(lidarRotation); + private IEnumerator LaserProcsssing() + { + var laserScanStamped = new messages.LaserScanStamped(); + laserScanStamped.Time = new messages.Time(); - const int BufferUnitSize = sizeof(double); - var laserSamplesH = (int)horizontal.samples; - var laserStartAngleH = (float)horizontal.angle.min; - var laserEndAngleH = (float)horizontal.angle.max; - var laserTotalAngleH = (float)horizontal.angle.range; - var dividedLaserTotalAngleH = 1f / laserTotalAngleH; + var sw = new Stopwatch(); + while (_startLaserWork) + { + sw.Restart(); + var lidarPosition = _lidarSensorPose.position + _lidarSensorInitPose.position; + var lidarRotation = _lidarSensorPose.rotation * _lidarSensorInitPose.rotation; - var laserSamplesV = (int)vertical.samples; - var laserStartAngleV = (float)vertical.angle.min; - var laserEndAngleV = (float)vertical.angle.max; - var laserTotalAngleV = (float)vertical.angle.range; + laserScanStamped.Scan = _laserScan; - var capturedTimeSum = 0f; + var laserScan = laserScanStamped.Scan; - Parallel.For(0, numberOfLaserCamData, _parallelOptions, index => - { - var laserCamData = _laserCamData[index]; - var srcBuffer = _laserDataOutput[index].data; - var srcBufferHorizontalLength = laserCamData.horizontalBufferLength; - var dataStartAngleH = laserCamData.StartAngleH; - var dataEndAngleH = laserCamData.EndAngleH; - var dividedDataTotalAngleH = 1f / laserCamData.TotalAngleH; - - var srcBufferOffset = 0; - var dstBufferOffset = 0; - var copyLength = 0; - var dataLengthRatio = 0f; - var doCopy = true; - - if (srcBuffer == null) - { - return; - } + laserScan.WorldPose.Position.Set(lidarPosition); + laserScan.WorldPose.Orientation.Set(lidarRotation); - capturedTimeSum += _laserDataOutput[index].capturedTime; + const int BufferUnitSize = sizeof(double); + var laserSamplesH = (int)horizontal.samples; + var laserStartAngleH = (float)horizontal.angle.min; + var laserEndAngleH = (float)horizontal.angle.max; + var laserTotalAngleH = (float)horizontal.angle.range; + var dividedLaserTotalAngleH = 1f / laserTotalAngleH; - if (laserStartAngleH < 0 && dataEndAngleH > DEG180) - { - dataStartAngleH -= DEG360; - dataEndAngleH -= DEG360; - } - // Debug.LogWarning(index + " capturedTime=" + _laserDataOutput[index].capturedTime.ToString("F8") - // + ", processingTime=" + _laserDataOutput[index].processingTime.ToString("F8")); - // Debug.LogFormat("index {0}: {1}~{2}, {3}~{4}", dataIndex, laserStartAngleH, laserEndAngleH, dataStartAngleH, dataEndAngleH); + var laserSamplesV = (int)vertical.samples; + var laserStartAngleV = (float)vertical.angle.min; + var laserEndAngleV = (float)vertical.angle.max; + var laserTotalAngleV = (float)vertical.angle.range; - for (var sampleIndexV = 0; sampleIndexV < laserSamplesV; sampleIndexV++, doCopy = true) + var capturedTime = 0f; + var processingTimeSum = 0f; + Parallel.For(0, numberOfLaserCamData, _parallelOptions, index => { - if (dataStartAngleH <= laserStartAngleH) // start side of laser angle + var laserCamData = _laserCamData[index]; + var srcBuffer = _laserDataOutput[index].data; + var srcBufferHorizontalLength = laserCamData.horizontalBufferLength; + var dataStartAngleH = laserCamData.StartAngleH; + var dataEndAngleH = laserCamData.EndAngleH; + var dividedDataTotalAngleH = 1f / laserCamData.TotalAngleH; + + var srcBufferOffset = 0; + var dstBufferOffset = 0; + var copyLength = 0; + var dataLengthRatio = 0f; + var doCopy = true; + + if (_laserDataOutput[index].capturedTime > capturedTime) { - dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH; - copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); - srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; - dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength; - } - else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) // middle of laser angle - { - dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH; - copyLength = srcBufferHorizontalLength; - srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; - dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength; - } - else if (dataEndAngleH >= laserEndAngleH) // end side of laser angle - { - dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH; - copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); - srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength; - dstBufferOffset = laserSamplesH * sampleIndexV + 1; - } - else - { - Debug.LogWarning("Something wrong data copy type in Generating Laser Data...."); - doCopy = false; + capturedTime = _laserDataOutput[index].capturedTime; } - if (doCopy && copyLength >= 0 && dstBufferOffset >= 0) + processingTimeSum += _laserDataOutput[index].processingTime; + + if (srcBuffer != null) { - try + if (laserStartAngleH < 0 && dataEndAngleH > DEG180) { - lock (laserScan.Ranges.SyncRoot) - { - Buffer.BlockCopy(srcBuffer, srcBufferOffset * BufferUnitSize, laserScan.Ranges, dstBufferOffset * BufferUnitSize, copyLength * BufferUnitSize); - } + dataStartAngleH -= DEG360; + dataEndAngleH -= DEG360; } - catch (Exception ex) + + // Debug.LogWarning(index + " capturedTime=" + _laserDataOutput[index].capturedTime.ToString("F8") + // + ", processingTime=" + _laserDataOutput[index].processingTime.ToString("F8")); + // Debug.LogFormat("index {0}: {1}~{2}, {3}~{4}", dataIndex, laserStartAngleH, laserEndAngleH, dataStartAngleH, dataEndAngleH); + + for (var sampleIndexV = 0; sampleIndexV < laserSamplesV; sampleIndexV++, doCopy = true) { - var copyType = -1; - if (dataStartAngleH <= laserStartAngleH) - copyType = 0; - else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) - copyType = 1; - else if (dataEndAngleH >= laserEndAngleH) - copyType = 2; - - Debug.LogWarningFormat("Error occured with Buffer.BlockCopy: {0}, Type: {1} Offset: src({2}) dst({3}) Len: src({4}) dst({5}) copy_size({6})", - ex.Message, copyType, srcBufferOffset, dstBufferOffset, srcBuffer.Length, laserScan.Ranges.Length, copyLength); + if (dataStartAngleH <= laserStartAngleH) // start side of laser angle + { + dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH; + copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); + srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; + dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength; + } + else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) // middle of laser angle + { + dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH; + copyLength = srcBufferHorizontalLength; + srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; + dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength; + } + else if (dataEndAngleH >= laserEndAngleH) // end side of laser angle + { + dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH; + copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); + srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength; + dstBufferOffset = laserSamplesH * sampleIndexV + 1; + } + else + { + Debug.LogWarning("Something wrong data copy type in Generating Laser Data...."); + doCopy = false; + } + + if (doCopy && copyLength >= 0 && dstBufferOffset >= 0) + { + try + { + lock (laserScan.Ranges.SyncRoot) + { + Buffer.BlockCopy(srcBuffer, srcBufferOffset * BufferUnitSize, laserScan.Ranges, dstBufferOffset * BufferUnitSize, copyLength * BufferUnitSize); + } + } + catch (Exception ex) + { + var copyType = -1; + if (dataStartAngleH <= laserStartAngleH) + copyType = 0; + else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) + copyType = 1; + else if (dataEndAngleH >= laserEndAngleH) + copyType = 2; + + Debug.LogWarningFormat("Error occured with Buffer.BlockCopy: {0}, Type: {1} Offset: src({2}) dst({3}) Len: src({4}) dst({5}) copy_size({6})", + ex.Message, copyType, srcBufferOffset, dstBufferOffset, srcBuffer.Length, laserScan.Ranges.Length, copyLength); + } + } + // else + // { + // Debug.LogWarning("wrong data index "+ copyLength + ", " + srcBufferOffset + ", " + dataCopyType); + // } } } - else - { - // Debug.LogWarning("wrong data index "+ copyLength + ", " + srcBufferOffset + ", " + dataCopyType); - } + }); + + if (_noise != null) + { + var ranges = laserScan.Ranges; + _noise.Apply(ref ranges); } - }); - if (laserFilter != null) - { - laserFilter.DoFilter(ref laserScan); - } + if (_laserFilter != null) + { + _laserFilter.DoFilter(ref laserScan); + } + + laserScanStamped.Time.Set(capturedTime); + + _laserStampedQueue.TryAdd(laserScanStamped); - capturedTimeSum += (float)DeviceHelper.GlobalClock.SimTime; - var capturedTime = capturedTimeSum / (numberOfLaserCamData + 1); - laserScanStamped.Time.Set(capturedTime); + sw.Stop(); + var laserProcsssingTime = (float)sw.Elapsed.TotalSeconds; + var avgProcessingTime = processingTimeSum / (float)numberOfLaserCamData; + // Debug.Log("LaserProcessing laserProcessingtime=" + laserProcsssingTime + ", " + avgProcessingTime); + yield return new WaitForSeconds(WaitPeriod(laserProcsssingTime + avgProcessingTime )); + } + } - PushDeviceMessage(laserScanStamped); + protected override void GenerateMessage() + { + if (_laserStampedQueue.TryTake(out var msg)) + { + _rangesForVisualize = msg.Scan.Ranges; + PushDeviceMessage(msg); + } } protected override IEnumerator OnVisualize() @@ -571,7 +618,7 @@ public double[] GetRangeData() { try { - return laserScanStamped.Scan.Ranges; + return _rangesForVisualize; } catch { diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index 548ca763..11bfa5e0 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -67,7 +67,7 @@ public static Device AddLidar(in SDF.Lidar element, in UE.GameObject targetObjec if (element.noise != null) { - lidar.noise = new SensorDevices.Noise(element.noise, "lidar"); + lidar.SetupNoise(element.noise); } return lidar; From 6b2b362e9991033f17d344a316418a30aa4368f6 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 16:41:05 +0900 Subject: [PATCH 7/9] Modify MotorControl Modules to set target velocity left and right simulataneously --- .../Scripts/Devices/Modules/MotorControl.cs | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/MotorControl.cs b/Assets/Scripts/Devices/Modules/MotorControl.cs index ff6431e5..c7660002 100644 --- a/Assets/Scripts/Devices/Modules/MotorControl.cs +++ b/Assets/Scripts/Devices/Modules/MotorControl.cs @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +using System.Threading.Tasks; using System.Collections.Generic; using UnityEngine; using messages = cloisim.msgs; @@ -22,6 +23,20 @@ public enum WheelLocation { NONE, LEFT, RIGHT, REAR_LEFT, REAR_RIGHT }; {WheelLocation.REAR_RIGHT, null} }; + public struct MotorTask + { + public Motor motor; + public float targetVelocity; + + public MotorTask(Motor motor, float targetVelocity) + { + this.motor = motor; + this.targetVelocity = targetVelocity; + } + }; + + List _motorTaskList = new List(); + private Odometry odometry = null; #endregion @@ -31,6 +46,8 @@ public enum WheelLocation { NONE, LEFT, RIGHT, REAR_LEFT, REAR_RIGHT }; public MotorControl(in Transform controllerTransform) { _baseTransform = controllerTransform; + + _motorTaskList.Clear(); } public void Reset() @@ -145,15 +162,22 @@ private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelo { if (wheel.Key.Equals(WheelLocation.RIGHT) || wheel.Key.Equals(WheelLocation.REAR_RIGHT)) { - motor.SetTargetVelocity(angularVelocityRight); + _motorTaskList.Add(new MotorTask(motor, angularVelocityRight)); } if (wheel.Key.Equals(WheelLocation.LEFT) || wheel.Key.Equals(WheelLocation.REAR_LEFT)) { - motor.SetTargetVelocity(angularVelocityLeft); + _motorTaskList.Add(new MotorTask(motor, angularVelocityLeft)); } } } + + Parallel.ForEach(_motorTaskList, motorTask => + { + motorTask.motor.SetTargetVelocity(motorTask.targetVelocity); + }); + + _motorTaskList.Clear(); } /// Get target Motor Velocity From 3c84a1e17e72a91d739a9295479282ba2fc0105a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 18:10:12 +0900 Subject: [PATCH 8/9] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 6dac4ac0..7f8ba22d 100644 --- a/README.md +++ b/README.md @@ -37,12 +37,12 @@ Here are the list of items that is implemented(marked) or planned to be implemen - [X] Joint Pose - [X] Sensor models - [X] LiDAR Sensor - - [X] 2D + - [X] 2D: Due to performance issue, < 40hz is recommended - [X] 3D - [X] Sonar sensor - [X] IMU - [X] Contact - - [X] Camera: Due to performance issue, < 40fps is recommended + - [X] Camera: Due to performance issue, < 40hz is recommended - [ ] Camera intrinsic parameter - [X] Depth Camera - [X] Multi-camera From e7e6566d02599ec4a1e16c2892e99a93a553ed52 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 19 Aug 2024 18:45:53 +0900 Subject: [PATCH 9/9] Modify Odometry Modules - use double type for precision - increse WheelResolution 13bits --- Assets/Scripts/Devices/Modules/Motor.cs | 2 +- .../Scripts/Devices/Modules/MotorControl.cs | 2 - .../Devices/Modules/Odometry.RollingMean.cs | 12 ++--- Assets/Scripts/Devices/Modules/Odometry.cs | 51 +++++++++++-------- Assets/Scripts/Tools/SDF/Util/Unity2SDF.cs | 5 ++ 5 files changed, 41 insertions(+), 31 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index f8cba9c2..a5369881 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -8,7 +8,7 @@ public class Motor : Articulation { - private const float WheelResolution = 0.087890625f; // in degree, encoding 12bits,360° + private const float WheelResolution = 0.043945312f; // in degree, encoding 13bits, 360° private PID _pidControl = null; private float _targetAngularVelocity = 0; // degree per seconds diff --git a/Assets/Scripts/Devices/Modules/MotorControl.cs b/Assets/Scripts/Devices/Modules/MotorControl.cs index c7660002..c249a46a 100644 --- a/Assets/Scripts/Devices/Modules/MotorControl.cs +++ b/Assets/Scripts/Devices/Modules/MotorControl.cs @@ -196,8 +196,6 @@ public bool GetCurrentVelocity(in WheelLocation location, out float angularVeloc public bool Update(messages.Micom.Odometry odomMessage, in float duration, SensorDevices.IMU imuSensor = null) { - // var decreaseVelocity = IsDirectionChanged(duration); - foreach (var wheel in wheelList) { var motor = wheel.Value; diff --git a/Assets/Scripts/Devices/Modules/Odometry.RollingMean.cs b/Assets/Scripts/Devices/Modules/Odometry.RollingMean.cs index a072d7ff..4a856278 100644 --- a/Assets/Scripts/Devices/Modules/Odometry.RollingMean.cs +++ b/Assets/Scripts/Devices/Modules/Odometry.RollingMean.cs @@ -11,16 +11,16 @@ public partial class Odometry private class RollingMean { private int _maxSize = 0; - private Queue _accumulate = null; - private float _accumulateSum = 0f; + private Queue _accumulate = null; + private double _accumulateSum = 0f; public RollingMean(in int windowSize = 5) { _maxSize = windowSize; - _accumulate = new Queue(windowSize); + _accumulate = new Queue(windowSize); } - public void Accumulate(in float value) + public void Accumulate(in double value) { if (_accumulate.Count == _maxSize) { @@ -37,9 +37,9 @@ public void Reset() _accumulateSum = 0; } - public float Get() + public double Get() { - return _accumulateSum / (float)_accumulate.Count; + return _accumulateSum / (double)_accumulate.Count; } } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/Odometry.cs b/Assets/Scripts/Devices/Modules/Odometry.cs index e6a9b8c8..307db8b4 100644 --- a/Assets/Scripts/Devices/Modules/Odometry.cs +++ b/Assets/Scripts/Devices/Modules/Odometry.cs @@ -3,9 +3,11 @@ * * SPDX-License-Identifier: MIT */ +#define USE_ROLLINGMEAN_FOR_ODOM using UnityEngine; using messages = cloisim.msgs; +using System; public partial class Odometry { @@ -16,15 +18,15 @@ public partial class Odometry private WheelInfo wheelInfo; private float _lastImuYaw = 0f; - private Vector3 _odomPose = Vector3.zero; - private float _odomTranslationalVelocity = 0; - private float _odomRotationalVelocity = 0; - + private Vector3d _odomPose = Vector3d.zero; + private double _odomTranslationalVelocity = 0; + private double _odomRotationalVelocity = 0; +#if USE_ROLLINGMEAN_FOR_ODOM private const int RollingMeanWindowSize = 10; private RollingMean rollingMeanOdomTransVelocity = new RollingMean(RollingMeanWindowSize); private RollingMean rollingMeanOdomTAngularVelocity = new RollingMean(RollingMeanWindowSize); - +#endif public float WheelSeparation => this.wheelInfo.wheelSeparation; public float InverseWheelRadius => this.wheelInfo.inversedWheelRadius; @@ -42,13 +44,20 @@ public void Reset() _odomPose.Set(0, 0, 0); _lastImuYaw = 0.0f; +#if USE_ROLLINGMEAN_FOR_ODOM rollingMeanOdomTransVelocity.Reset(); rollingMeanOdomTAngularVelocity.Reset(); +#endif } private bool IsZero(in float value) { - return Mathf.Abs(value) < Quaternion.kEpsilon; + return Mathf.Abs(value) < float.Epsilon; + } + + private bool IsZero(in double value) + { + return Math.Abs(value) < float.Epsilon; } /// Calculate odometry on this robot @@ -76,8 +85,8 @@ private void CalculateOdometry( var direction = _odomPose.y + angular * 0.5f; // Runge-Kutta 2nd order integration: - _odomPose.z += linear * Mathf.Cos(direction); - _odomPose.x += linear * Mathf.Sin(direction); + _odomPose.z += linear * Math.Cos(direction); + _odomPose.x += linear * Math.Sin(direction); _odomPose.y += angular; // Debug.Log("CalcOdom 0 = " + _odomPose.y + ", " + angular); @@ -89,8 +98,8 @@ private void CalculateOdometry( var r = linear / angular; _odomPose.y += angular; - _odomPose.z += r * (Mathf.Sin(_odomPose.y) - Mathf.Sin(headingOld)); - _odomPose.x += -r * (Mathf.Cos(_odomPose.y) - Mathf.Cos(headingOld)); + _odomPose.z += r * (Math.Sin(_odomPose.y) - Math.Sin(headingOld)); + _odomPose.x += -r * (Math.Cos(_odomPose.y) - Math.Cos(headingOld)); // Debug.Log("CalcOdom 1 = " + _odomPose.y + ", " + angular); } @@ -116,7 +125,6 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso return false; } - var deltaThetaIMU = 0f; if (imuSensor != null) { var imuOrientation = imuSensor.GetOrientation(); @@ -124,31 +132,30 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso var deltaAngleImu = Mathf.DeltaAngle(_lastImuYaw, imuYaw); _lastImuYaw = imuYaw; - deltaThetaIMU = IsZero(deltaAngleImu) ? 0 : deltaAngleImu * Mathf.Deg2Rad; - // Debug.Log("deltaThetaIMU =" + deltaThetaIMU); + var deltaThetaIMU = IsZero(deltaAngleImu) ? 0 : deltaAngleImu * Mathf.Deg2Rad; CalculateOdometry(angularVelocityLeft, angularVelocityRight, duration, deltaThetaIMU); } else { - var diffRightLeft = angularVelocityRight - angularVelocityLeft; - var rotationVelocity = IsZero(diffRightLeft) ? 0 : (diffRightLeft * wheelInfo.wheelRadius * wheelInfo.inversedWheelSeparation); - var deltaTheta = rotationVelocity * duration; - // Debug.LogFormat("diff {0:F7}", deltaTheta - deltaThetaIMU); CalculateOdometry(angularVelocityLeft, angularVelocityRight, duration); } - odomMessage.Pose.Set(Unity2SDF.Direction.Reverse(_odomPose)); + var odomPose = new Vector3((float)_odomPose.x, (float)_odomPose.y, (float)_odomPose.z); + odomMessage.Pose.Set(Unity2SDF.Direction.Reverse(odomPose)); // rolling mean filtering var odomTransVel = Unity2SDF.Direction.Curve(_odomTranslationalVelocity); - rollingMeanOdomTransVelocity.Accumulate(odomTransVel); - var odomAngularVel = Unity2SDF.Direction.Curve(_odomRotationalVelocity); - rollingMeanOdomTAngularVelocity.Accumulate(odomAngularVel); +#if USE_ROLLINGMEAN_FOR_ODOM + rollingMeanOdomTransVelocity.Accumulate(odomTransVel); + rollingMeanOdomTAngularVelocity.Accumulate(odomAngularVel); odomMessage.TwistLinear.X = rollingMeanOdomTransVelocity.Get(); odomMessage.TwistAngular.Z = rollingMeanOdomTAngularVelocity.Get(); - +#else + odomMessage.TwistLinear.X = odomTransVel; + odomMessage.TwistAngular.Z = odomAngularVel; +#endif // Debug.LogFormat("odom Vel: {0:F6}, {1:F6}", odomMessage.TwistLinear.X, odomMessage.TwistAngular.Z); // Debug.LogFormat("Odom angular: {0:F6}, {1:F6}", odomMessage.AngularVelocity.Left, odomMessage.AngularVelocity.Right); return true; diff --git a/Assets/Scripts/Tools/SDF/Util/Unity2SDF.cs b/Assets/Scripts/Tools/SDF/Util/Unity2SDF.cs index 5852c013..f82783b3 100644 --- a/Assets/Scripts/Tools/SDF/Util/Unity2SDF.cs +++ b/Assets/Scripts/Tools/SDF/Util/Unity2SDF.cs @@ -53,6 +53,11 @@ public static float Curve(in float value) return -value; } + public static double Curve(in double value) + { + return -value; + } + public class Joint { public static float Prismatic(in float value, in Vector3 rotation)