From bbcb79c808bac1498374eec65eb322d9fe37d749 Mon Sep 17 00:00:00 2001 From: chy <1274653465@qq.com> Date: Wed, 20 Nov 2024 00:36:56 +0800 Subject: [PATCH] Added ImageEncodeJob to speed up Msg encoding; updated Schedule's BatchCount in Job to improve performance; added logging of GPU read errors; removed unsafe code in PointCloud2MsgSerializer. --- .../Camera/DepthCamera/DepthCameraSensor.cs | 4 +- .../Camera/RGBCamera/RGBCameraSensor.cs | 1 + .../Camera/RGBDCamera/RGBDCameraSensor.cs | 6 +- .../DepthBufferLiDARSensor.cs | 5 +- .../LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs | 6 +- .../Prefabs/Camera/DepthCamera_ros.prefab | 7 +- .../SensorMsgs/ImageMsgSerializer.cs | 128 ++++++++---------- .../SensorMsgs/PointCloud2MsgSerializer.cs | 8 +- .../Runtime/Scripts/Utils/Image.meta | 8 ++ .../Scripts/Utils/Image/IImageEncodeJobs.cs | 78 +++++++++++ .../Utils/Image/IImageEncodeJobs.cs.meta | 2 + .../Runtime/UnitySensorsROSRuntime.asmdef | 2 +- 12 files changed, 163 insertions(+), 92 deletions(-) create mode 100644 Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta create mode 100644 Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs create mode 100644 Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index b6d4cdd7..0bc18cee 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -113,8 +113,8 @@ protected override void UpdateSensor() { if (!LoadTexture()) return; - JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); - _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle); + JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1024); + _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1024, updateGaussianNoisesJobHandle); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBCamera/RGBCameraSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBCamera/RGBCameraSensor.cs index 1032012d..26011262 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBCamera/RGBCameraSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBCamera/RGBCameraSensor.cs @@ -43,6 +43,7 @@ protected bool LoadTexture() { if (request.hasError) { + Debug.LogError("GPU readback error detected."); } else { diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBDCamera/RGBDCameraSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBDCamera/RGBDCameraSensor.cs index 36e86d63..d94ad85d 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBDCamera/RGBDCameraSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBDCamera/RGBDCameraSensor.cs @@ -130,8 +130,8 @@ protected override void UpdateSensor() { if (!LoadDepthTexture() || !LoadColorTexture()) return; - JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); - _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle); + JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1024); + _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1024, updateGaussianNoisesJobHandle); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); @@ -146,6 +146,7 @@ private bool LoadDepthTexture() { if (request.hasError) { + Debug.LogError("GPU readback error detected."); } else { @@ -166,6 +167,7 @@ private bool LoadColorTexture() { if (request.hasError) { + Debug.LogError("GPU readback error detected."); } else { diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/DepthBufferLiDARSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/DepthBufferLiDARSensor.cs index 440d4787..46092a91 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/DepthBufferLiDARSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/DepthBufferLiDARSensor.cs @@ -147,8 +147,8 @@ protected override void UpdateSensor() { if (!LoadTexture()) return; - JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1); - _jobHandle = _textureToPointsJob.Schedule(pointsNum, 1, updateGaussianNoisesJobHandle); + JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1024); + _jobHandle = _textureToPointsJob.Schedule(pointsNum, 1024, updateGaussianNoisesJobHandle); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); @@ -166,6 +166,7 @@ private bool LoadTexture() { if (request.hasError) { + Debug.LogError("GPU readback error detected."); } else { diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs index ca9cf84d..eee45f04 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs @@ -91,10 +91,10 @@ protected override void UpdateSensor() _updateRaycastCommandsJob.origin = _transform.position; _updateRaycastCommandsJob.localToWorldMatrix = _transform.localToWorldMatrix; - JobHandle updateRaycastCommandsJobHandle = _updateRaycastCommandsJob.Schedule(pointsNum, 1); + JobHandle updateRaycastCommandsJobHandle = _updateRaycastCommandsJob.Schedule(pointsNum, 1024); JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1, updateRaycastCommandsJobHandle); - JobHandle raycastJobHandle = RaycastCommand.ScheduleBatch(_raycastCommands, _raycastHits, 256, updateGaussianNoisesJobHandle); - _jobHandle = _raycastHitsToPointsJob.Schedule(pointsNum, 1, raycastJobHandle); + JobHandle raycastJobHandle = RaycastCommand.ScheduleBatch(_raycastCommands, _raycastHits, 1024, updateGaussianNoisesJobHandle); + _jobHandle = _raycastHitsToPointsJob.Schedule(pointsNum, 1024, raycastJobHandle); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); diff --git a/Assets/UnitySensorsROS/Runtime/Prefabs/Camera/DepthCamera_ros.prefab b/Assets/UnitySensorsROS/Runtime/Prefabs/Camera/DepthCamera_ros.prefab index 70efb7b2..7516a0b4 100644 --- a/Assets/UnitySensorsROS/Runtime/Prefabs/Camera/DepthCamera_ros.prefab +++ b/Assets/UnitySensorsROS/Runtime/Prefabs/Camera/DepthCamera_ros.prefab @@ -125,7 +125,7 @@ MonoBehaviour: _source: {fileID: 4853532333615571030} _header: _source: {fileID: 4853532333615571030} - _frame_id: /camera_frame + _frame_id: camera_frame --- !u!114 &9201004833233192004 MonoBehaviour: m_ObjectHideFlags: 0 @@ -143,7 +143,7 @@ MonoBehaviour: _serializer: _header: _source: {fileID: 4853532333615571030} - _frame_id: /camera_frame + _frame_id: camera_frame _source: {fileID: 4853532333615571030} --- !u!114 &7171801529083048031 MonoBehaviour: @@ -165,4 +165,5 @@ MonoBehaviour: _encoding: 2 _header: _source: {fileID: 4853532333615571030} - _frame_id: /camera_frame + _frame_id: camera_frame + _encodedTexture: {fileID: 0} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs index ccdf7841..9a636410 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs @@ -5,6 +5,9 @@ using UnitySensors.Attribute; using UnitySensors.Interface.Sensor; using UnitySensors.ROS.Serializer.Std; +using UnitySensors.ROS.Utils.Image; +using Unity.Jobs; +using Unity.Collections; namespace UnitySensors.ROS.Serializer.Sensor { @@ -16,26 +19,6 @@ private enum SourceTexture Texture0, Texture1 } - private enum Encoding - { - _RGB8, - _32FC1, - _16UC1 - } - private struct ColorRGB24 - { - public byte r; - public byte g; - public byte b; - } - private struct Color32FC1 - { - public float r; - } - private struct Color16UC1 - { - public ushort r; - } [SerializeField, Interface(typeof(ITextureInterface))] private Object _source; @@ -43,18 +26,15 @@ private struct Color16UC1 private SourceTexture _sourceTexture; [SerializeField] private Encoding _encoding; + [SerializeField, Attribute.ReadOnly] + private Texture2D _encodedTexture; [SerializeField] private HeaderSerializer _header; private ITextureInterface _sourceInterface; - private Texture2D _flippedTexture; - private float distanceFactor; + private ImageEncodeJob _imageEncodeJob; - private Color[] _sourcePixels; - private ColorRGB24[] _targetPixelsRGB24; - private Color32FC1[] _targetPixels32FC1; - private Color16UC1[] _targetPixels16UC1; public override void Init() { @@ -81,15 +61,31 @@ public override void Init() _msg.encoding = "32FC1"; bytesPerPixel = 1 * 4; textureFormat = TextureFormat.RFloat; - distanceFactor = _sourceInterface.texture0FarClipPlane; - _targetPixels32FC1 = new Color32FC1[width * height]; + float distanceFactor = _sourceInterface.texture0FarClipPlane; + _imageEncodeJob = new() + { + width = width, + height = height, + distanceFactor = distanceFactor, + encoding = _encoding, + targetTexture16UC1 = new NativeArray(0, Allocator.Persistent), + targetTextureRGB8 = new NativeArray(0, Allocator.Persistent) + }; break; case Encoding._16UC1: _msg.encoding = "16UC1"; bytesPerPixel = 1 * 2; textureFormat = TextureFormat.R16; distanceFactor = _sourceInterface.texture0FarClipPlane * 1000; - _targetPixels16UC1 = new Color16UC1[width * height]; + _imageEncodeJob = new() + { + width = width, + height = height, + distanceFactor = distanceFactor, + encoding = _encoding, + targetTexture32FC1 = new NativeArray(0, Allocator.Persistent), + targetTextureRGB8 = new NativeArray(0, Allocator.Persistent) + }; break; case Encoding._RGB8: default: @@ -97,10 +93,17 @@ public override void Init() bytesPerPixel = 3 * 1; textureFormat = TextureFormat.RGB24; distanceFactor = 1; - _targetPixelsRGB24 = new ColorRGB24[width * height]; + _imageEncodeJob = new() + { + width = width, + height = height, + distanceFactor = distanceFactor, + encoding = _encoding, + targetTexture32FC1 = new NativeArray(0, Allocator.Persistent), + targetTexture16UC1 = new NativeArray(0, Allocator.Persistent), + }; break; } - _sourcePixels = new Color[width * height]; _msg.is_bigendian = 0; _msg.width = (uint)width; @@ -108,7 +111,7 @@ public override void Init() _msg.step = (uint)(bytesPerPixel * width); _msg.data = new byte[_msg.step * height]; - _flippedTexture = new Texture2D(width, height, textureFormat, false); + _encodedTexture = new Texture2D(width, height, textureFormat, false); } @@ -117,58 +120,37 @@ public override ImageMsg Serialize() _msg.header = _header.Serialize(); var texture = _sourceTexture == SourceTexture.Texture0 ? _sourceInterface.texture0 : _sourceInterface.texture1; - FlipTextureVertically(texture, _flippedTexture); - - // Manually copy the data to the message to avoid GC allocation - _flippedTexture.GetRawTextureData().CopyTo(_msg.data); - return _msg; - } - private void FlipTextureVertically(Texture2D sourceTexture, Texture2D targetTexture) - { - // TODO: Use shader or jobs to flip the texture - int width = sourceTexture.width; - int height = sourceTexture.height; - - // Manually copy the data to the message to avoid GC allocation - sourceTexture.GetPixelData(0).CopyTo(_sourcePixels); + _imageEncodeJob.sourceTextureRawData = texture.GetRawTextureData(); switch (_encoding) { case Encoding._32FC1: - for (int j = 0; j < height; j++) - { - for (int i = 0; i < width; i++) - { - _targetPixels32FC1[j * width + i].r = _sourcePixels[(height - j - 1) * width + i].r * distanceFactor; - } - } - targetTexture.SetPixelData(_targetPixels32FC1, 0); + _imageEncodeJob.targetTexture32FC1 = _encodedTexture.GetRawTextureData(); break; case Encoding._16UC1: - for (int j = 0; j < height; j++) - { - for (int i = 0; i < width; i++) - { - _targetPixels16UC1[j * width + i].r = (ushort)(_sourcePixels[(height - j - 1) * width + i].r * distanceFactor); - } - } - targetTexture.SetPixelData(_targetPixels16UC1, 0); + _imageEncodeJob.targetTexture16UC1 = _encodedTexture.GetRawTextureData(); break; case Encoding._RGB8: default: - for (int j = 0; j < height; j++) - { - for (int i = 0; i < width; i++) - { - _targetPixelsRGB24[j * width + i].r = (byte)(_sourcePixels[(height - j - 1) * width + i].r * 255); - _targetPixelsRGB24[j * width + i].g = (byte)(_sourcePixels[(height - j - 1) * width + i].g * 255); - _targetPixelsRGB24[j * width + i].b = (byte)(_sourcePixels[(height - j - 1) * width + i].b * 255); - } - } - targetTexture.SetPixelData(_targetPixelsRGB24, 0); + _imageEncodeJob.targetTextureRGB8 = _encodedTexture.GetRawTextureData(); break; } - targetTexture.Apply(); + + _imageEncodeJob.Schedule(texture.width * texture.height, 1024).Complete(); + + _encodedTexture.Apply(); + + // Manually copy the data to the message to avoid GC allocation + _encodedTexture.GetRawTextureData().CopyTo(_msg.data); + return _msg; + } + + public override void OnDestroy() + { + base.OnDestroy(); + _imageEncodeJob.targetTexture32FC1.Dispose(); + _imageEncodeJob.targetTexture16UC1.Dispose(); + _imageEncodeJob.targetTextureRGB8.Dispose(); } } } diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/PointCloud2MsgSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/PointCloud2MsgSerializer.cs index 68708d49..fda9be6a 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/PointCloud2MsgSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/PointCloud2MsgSerializer.cs @@ -1,6 +1,5 @@ using UnityEngine; using Unity.Collections; -using Unity.Collections.LowLevel.Unsafe; using Unity.Jobs; using RosMessageTypes.Sensor; @@ -61,11 +60,8 @@ public override PointCloud2Msg Serialize() { _msg.header = _header.Serialize(); - unsafe - { - UnsafeUtility.MemCpy(NativeArrayUnsafeUtility.GetUnsafePtr(_data), NativeArrayUnsafeUtility.GetUnsafePtr(_sourceInterface.pointCloud.points), _data.Length); - } - _jobHandle = _invertXJob.Schedule(_pointsNum, 1); + _sourceInterface.pointCloud.points.Reinterpret(PointUtilitiesSO.pointDataSizes[typeof(T)]).CopyTo(_data); + _jobHandle = _invertXJob.Schedule(_pointsNum, 1024); _jobHandle.Complete(); _data.CopyTo(_msg.data); diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta new file mode 100644 index 00000000..4f99bcc5 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 2b9ccca7b8586b04fa6457768c935ac0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs new file mode 100644 index 00000000..d4eaa0b8 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs @@ -0,0 +1,78 @@ + +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine; + +namespace UnitySensors.ROS.Utils.Image +{ + [BurstCompile] + struct ImageEncodeJob : IJobParallelFor + { + [ReadOnly] + public NativeArray sourceTextureRawData; + [WriteOnly] + public NativeArray targetTextureRGB8; + [WriteOnly] + public NativeArray targetTexture32FC1; + [WriteOnly] + public NativeArray targetTexture16UC1; + [ReadOnly] + public int width; + [ReadOnly] + public int height; + [ReadOnly] + public float distanceFactor; + [ReadOnly] + public Encoding encoding; + public void Execute(int index) + { + int i = index % width; + int j = index / width; + + var sourceColor = sourceTextureRawData[(height - j - 1) * width + i]; + + switch (encoding) + { + case Encoding._32FC1: + var targetColor32FC1 = new Color32FC1(); + targetColor32FC1.r = sourceColor.r * distanceFactor; + targetTexture32FC1[index] = targetColor32FC1; + break; + case Encoding._16UC1: + var targetColor16UC1 = new Color16UC1(); + targetColor16UC1.r = (ushort)(sourceColor.r * distanceFactor); + targetTexture16UC1[index] = targetColor16UC1; + break; + case Encoding._RGB8: + default: + var targetColorRGB8 = new ColorRGB8(); + targetColorRGB8.r = (byte)(sourceColor.r * 255); + targetColorRGB8.g = (byte)(sourceColor.g * 255); + targetColorRGB8.b = (byte)(sourceColor.b * 255); + targetTextureRGB8[index] = targetColorRGB8; + break; + } + } + } + struct Color32FC1 + { + public float r; + } + struct Color16UC1 + { + public ushort r; + } + struct ColorRGB8 + { + public byte r; + public byte g; + public byte b; + } + enum Encoding + { + _RGB8, + _32FC1, + _16UC1 + } +} \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta new file mode 100644 index 00000000..a2ad74b2 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4374006d20c280c47969bfebc6ec04c5 \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Runtime/UnitySensorsROSRuntime.asmdef b/Assets/UnitySensorsROS/Runtime/UnitySensorsROSRuntime.asmdef index a6c55a8a..addb9f16 100644 --- a/Assets/UnitySensorsROS/Runtime/UnitySensorsROSRuntime.asmdef +++ b/Assets/UnitySensorsROS/Runtime/UnitySensorsROSRuntime.asmdef @@ -11,7 +11,7 @@ ], "includePlatforms": [], "excludePlatforms": [], - "allowUnsafeCode": true, + "allowUnsafeCode": false, "overrideReferences": false, "precompiledReferences": [], "autoReferenced": true,