diff --git a/Assets/Resources/ROSConnectionPrefab.prefab b/Assets/Resources/ROSConnectionPrefab.prefab index aa3e72f4..b8128ce3 100644 --- a/Assets/Resources/ROSConnectionPrefab.prefab +++ b/Assets/Resources/ROSConnectionPrefab.prefab @@ -24,13 +24,13 @@ Transform: m_PrefabInstance: {fileID: 0} m_PrefabAsset: {fileID: 0} m_GameObject: {fileID: 5759000335426716240} + serializedVersion: 2 m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} m_LocalPosition: {x: 0, y: 0, z: 0} m_LocalScale: {x: 1, y: 1, z: 1} m_ConstrainProportionsScale: 0 m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 0 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!114 &8497029536680012343 MonoBehaviour: @@ -50,7 +50,7 @@ MonoBehaviour: m_KeepaliveTime: 1 m_NetworkTimeoutSeconds: 2 m_SleepTimeSeconds: 0.01 - m_ShowHUD: 1 + m_ShowHUD: 0 m_TFTopics: - /tf listenForTFMessages: 1 diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/IImageEncodeJobs.cs similarity index 79% rename from Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs rename to Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/IImageEncodeJobs.cs index d4eaa0b8..60c61408 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/IImageEncodeJobs.cs @@ -4,7 +4,7 @@ using Unity.Jobs; using UnityEngine; -namespace UnitySensors.ROS.Utils.Image +namespace UnitySensors.ROS.Serializer.Image { [BurstCompile] struct ImageEncodeJob : IJobParallelFor @@ -12,11 +12,7 @@ struct ImageEncodeJob : IJobParallelFor [ReadOnly] public NativeArray sourceTextureRawData; [WriteOnly] - public NativeArray targetTextureRGB8; - [WriteOnly] - public NativeArray targetTexture32FC1; - [WriteOnly] - public NativeArray targetTexture16UC1; + public NativeArray targetTextureRawData; [ReadOnly] public int width; [ReadOnly] @@ -25,10 +21,13 @@ struct ImageEncodeJob : IJobParallelFor public float distanceFactor; [ReadOnly] public Encoding encoding; + [ReadOnly] + public int bytesPerPixel; public void Execute(int index) { int i = index % width; int j = index / width; + int targetIndex = index * bytesPerPixel; var sourceColor = sourceTextureRawData[(height - j - 1) * width + i]; @@ -37,12 +36,12 @@ public void Execute(int index) case Encoding._32FC1: var targetColor32FC1 = new Color32FC1(); targetColor32FC1.r = sourceColor.r * distanceFactor; - targetTexture32FC1[index] = targetColor32FC1; + targetTextureRawData.ReinterpretStore(targetIndex, targetColor32FC1); break; case Encoding._16UC1: var targetColor16UC1 = new Color16UC1(); targetColor16UC1.r = (ushort)(sourceColor.r * distanceFactor); - targetTexture16UC1[index] = targetColor16UC1; + targetTextureRawData.ReinterpretStore(targetIndex, targetColor16UC1); break; case Encoding._RGB8: default: @@ -50,7 +49,7 @@ public void Execute(int index) targetColorRGB8.r = (byte)(sourceColor.r * 255); targetColorRGB8.g = (byte)(sourceColor.g * 255); targetColorRGB8.b = (byte)(sourceColor.b * 255); - targetTextureRGB8[index] = targetColorRGB8; + targetTextureRawData.ReinterpretStore(targetIndex, targetColorRGB8); break; } } diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/IImageEncodeJobs.cs.meta similarity index 100% rename from Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image/IImageEncodeJobs.cs.meta rename to Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/IImageEncodeJobs.cs.meta diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs index 9a636410..afef98e9 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs @@ -1,3 +1,4 @@ +using Unity.Jobs; using UnityEngine; using RosMessageTypes.Sensor; @@ -5,9 +6,7 @@ using UnitySensors.Attribute; using UnitySensors.Interface.Sensor; using UnitySensors.ROS.Serializer.Std; -using UnitySensors.ROS.Utils.Image; -using Unity.Jobs; -using Unity.Collections; +using UnitySensors.ROS.Serializer.Image; namespace UnitySensors.ROS.Serializer.Sensor { @@ -26,12 +25,13 @@ private enum SourceTexture private SourceTexture _sourceTexture; [SerializeField] private Encoding _encoding; - [SerializeField, Attribute.ReadOnly] + [SerializeField, ReadOnly] private Texture2D _encodedTexture; [SerializeField] private HeaderSerializer _header; - + private int _width; + private int _height; private ITextureInterface _sourceInterface; private ImageEncodeJob _imageEncodeJob; @@ -50,93 +50,61 @@ public override void Init() return; } - int width = texture.width; - int height = texture.height; + _width = texture.width; + _height = texture.height; uint bytesPerPixel; TextureFormat textureFormat; + + _imageEncodeJob = new() + { + width = _width, + height = _height, + encoding = _encoding, + sourceTextureRawData = texture.GetRawTextureData(), + }; + switch (_encoding) { case Encoding._32FC1: _msg.encoding = "32FC1"; bytesPerPixel = 1 * 4; textureFormat = TextureFormat.RFloat; - 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) - }; + _imageEncodeJob.distanceFactor = _sourceInterface.texture0FarClipPlane; + break; case Encoding._16UC1: _msg.encoding = "16UC1"; bytesPerPixel = 1 * 2; textureFormat = TextureFormat.R16; - distanceFactor = _sourceInterface.texture0FarClipPlane * 1000; - _imageEncodeJob = new() - { - width = width, - height = height, - distanceFactor = distanceFactor, - encoding = _encoding, - targetTexture32FC1 = new NativeArray(0, Allocator.Persistent), - targetTextureRGB8 = new NativeArray(0, Allocator.Persistent) - }; + _imageEncodeJob.distanceFactor = _sourceInterface.texture0FarClipPlane * 1000; break; case Encoding._RGB8: default: _msg.encoding = "rgb8"; bytesPerPixel = 3 * 1; textureFormat = TextureFormat.RGB24; - distanceFactor = 1; - _imageEncodeJob = new() - { - width = width, - height = height, - distanceFactor = distanceFactor, - encoding = _encoding, - targetTexture32FC1 = new NativeArray(0, Allocator.Persistent), - targetTexture16UC1 = new NativeArray(0, Allocator.Persistent), - }; + _imageEncodeJob.distanceFactor = 1; break; } _msg.is_bigendian = 0; - _msg.width = (uint)width; - _msg.height = (uint)height; - _msg.step = (uint)(bytesPerPixel * width); - _msg.data = new byte[_msg.step * height]; + _msg.width = (uint)_width; + _msg.height = (uint)_height; + _msg.step = (uint)(bytesPerPixel * _width); + _msg.data = new byte[_msg.step * _height]; - _encodedTexture = new Texture2D(width, height, textureFormat, false); + _imageEncodeJob.bytesPerPixel = (int)bytesPerPixel; + _encodedTexture = new Texture2D(_width, _height, textureFormat, false); } public override ImageMsg Serialize() { _msg.header = _header.Serialize(); - var texture = _sourceTexture == SourceTexture.Texture0 ? _sourceInterface.texture0 : _sourceInterface.texture1; - _imageEncodeJob.sourceTextureRawData = texture.GetRawTextureData(); - - switch (_encoding) - { - case Encoding._32FC1: - _imageEncodeJob.targetTexture32FC1 = _encodedTexture.GetRawTextureData(); - break; - case Encoding._16UC1: - _imageEncodeJob.targetTexture16UC1 = _encodedTexture.GetRawTextureData(); - break; - case Encoding._RGB8: - default: - _imageEncodeJob.targetTextureRGB8 = _encodedTexture.GetRawTextureData(); - break; - } - - _imageEncodeJob.Schedule(texture.width * texture.height, 1024).Complete(); + _imageEncodeJob.targetTextureRawData = _encodedTexture.GetRawTextureData(); + _imageEncodeJob.Schedule(_width * _height, 1024).Complete(); _encodedTexture.Apply(); @@ -144,13 +112,5 @@ public override ImageMsg Serialize() _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/Utils/Image.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta deleted file mode 100644 index 4f99bcc5..00000000 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: 2b9ccca7b8586b04fa6457768c935ac0 -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/ProjectSettings/GraphicsSettings.asset b/ProjectSettings/GraphicsSettings.asset index e1275984..ad090e32 100644 --- a/ProjectSettings/GraphicsSettings.asset +++ b/ProjectSettings/GraphicsSettings.asset @@ -33,9 +33,6 @@ GraphicsSettings: - {fileID: 15106, guid: 0000000000000000f000000000000000, type: 0} - {fileID: 10753, guid: 0000000000000000f000000000000000, type: 0} - {fileID: 10770, guid: 0000000000000000f000000000000000, type: 0} - - {fileID: 4800000, guid: 0faabdfb36dc37944bd5237eb87dff3e, type: 3} - - {fileID: 4800000, guid: f9fa29512657a5b408ef0c50698b141b, type: 3} - - {fileID: 4800000, guid: 417e6f59011b2b743a51b9344a78b15d, type: 3} m_PreloadedShaders: [] m_PreloadShadersBatchTimeLimit: -1 m_SpritesDefaultMaterial: {fileID: 10754, guid: 0000000000000000f000000000000000,