diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..e232cd65 --- /dev/null +++ b/.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 + } +} \ No newline at end of file diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs index e7092802..59cd2f5f 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs @@ -1,3 +1,4 @@ +using System; using System.Collections; using System.Collections.Generic; using UnityEngine; @@ -8,6 +9,8 @@ using UnityEngine.Jobs; using Unity.Jobs; +using Random = Unity.Mathematics.Random; + namespace UnitySensors { public class LivoxSensor : Sensor @@ -19,9 +22,13 @@ public class LivoxSensor : Sensor private int _scanSeparation = 40; [SerializeField] - private float _minRange = 0.1f; + private float _minDistance = 0.1f; + [SerializeField] + private float _maxDistance = 100.0f; + [SerializeField] + private float _maxIntensity = 255.0f; [SerializeField] - private float _maxRange = 100.0f; + private float _gaussianNoiseSigma = 0.0f; [SerializeField] private int _resolution = 100; @@ -36,11 +43,16 @@ public class LivoxSensor : Sensor private Texture2D _texture; private JobHandle _handle; - private TextureToPointsJob _job; + private TextureToPointsJob _textureToPointsJob; + private UpdateGaussianNoisesJob _updateGaussianNoisesJob; + private Random _random; public NativeArray points; + public NativeArray intensities; private NativeArray _directions; private NativeArray _pixelIndices; + private NativeArray _noises; + private uint _randomSeed; private int _pointsNum; public uint pointsNum { get=>(uint)(_pointsNum);} @@ -80,8 +92,8 @@ private void SetupCamera() _cam.targetTexture = _rt; _cam.fieldOfView = fov; - _cam.nearClipPlane = _minRange; - _cam.farClipPlane = _maxRange; + _cam.nearClipPlane = _minDistance; + _cam.farClipPlane = _maxDistance; _cam.gameObject.AddComponent(); _cam.clearFlags = CameraClearFlags.SolidColor; @@ -109,23 +121,44 @@ private void SetupIndicesAndDirections() private void SetupJob() { points = new NativeArray(_pointsNum, Allocator.Persistent); - _job = new TextureToPointsJob() + intensities = new NativeArray(_pointsNum, Allocator.Persistent); + _randomSeed = (uint)Environment.TickCount; + _random = new Random(_randomSeed); + + _noises = new NativeArray(_pointsNum, Allocator.Persistent); + + _updateGaussianNoisesJob = new UpdateGaussianNoisesJob() + { + sigma = _gaussianNoiseSigma, + random = _random, + noises = _noises + }; + + _textureToPointsJob = new TextureToPointsJob() { - far = _maxRange, scanSeparation = _scanSeparation, separationCounter = 0, + minDistance = _minDistance, + minDistance_sqr = _minDistance * _minDistance, + maxDistance = _maxDistance, + maxIntensity = _maxIntensity, pixelIndices = _pixelIndices, directions = _directions, pixels = _texture.GetPixelData(0), - points = points + noises = _noises, + points = points, + intensities = intensities }; } protected override void UpdateSensor() { _handle.Complete(); - _job.separationCounter++; - if (_job.separationCounter >= _scanSeparation) _job.separationCounter = 0; + if (_randomSeed++ == 0) _randomSeed = 1; + _updateGaussianNoisesJob.random.InitState(_randomSeed); + + _textureToPointsJob.separationCounter++; + if (_textureToPointsJob.separationCounter >= _scanSeparation) _textureToPointsJob.separationCounter = 0; AsyncGPUReadback.Request(_rt, 0, request => { if (request.hasError) @@ -140,7 +173,8 @@ protected override void UpdateSensor() } }); - _handle = _job.Schedule(_pointsNum, 1); + JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); + _handle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle); JobHandle.ScheduleBatchedJobs(); } @@ -153,20 +187,48 @@ public void CompleteJob() private void OnDestroy() { _handle.Complete(); + _noises.Dispose(); _pixelIndices.Dispose(); _directions.Dispose(); points.Dispose(); + intensities.Dispose(); _rt.Release(); } + [BurstCompile] + private struct UpdateGaussianNoisesJob : IJobParallelFor + { + public float sigma; + public Random random; + public NativeArray noises; + + public void Execute(int index) + { + var rand2 = random.NextFloat(); + var rand3 = random.NextFloat(); + float normrand = + (float)Math.Sqrt(-2.0f * Math.Log(rand2)) * + (float)Math.Cos(2.0f * Math.PI * rand3); + noises[index] = sigma * normrand; + } + } + [BurstCompile] private struct TextureToPointsJob : IJobParallelFor { - public float far; public int scanSeparation; public int separationCounter; + [ReadOnly] + public float minDistance; + [ReadOnly] + public float minDistance_sqr; + [ReadOnly] + public float maxDistance; + [ReadOnly] + public float maxIntensity; + [ReadOnly] public NativeArray pixelIndices; [ReadOnly] @@ -174,15 +236,22 @@ private struct TextureToPointsJob : IJobParallelFor [ReadOnly] public NativeArray pixels; + [ReadOnly] + public NativeArray noises; public NativeArray points; + public NativeArray intensities; public void Execute(int index) { int offset = points.Length * separationCounter / scanSeparation; int pixelIndex = pixelIndices.AsReadOnly()[index + offset]; - float distance = pixels.AsReadOnly()[pixelIndex].r; - points[index] = directions.AsReadOnly()[index + offset] * far * Mathf.Clamp01(1.0f - distance); + float distance = maxDistance * Mathf.Clamp01(1.0f - pixels.AsReadOnly()[pixelIndex].r) + noises[index]; + bool isValid = (minDistance <= distance && distance <= maxDistance); + if (!isValid) distance = 0; + points[index] = directions.AsReadOnly()[index + offset] * distance; + float distance_sqr = distance * distance; + intensities[index] = isValid ? maxIntensity * minDistance_sqr / distance_sqr : 0; } } } diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs index 9fab2c2f..4bfbe674 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs +++ b/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 _raycastHits; private Random _random; private NativeArray _noises; + + public NativeArray distances; public NativeArray points; + public NativeArray 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(_pointsNum, Allocator.Persistent); points = new NativeArray(_pointsNum, Allocator.Persistent); + intensities = new NativeArray(_pointsNum, Allocator.Persistent); _raycastCommands = new NativeArray(_pointsNum, Allocator.Persistent); _raycastHits = new NativeArray(_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,6 +182,14 @@ 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 directions; [ReadOnly] @@ -168,11 +197,19 @@ private struct RaycastHitsToPointsJob : IJobParallelFor [ReadOnly] public NativeArray noises; + public NativeArray distances; public NativeArray points; + public NativeArray 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; } } } diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF.meta b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF.meta new file mode 100644 index 00000000..6284bd92 --- /dev/null +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 56f1de2a683e73741815ae06599a3302 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs new file mode 100644 index 00000000..b6f17b9d --- /dev/null +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs @@ -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; } +} \ No newline at end of file diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs.meta b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs.meta new file mode 100644 index 00000000..820f3b96 --- /dev/null +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 60a6512c6d49c2244a1a77e382c678b1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/CSVLiDARScanPattern/CSVLiDARScanPattern.cs b/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/CSVLiDARScanPattern/CSVLiDARScanPattern.cs index d3b639b4..85067ad9 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/CSVLiDARScanPattern/CSVLiDARScanPattern.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/CSVLiDARScanPattern/CSVLiDARScanPattern.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 } } } diff --git a/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/RotatingLiDARScanPattern/RotatingLiDARScanPattern.cs b/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/RotatingLiDARScanPattern/RotatingLiDARScanPattern.cs index 17ba27ba..b31091d9 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/RotatingLiDARScanPattern/RotatingLiDARScanPattern.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Utils/ScanPattern/RotatingLiDARScanPattern/RotatingLiDARScanPattern.cs @@ -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 } } } \ No newline at end of file diff --git a/Assets/UnitySensors/Samples/TF.meta b/Assets/UnitySensors/Samples/TF.meta new file mode 100644 index 00000000..1ca4ad7c --- /dev/null +++ b/Assets/UnitySensors/Samples/TF.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 55b2adbf850448d45afda235a8f2ea87 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensors/Samples/TF/TF.unity b/Assets/UnitySensors/Samples/TF/TF.unity new file mode 100644 index 00000000..d0c59dcf --- /dev/null +++ b/Assets/UnitySensors/Samples/TF/TF.unity @@ -0,0 +1,767 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!29 &1 +OcclusionCullingSettings: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_OcclusionBakeSettings: + smallestOccluder: 5 + smallestHole: 0.25 + backfaceThreshold: 100 + m_SceneGUID: 00000000000000000000000000000000 + m_OcclusionCullingData: {fileID: 0} +--- !u!104 &2 +RenderSettings: + m_ObjectHideFlags: 0 + serializedVersion: 9 + m_Fog: 0 + m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1} + m_FogMode: 3 + m_FogDensity: 0.01 + m_LinearFogStart: 0 + m_LinearFogEnd: 300 + m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1} + m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1} + m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1} + m_AmbientIntensity: 1 + m_AmbientMode: 0 + m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1} + m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0} + m_HaloStrength: 0.5 + m_FlareStrength: 1 + m_FlareFadeSpeed: 3 + m_HaloTexture: {fileID: 0} + m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0} + m_DefaultReflectionMode: 0 + m_DefaultReflectionResolution: 128 + m_ReflectionBounces: 1 + m_ReflectionIntensity: 1 + m_CustomReflection: {fileID: 0} + m_Sun: {fileID: 0} + m_IndirectSpecularColor: {r: 0.44657826, g: 0.49641263, b: 0.57481676, a: 1} + m_UseRadianceAmbientProbe: 0 +--- !u!157 &3 +LightmapSettings: + m_ObjectHideFlags: 0 + serializedVersion: 12 + m_GIWorkflowMode: 1 + m_GISettings: + serializedVersion: 2 + m_BounceScale: 1 + m_IndirectOutputScale: 1 + m_AlbedoBoost: 1 + m_EnvironmentLightingMode: 0 + m_EnableBakedLightmaps: 1 + m_EnableRealtimeLightmaps: 0 + m_LightmapEditorSettings: + serializedVersion: 12 + m_Resolution: 2 + m_BakeResolution: 40 + m_AtlasSize: 1024 + m_AO: 0 + m_AOMaxDistance: 1 + m_CompAOExponent: 1 + m_CompAOExponentDirect: 0 + m_ExtractAmbientOcclusion: 0 + m_Padding: 2 + m_LightmapParameters: {fileID: 0} + m_LightmapsBakeMode: 1 + m_TextureCompression: 1 + m_FinalGather: 0 + m_FinalGatherFiltering: 1 + m_FinalGatherRayCount: 256 + m_ReflectionCompression: 2 + m_MixedBakeMode: 2 + m_BakeBackend: 1 + m_PVRSampling: 1 + m_PVRDirectSampleCount: 32 + m_PVRSampleCount: 512 + m_PVRBounces: 2 + m_PVREnvironmentSampleCount: 256 + m_PVREnvironmentReferencePointCount: 2048 + m_PVRFilteringMode: 1 + m_PVRDenoiserTypeDirect: 1 + m_PVRDenoiserTypeIndirect: 1 + m_PVRDenoiserTypeAO: 1 + m_PVRFilterTypeDirect: 0 + m_PVRFilterTypeIndirect: 0 + m_PVRFilterTypeAO: 0 + m_PVREnvironmentMIS: 1 + m_PVRCulling: 1 + m_PVRFilteringGaussRadiusDirect: 1 + m_PVRFilteringGaussRadiusIndirect: 5 + m_PVRFilteringGaussRadiusAO: 2 + m_PVRFilteringAtrousPositionSigmaDirect: 0.5 + m_PVRFilteringAtrousPositionSigmaIndirect: 2 + m_PVRFilteringAtrousPositionSigmaAO: 1 + m_ExportTrainingData: 0 + m_TrainingDataDestination: TrainingData + m_LightProbeSampleCountMultiplier: 4 + m_LightingDataAsset: {fileID: 0} + m_LightingSettings: {fileID: 0} +--- !u!196 &4 +NavMeshSettings: + serializedVersion: 2 + m_ObjectHideFlags: 0 + m_BuildSettings: + serializedVersion: 2 + agentTypeID: 0 + agentRadius: 0.5 + agentHeight: 2 + agentSlope: 45 + agentClimb: 0.4 + ledgeDropHeight: 0 + maxJumpAcrossDistance: 0 + minRegionArea: 2 + manualCellSize: 0 + cellSize: 0.16666667 + manualTileSize: 0 + tileSize: 256 + accuratePlacement: 0 + maxJobWorkers: 0 + preserveTilesOutsideBounds: 0 + debug: + m_Flags: 0 + m_NavMeshData: {fileID: 0} +--- !u!1001 &270305095 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 0} + m_Modifications: + - target: {fileID: 6235299019127333997, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_Name + value: SimpleEnv + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_RootOrder + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 6235299019127333998, guid: 56564b72d282a2046835e0629759e128, + type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 56564b72d282a2046835e0629759e128, type: 3} +--- !u!1001 &302656453 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 304106766} + m_Modifications: + - target: {fileID: 3835360430879934882, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_Name + value: VLP-16_ros + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_RootOrder + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalPosition.y + value: 0.536 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalPosition.z + value: -0.02 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalRotation.x + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalRotation.y + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalRotation.z + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: d7ba18bcf853116459c3becb08f01c65, type: 3} +--- !u!4 &302656454 stripped +Transform: + m_CorrespondingSourceObject: {fileID: 3835360430879934884, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + m_PrefabInstance: {fileID: 302656453} + m_PrefabAsset: {fileID: 0} +--- !u!1 &304106762 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 304106766} + - component: {fileID: 304106765} + - component: {fileID: 304106764} + - component: {fileID: 304106763} + - component: {fileID: 304106768} + - component: {fileID: 304106767} + - component: {fileID: 304106769} + - component: {fileID: 304106770} + m_Layer: 0 + m_Name: Base_link + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!65 &304106763 +BoxCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Material: {fileID: 0} + m_IsTrigger: 0 + m_Enabled: 1 + serializedVersion: 2 + m_Size: {x: 1, y: 1, z: 1} + m_Center: {x: 0, y: 0, z: 0} +--- !u!23 &304106764 +MeshRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Enabled: 1 + m_CastShadows: 1 + m_ReceiveShadows: 1 + m_DynamicOccludee: 1 + m_StaticShadowCaster: 0 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 2 + m_RayTraceProcedural: 0 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 3 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 + m_AdditionalVertexStreams: {fileID: 0} +--- !u!33 &304106765 +MeshFilter: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} +--- !u!4 &304106766 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 1.82, y: 0.11, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: + - {fileID: 1983887110} + - {fileID: 302656454} + m_Father: {fileID: 0} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &304106767 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0cde12085c739d548a159c84b40bbd2a, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 + _serializer: + _msg: + transforms: [] + _topicName: /tf + thisFrameID: base_link + rootTF: 1 + parentSensor: {fileID: 0} + parentFrameID: ground_truth +--- !u!114 &304106768 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 60a6512c6d49c2244a1a77e382c678b1, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 +--- !u!114 &304106769 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 89775f84ec1b05048997be0d603cca35, type: 3} + m_Name: + m_EditorClassIdentifier: + axesScale: 0.1 + lineThickness: 0.01 + color: {r: 1, g: 1, b: 1, a: 1} +--- !u!114 &304106770 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 304106762} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d0d410a4ef658bc43a5f7617794a8efb, type: 3} + m_Name: + m_EditorClassIdentifier: + _topicName: clock +--- !u!1 &640845642 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 640845644} + - component: {fileID: 640845643} + m_Layer: 0 + m_Name: Directional Light + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!108 &640845643 +Light: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 640845642} + m_Enabled: 1 + serializedVersion: 10 + m_Type: 1 + m_Shape: 0 + m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1} + m_Intensity: 1 + m_Range: 10 + m_SpotAngle: 30 + m_InnerSpotAngle: 21.80208 + m_CookieSize: 10 + m_Shadows: + m_Type: 2 + m_Resolution: -1 + m_CustomResolution: -1 + m_Strength: 1 + m_Bias: 0.05 + m_NormalBias: 0.4 + m_NearPlane: 0.2 + m_CullingMatrixOverride: + e00: 1 + e01: 0 + e02: 0 + e03: 0 + e10: 0 + e11: 1 + e12: 0 + e13: 0 + e20: 0 + e21: 0 + e22: 1 + e23: 0 + e30: 0 + e31: 0 + e32: 0 + e33: 1 + m_UseCullingMatrixOverride: 0 + m_Cookie: {fileID: 0} + m_DrawHalo: 0 + m_Flare: {fileID: 0} + m_RenderMode: 0 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingLayerMask: 1 + m_Lightmapping: 4 + m_LightShadowCasterMode: 0 + m_AreaSize: {x: 1, y: 1} + m_BounceIntensity: 1 + m_ColorTemperature: 6570 + m_UseColorTemperature: 0 + m_BoundingSphereOverride: {x: 0, y: 0, z: 0, w: 0} + m_UseBoundingSphereOverride: 0 + m_UseViewFrustumForShadowCasterCull: 1 + m_ShadowRadius: 0 + m_ShadowAngle: 0 +--- !u!4 &640845644 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 640845642} + m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261} + m_LocalPosition: {x: 0, y: 3, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} +--- !u!1 &949585412 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 949585415} + - component: {fileID: 949585414} + - component: {fileID: 949585413} + m_Layer: 0 + m_Name: Main Camera + m_TagString: MainCamera + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!81 &949585413 +AudioListener: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 949585412} + m_Enabled: 1 +--- !u!20 &949585414 +Camera: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 949585412} + m_Enabled: 1 + serializedVersion: 2 + m_ClearFlags: 1 + m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_projectionMatrixMode: 1 + m_GateFitMode: 2 + m_FOVAxisMode: 0 + m_SensorSize: {x: 36, y: 24} + m_LensShift: {x: 0, y: 0} + m_FocalLength: 50 + m_NormalizedViewPortRect: + serializedVersion: 2 + x: 0 + y: 0 + width: 1 + height: 1 + near clip plane: 0.3 + far clip plane: 1000 + field of view: 60 + orthographic: 0 + orthographic size: 5 + m_Depth: -1 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingPath: -1 + m_TargetTexture: {fileID: 0} + m_TargetDisplay: 0 + m_TargetEye: 3 + m_HDR: 1 + m_AllowMSAA: 1 + m_AllowDynamicResolution: 0 + m_ForceIntoRT: 0 + m_OcclusionCulling: 1 + m_StereoConvergence: 10 + m_StereoSeparation: 0.022 +--- !u!4 &949585415 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 949585412} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 1, z: -10} + 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!1 &1822541744 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 4278881538632891480, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + m_PrefabInstance: {fileID: 1983887109} + m_PrefabAsset: {fileID: 0} +--- !u!1 &1891329939 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 5416862128607984436, guid: d7ba18bcf853116459c3becb08f01c65, + type: 3} + m_PrefabInstance: {fileID: 302656453} + m_PrefabAsset: {fileID: 0} +--- !u!114 &1891329943 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1891329939} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 60a6512c6d49c2244a1a77e382c678b1, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 +--- !u!114 &1891329944 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1891329939} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0cde12085c739d548a159c84b40bbd2a, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 + _serializer: + _msg: + transforms: [] + _topicName: /tf + thisFrameID: velodyne_link + rootTF: 0 + parentSensor: {fileID: 304106762} + parentFrameID: base_link +--- !u!1001 &1983887109 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 304106766} + m_Modifications: + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_RootOrder + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalPosition.y + value: 0.391 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalPosition.z + value: 0.626 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalRotation.x + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalRotation.y + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalRotation.z + value: -0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4278881538632891480, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + propertyPath: m_Name + value: RGBCamera_ros + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 8ab4d4b2351e5644d9abeaa752110709, type: 3} +--- !u!4 &1983887110 stripped +Transform: + m_CorrespondingSourceObject: {fileID: 4278881538632891476, guid: 8ab4d4b2351e5644d9abeaa752110709, + type: 3} + m_PrefabInstance: {fileID: 1983887109} + m_PrefabAsset: {fileID: 0} +--- !u!114 &1983887114 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1822541744} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0cde12085c739d548a159c84b40bbd2a, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 + _serializer: + _msg: + transforms: [] + _topicName: /tf + thisFrameID: camera + rootTF: 0 + parentSensor: {fileID: 304106762} + parentFrameID: base_link +--- !u!114 &1983887115 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1822541744} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 60a6512c6d49c2244a1a77e382c678b1, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 diff --git a/Assets/UnitySensors/Samples/TF/TF.unity.meta b/Assets/UnitySensors/Samples/TF/TF.unity.meta new file mode 100644 index 00000000..e45fd64f --- /dev/null +++ b/Assets/UnitySensors/Samples/TF/TF.unity.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: e84e2ab71efe63b4a984b45a10c6ff38 +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne.meta new file mode 100644 index 00000000..7ace4fb2 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e468c3facd00acf438421524b56859ea +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg.meta new file mode 100644 index 00000000..5c486aaa --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e9de86ff082cf5647b3cccb778de93a4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs new file mode 100644 index 00000000..e9c944a0 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs @@ -0,0 +1,66 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.BuiltinInterfaces; + +namespace RosMessageTypes.Velodyne +{ + [Serializable] + public class VelodynePacketMsg : Message + { + public const string k_RosMessageName = "velodyne_msgs/VelodynePacket"; + public override string RosMessageName => k_RosMessageName; + + // Raw Velodyne LIDAR packet. + public TimeMsg stamp; + // packet timestamp + public byte[] data; + // packet contents + + public VelodynePacketMsg() + { + this.stamp = new TimeMsg(); + this.data = new byte[1206]; + } + + public VelodynePacketMsg(TimeMsg stamp, byte[] data) + { + this.stamp = stamp; + this.data = data; + } + + public static VelodynePacketMsg Deserialize(MessageDeserializer deserializer) => new VelodynePacketMsg(deserializer); + + private VelodynePacketMsg(MessageDeserializer deserializer) + { + this.stamp = TimeMsg.Deserialize(deserializer); + deserializer.Read(out this.data, sizeof(byte), 1206); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.stamp); + serializer.Write(this.data); + } + + public override string ToString() + { + return "VelodynePacketMsg: " + + "\nstamp: " + stamp.ToString() + + "\ndata: " + System.String.Join(", ", data.ToList()); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs.meta new file mode 100644 index 00000000..1db9744e --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodynePacketMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8f8c3ad2b3016f9479dc8f2c5f9cc57d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs new file mode 100644 index 00000000..7eedc8fb --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs @@ -0,0 +1,67 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Std; + +namespace RosMessageTypes.Velodyne +{ + [Serializable] + public class VelodyneScanMsg : Message + { + public const string k_RosMessageName = "velodyne_msgs/VelodyneScan"; + public override string RosMessageName => k_RosMessageName; + + // Velodyne LIDAR scan packets. + public HeaderMsg header; + // standard ROS message header + public VelodynePacketMsg[] packets; + // vector of raw packets + + public VelodyneScanMsg() + { + this.header = new HeaderMsg(); + this.packets = new VelodynePacketMsg[0]; + } + + public VelodyneScanMsg(HeaderMsg header, VelodynePacketMsg[] packets) + { + this.header = header; + this.packets = packets; + } + + public static VelodyneScanMsg Deserialize(MessageDeserializer deserializer) => new VelodyneScanMsg(deserializer); + + private VelodyneScanMsg(MessageDeserializer deserializer) + { + this.header = HeaderMsg.Deserialize(deserializer); + deserializer.Read(out this.packets, VelodynePacketMsg.Deserialize, deserializer.ReadLength()); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.header); + serializer.WriteLength(this.packets); + serializer.Write(this.packets); + } + + public override string ToString() + { + return "VelodyneScanMsg: " + + "\nheader: " + header.ToString() + + "\npackets: " + System.String.Join(", ", packets.ToList()); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs.meta new file mode 100644 index 00000000..1309e76d --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Messages/Velodyne/msg/VelodyneScanMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0cc1f1984e0237d41bd7ed6880097c68 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs index c130d51e..911da026 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs @@ -36,10 +36,12 @@ protected override void Update() { base.Update(); _serializer_gps.Update(); +#if UNITY_EDITOR if (!Application.isPlaying && (_serializer_gps.format.updated)) { EditorUtility.SetDirty(this); } +#endif } protected override void Publish(float time) diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs new file mode 100644 index 00000000..3689067d --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs @@ -0,0 +1,47 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using RosMessageTypes.Velodyne; + +namespace UnitySensors.ROS +{ + [RequireComponent(typeof(VelodyneSensor))] + public class VelodynePacketsPublisher : Publisher + { + [SerializeField] + private string _topicName = "velodyne_packets"; + + [SerializeField] + private string _frameId = "velodyne_link"; + + private VelodyneMsgSerializer _serializer_vmsg; + private bool _init = false; + + protected override void Init() + { + if (!_sensor.initialized) return; + _ros.RegisterPublisher(_topicName); + + _serializer_vmsg = new VelodyneMsgSerializer(); + _serializer_vmsg.Init(_frameId, ref _sensor.distances, ref _sensor.intensities, (int)_sensor.pointsNum, _sensor.azimuthResolution); + _init = true; + } + private void OnApplicationQuit() + { + _serializer_vmsg.Dispose(); + } + + protected override void Publish(float time) + { + if (!_init) + { + if (_sensor.initialized) Init(); + return; + } + _sensor.CompleteJob(); + _serializer_vmsg.Serialize(time); + _ros.Publish(_topicName, _serializer_vmsg.msg); + } + } +} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs.meta new file mode 100644 index 00000000..e172b88b --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePacketsPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c1ac5f79f8e56d54b9e57b7d8dd97fbe +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs new file mode 100644 index 00000000..22d530cf --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs @@ -0,0 +1,47 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using RosMessageTypes.Sensor; + +namespace UnitySensors.ROS +{ + [RequireComponent(typeof(VelodyneSensor))] + public class VelodynePointsPublisher : Publisher + { + [SerializeField] + private string _topicName = "velodyne_points"; + + [SerializeField] + private string _frameId = "velodyne_link"; + + private PointCloud2Serializer _serializer_pc; + private bool _init = false; + + protected override void Init() + { + if (!_sensor.initialized) return; + _ros.RegisterPublisher(_topicName); + + _serializer_pc = new PointCloud2Serializer(); + _serializer_pc.Init(_frameId, ref _sensor.points, _sensor.pointsNum); + _init = true; + } + private void OnApplicationQuit() + { + _serializer_pc.Dispose(); + } + + protected override void Publish(float time) + { + if (!_init) + { + if (_sensor.initialized) Init(); + return; + } + _sensor.CompleteJob(); + _serializer_pc.Serialize(time); + _ros.Publish(_topicName, _serializer_pc.msg); + } + } +} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs.meta new file mode 100644 index 00000000..38ee030f --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/Velodyne/VelodynePointsPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 43d70e58774ec4c41aeb18b112eab899 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/Publisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/Publisher.cs index 7d73a332..50d2443e 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/Publisher.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/Publisher.cs @@ -8,7 +8,7 @@ namespace UnitySensors.ROS { /// - /// T( : Sensor)‚ªŽæ“¾‚µ‚½ƒf[ƒ^‚ðTT( : Serializer)‚ŃVƒŠƒAƒ‰ƒCƒY‚µAROSƒgƒsƒbƒN‚Æ‚µ‚ÄPub‚·‚é + /// T( : Sensor)ãŒå–å¾—ã—ãŸãƒ‡ãƒ¼ã‚¿ã‚’TT( : Serializer)ã§ã‚·ãƒªã‚¢ãƒ©ã‚¤ã‚ºã—ã€ROSトピックã¨ã—ã¦Pubã™ã‚‹ /// public class Publisher : MonoBehaviour where T : Sensor where TT : Serializer, new() { @@ -41,7 +41,7 @@ protected virtual void Start() } /// - /// ‰Šú‰»‚̉¼‘zŠÖ” + /// åˆæœŸåŒ–ã®ä»®æƒ³é–¢æ•° /// protected virtual void Init() { @@ -55,11 +55,12 @@ protected virtual void Update() if (_time_now - _time_old > _frequency_inv) { Publish(_time_now); + _time_old = _time_now; } } /// - /// ROSƒgƒsƒbƒN‘—M‚̉¼‘zŠÖ” + /// ROSトピックé€ä¿¡ã®ä»®æƒ³é–¢æ•° /// protected virtual void Publish(float time) { diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF.meta new file mode 100644 index 00000000..0b3d4d5a --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e36dd71420e5e414e94bdb617981d9c9 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs new file mode 100644 index 00000000..9faf8174 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs @@ -0,0 +1,45 @@ +using RosMessageTypes.Tf2; +using UnityEngine; + +namespace UnitySensors.ROS +{ + + public class TFPublisher : Publisher + { + public string _topicName = "/tf"; + public string thisFrameID; + public bool rootTF; + public GameObject parentSensor; + public string parentFrameID; + + + private Vector3 position_rel; // local position relative to parentSensor + private Quaternion rotation_rel; // local rotation relative to parentSensor + + protected override void Init() + { + _ros.RegisterPublisher(_topicName); + if (rootTF) + { + _serializer.Init(parentFrameID); + } + else + { + _serializer.Init(parentFrameID); + position_rel = parentSensor.transform.InverseTransformPoint(transform.position); + rotation_rel = Quaternion.Inverse(parentSensor.transform.rotation) * transform.rotation; + } + } + + protected override void Publish(float time) + { + if (rootTF) + { + position_rel = transform.position; + rotation_rel = transform.rotation; + } + _serializer.Serialize(time, thisFrameID, position_rel, rotation_rel); + _ros.Publish(_topicName, _serializer.msg); + } + } +} \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs.meta new file mode 100644 index 00000000..379d0c36 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0cde12085c739d548a159c84b40bbd2a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/PointCloud2/PointCloud2Serializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/PointCloud2/PointCloud2Serializer.cs index e106692c..499e61d5 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/PointCloud2/PointCloud2Serializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/PointCloud2/PointCloud2Serializer.cs @@ -22,23 +22,23 @@ public class PointCloud2Serializer : Serializer private JobHandle _handle; private PointsToPointCloud2MsgJob _job; - private int _pointNum; + private int _pointsNum; private NativeArray _data; public PointCloud2Msg msg { get => _msg; } - public void Init(string frame_id, ref NativeArray points, uint pointNum) + public void Init(string frame_id, ref NativeArray points, uint pointsNum) { - _pointNum = (int)pointNum; + _pointsNum = (int)pointsNum; _header = new AutoHeader(); _header.Init(frame_id); _msg = new PointCloud2Msg(); _msg.height = 1; - _msg.width = pointNum; + _msg.width = pointsNum; _msg.fields = new PointFieldMsg[3]; - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { _msg.fields[i] = new PointFieldMsg(); _msg.fields[i].name = ((char)('x' + i)).ToString(); @@ -48,15 +48,15 @@ public void Init(string frame_id, ref NativeArray points, uint pointNum } _msg.is_bigendian = false; _msg.point_step = 12; - _msg.row_step = pointNum * 12; - _msg.data = new byte[_pointNum * 12]; + _msg.row_step = pointsNum * 12; + _msg.data = new byte[_pointsNum * 12]; _msg.is_dense = true; - _data = new NativeArray(_pointNum * 12, Allocator.Persistent); + _data = new NativeArray(_pointsNum * 12, Allocator.Persistent); _job = new PointsToPointCloud2MsgJob { - pointNum = _pointNum, + pointNum = _pointsNum, points = points, data = _data }; @@ -64,7 +64,7 @@ public void Init(string frame_id, ref NativeArray points, uint pointNum public PointCloud2Msg Serialize(float time) { - _handle = _job.Schedule(_pointNum, 1); + _handle = _job.Schedule(_pointsNum, 1); JobHandle.ScheduleBatchedJobs(); _handle.Complete(); @@ -94,8 +94,8 @@ public struct PointsToPointCloud2MsgJob : IJobParallelFor public void Execute(int index) { NativeArray tmp = new NativeArray(3, Allocator.Temp); - tmp[0] = points[index].x; - tmp[1] = points[index].z; + tmp[0] = -points[index].z; + tmp[1] = points[index].x; tmp[2] = points[index].y; var slice = new NativeSlice(tmp).SliceConvert(); slice.CopyTo(data.GetSubArray(index * 12, 12)); diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs new file mode 100644 index 00000000..bba6f0d9 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs @@ -0,0 +1,147 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using Unity.Burst; +using Unity.Collections; +using UnityEngine.Jobs; +using Unity.Jobs; + +using RosMessageTypes.Velodyne; + +namespace UnitySensors.ROS +{ + [System.Serializable] + public class VelodyneMsgSerializer : Serializer + { + private VelodyneScanMsg _msg; + private AutoHeader _header; + + private JobHandle _handle; + private DistancesToVelodyneScanMsgJob _job; + + private NativeArray _packets; + + private int _jobSize; + + public VelodyneScanMsg msg { get => _msg; } + + public void Init(string frame_id, ref NativeArray distances, ref NativeArray intensities, int pointsNum, int azimuthResolution) + { + _header = new AutoHeader(); + _header.Init(frame_id); + + _msg = new VelodyneScanMsg(); + _msg.packets = new VelodynePacketMsg[azimuthResolution/12]; + for (int i = 0; i < _msg.packets.Length; i++) + { + _msg.packets[i] = new VelodynePacketMsg(); + _msg.packets[i].data = new byte[1206]; + } + + _packets = new NativeArray(pointsNum / 12 * 1260, Allocator.Persistent); + + _jobSize = azimuthResolution / 12; + + _job = new DistancesToVelodyneScanMsgJob + { + azimuth_coef = 1200.0f * 360.0f / azimuthResolution, + timeStamp = 0, + distances = distances, + intensities = intensities, + packets = _packets + }; + } + + public VelodyneScanMsg Serialize(float time) + { + _handle = _job.Schedule(_jobSize, 1); + JobHandle.ScheduleBatchedJobs(); + _handle.Complete(); + + _header.Serialize(time); + + _msg.header = _header.header; + + uint sec = _header.header.stamp.sec; + uint nanosec = _header.header.stamp.nanosec; + + for (int i = 0; i < _msg.packets.Length; i++) + { + _msg.packets[i].data = _packets.GetSubArray(i * 1206, 1206).ToArray(); + _msg.packets[i].stamp.sec = sec; + _msg.packets[i].stamp.nanosec = nanosec; + } + + return _msg; + } + + public void Dispose() + { + _handle.Complete(); + _packets.Dispose(); + } + + [BurstCompile] + public struct DistancesToVelodyneScanMsgJob : IJobParallelFor + { + [ReadOnly] + public float azimuth_coef; + [ReadOnly] + public float timeStamp; + + [ReadOnly, NativeDisableParallelForRestriction] + public NativeArray distances; + [ReadOnly, NativeDisableParallelForRestriction] + public NativeArray intensities; + + [NativeDisableParallelForRestriction] + public NativeArray packets; + + public void Execute(int index) + { + ushort azimuth = (ushort)Mathf.Round(index * azimuth_coef); + byte azimuth_packet_u = (byte)((azimuth << 8) >> 8); + byte azimuth_packet_d = (byte)(azimuth >> 8); + + for (int i = 0; i < 1200; i += 100) + { + int index_i = index + i; + + // Header + packets[index_i++] = 0xff; + packets[index_i++] = 0xee; + + // Azimuth Data + packets[index_i++] = azimuth_packet_u; + packets[index_i++] = azimuth_packet_d; + + for(int j = 0; j < 16; j++) + { + int index_j = index_i + j * 3; + int index_d = index_i + (j / 2) + (j % 2) * 8; + + // Distance + ushort distance = (ushort)Mathf.Round((distances[index_d] * 1e+3f) / 2.0f); + packets[index_j++] = (byte)((distance << 8) >> 8); + packets[index_j++] = (byte)(distance >> 8); + + // Intensity + packets[index_j] = (byte)(intensities[index_d]); + } + } + // Time Stamp + var time = (uint)Math.Truncate(timeStamp * 1e+6f); + packets[index + 1200] = (byte)((time << 24) >> 24); + packets[index + 1201] = (byte)((time << 16) >> 24); + packets[index + 1202] = (byte)((time << 8) >> 24); + packets[index + 1203] = (byte)(time >> 24); + + // Footer + packets[index + 1204] = 0x37; + packets[index + 1205] = 0x22; + } + } + } +} diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs.meta new file mode 100644 index 00000000..c49cd6da --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 15217ef405f8ea5408d1f4f2eee2d916 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/Serializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/Serializer.cs index 09c14e37..e79a10c6 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/Serializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/Serializer.cs @@ -28,7 +28,10 @@ public void Init(string frame_id) { _header = new HeaderMsg(); _header.frame_id = frame_id; +#if ROS2 +#else _header.seq = 0; +#endif } /// @@ -36,10 +39,17 @@ public void Init(string frame_id) /// public void Serialize(float time) { +#if ROS2 + int sec = (int)Math.Truncate(time); +#else uint sec = (uint)Math.Truncate(time); +# endif _header.stamp.sec = sec; _header.stamp.nanosec = (uint)((time - sec) * 1e+9); +#if ROS2 +#else _header.seq++; +#endif } } } diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF.meta new file mode 100644 index 00000000..192c46ec --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: c092a0b613d2d094699154ff5618ce4c +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs new file mode 100644 index 00000000..e0cded7a --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs @@ -0,0 +1,41 @@ +using RosMessageTypes.Geometry; +using RosMessageTypes.Tf2; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace UnitySensors.ROS +{ + + [System.Serializable] + public class TFSerializer : Serializer + { + + [SerializeField] + private TFMessageMsg _msg; + + private AutoHeader _header; + + public TFMessageMsg msg { get => _msg; } + + public void Init(string parent_frame_id) + { + _msg = new TFMessageMsg(); + _msg.transforms = new TransformStampedMsg[1]; + _header = new AutoHeader(); + + _header.Init(parent_frame_id); + } + + public TFMessageMsg Serialize(float time, string thisFrameID, Vector3 position, Quaternion rotation) + { + _header.Serialize(time); + TransformStampedMsg tfStamped = new TransformStampedMsg(); + tfStamped.header = _header.header; + tfStamped.child_frame_id = thisFrameID; + tfStamped.transform.translation = position.To(); + tfStamped.transform.rotation = rotation.To(); + _msg.transforms[0] = tfStamped; + return _msg; + } + } +} \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs.meta new file mode 100644 index 00000000..d9af177f --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 60457f8606f92814587c0f816d19ccb3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Clock/ROSClock.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Clock/ROSClock.cs index 6fca642e..666e3b31 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Clock/ROSClock.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Utils/Clock/ROSClock.cs @@ -28,7 +28,11 @@ void Start() void Update() { float time = Time.time; +#if ROS2 + int sec = (int)Math.Truncate(time); +#else uint sec = (uint)Math.Truncate(time); +# endif uint nanosec = (uint)((time - sec) * 1e+9); _message.clock.sec = sec; _message.clock.nanosec = nanosec; diff --git a/Packages/manifest.json b/Packages/manifest.json index afe95492..2442d913 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -6,10 +6,11 @@ "com.unity.ide.visualstudio": "2.0.16", "com.unity.ide.vscode": "1.2.5", "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector", - "com.unity.test-framework": "1.1.31", + "com.unity.robotics.visualizations": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations", + "com.unity.test-framework": "1.1.33", "com.unity.textmeshpro": "3.0.6", - "com.unity.timeline": "1.6.4", - "com.unity.toolchain.linux-x86_64": "2.0.2", + "com.unity.timeline": "1.6.5", + "com.unity.toolchain.linux-x86_64": "2.0.4", "com.unity.ugui": "1.0.0", "com.unity.modules.ai": "1.0.0", "com.unity.modules.androidjni": "1.0.0", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 28662a88..e1b99ae5 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -71,6 +71,13 @@ "dependencies": {}, "hash": "c27f00c6cf750d2d0564349b3039d19aa3925e7c" }, + "com.unity.robotics.visualizations": { + "version": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations", + "depth": 0, + "source": "git", + "dependencies": {}, + "hash": "c27f00c6cf750d2d0564349b3039d19aa3925e7c" + }, "com.unity.services.core": { "version": "1.6.0", "depth": 1, @@ -83,23 +90,23 @@ "url": "https://packages.unity.com" }, "com.unity.sysroot": { - "version": "2.0.3", + "version": "2.0.5", "depth": 1, "source": "registry", "dependencies": {}, "url": "https://packages.unity.com" }, "com.unity.sysroot.linux-x86_64": { - "version": "2.0.2", + "version": "2.0.4", "depth": 1, "source": "registry", "dependencies": { - "com.unity.sysroot": "2.0.3" + "com.unity.sysroot": "2.0.5" }, "url": "https://packages.unity.com" }, "com.unity.test-framework": { - "version": "1.1.31", + "version": "1.1.33", "depth": 0, "source": "registry", "dependencies": { @@ -119,7 +126,7 @@ "url": "https://packages.unity.com" }, "com.unity.timeline": { - "version": "1.6.4", + "version": "1.6.5", "depth": 0, "source": "registry", "dependencies": { @@ -131,12 +138,12 @@ "url": "https://packages.unity.com" }, "com.unity.toolchain.linux-x86_64": { - "version": "2.0.2", + "version": "2.0.4", "depth": 0, "source": "registry", "dependencies": { - "com.unity.sysroot": "2.0.3", - "com.unity.sysroot.linux-x86_64": "2.0.2" + "com.unity.sysroot": "2.0.5", + "com.unity.sysroot.linux-x86_64": "2.0.4" }, "url": "https://packages.unity.com" }, diff --git a/ProjectSettings/AutoStreamingSettings.asset b/ProjectSettings/AutoStreamingSettings.asset new file mode 100644 index 00000000..d3e071e2 --- /dev/null +++ b/ProjectSettings/AutoStreamingSettings.asset @@ -0,0 +1,21 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1200 &1 +AutoStreamingSettings: + m_ObjectHideFlags: 0 + serializedVersion: 2 + mSearchMode: 15 + mCustomSearchFile: + mTextureSearchString: + mMeshSearchString: + mTextures: [] + mAudios: [] + mMeshes: [] + mScenes: [] + mConfigCCD: + useCCD: 0 + cosKey: + projectGuid: + bucketUuid: + bucketName: + badgeName: diff --git a/ProjectSettings/BurstAotSettings_StandaloneWindows.json b/ProjectSettings/BurstAotSettings_StandaloneWindows.json new file mode 100644 index 00000000..e02ae332 --- /dev/null +++ b/ProjectSettings/BurstAotSettings_StandaloneWindows.json @@ -0,0 +1,17 @@ +{ + "MonoBehaviour": { + "Version": 4, + "EnableBurstCompilation": true, + "EnableOptimisations": true, + "EnableSafetyChecks": false, + "EnableDebugInAllBuilds": false, + "UsePlatformSDKLinker": false, + "CpuMinTargetX32": 0, + "CpuMaxTargetX32": 0, + "CpuMinTargetX64": 0, + "CpuMaxTargetX64": 0, + "CpuTargetsX32": 6, + "CpuTargetsX64": 72, + "OptimizeFor": 0 + } +} diff --git a/ProjectSettings/CommonBurstAotSettings.json b/ProjectSettings/CommonBurstAotSettings.json new file mode 100644 index 00000000..0293dafc --- /dev/null +++ b/ProjectSettings/CommonBurstAotSettings.json @@ -0,0 +1,6 @@ +{ + "MonoBehaviour": { + "Version": 4, + "DisabledWarnings": "" + } +} diff --git a/ProjectSettings/EditorBuildSettings.asset b/ProjectSettings/EditorBuildSettings.asset index 4bcac796..50d6f498 100644 --- a/ProjectSettings/EditorBuildSettings.asset +++ b/ProjectSettings/EditorBuildSettings.asset @@ -4,7 +4,10 @@ EditorBuildSettings: m_ObjectHideFlags: 0 serializedVersion: 2 - m_Scenes: [] + m_Scenes: + - enabled: 1 + path: Assets/UnitySensors/Samples/TF/TF.unity + guid: e84e2ab71efe63b4a984b45a10c6ff38 m_configObjects: com.unity.addressableassets: {fileID: 11400000, guid: 18f8076c387088155a4fefb632dbebe6, type: 2} diff --git a/ProjectSettings/PackageManagerSettings.asset b/ProjectSettings/PackageManagerSettings.asset index be4a7974..dec5942a 100644 --- a/ProjectSettings/PackageManagerSettings.asset +++ b/ProjectSettings/PackageManagerSettings.asset @@ -12,10 +12,11 @@ MonoBehaviour: m_Script: {fileID: 13964, guid: 0000000000000000e000000000000000, type: 0} m_Name: m_EditorClassIdentifier: - m_EnablePreviewPackages: 0 + m_EnablePreReleasePackages: 0 m_EnablePackageDependencies: 0 m_AdvancedSettingsExpanded: 1 m_ScopedRegistriesSettingsExpanded: 1 + m_SeeAllPackageVersions: 0 oneTimeWarningShown: 0 m_Registries: - m_Id: main @@ -24,20 +25,12 @@ MonoBehaviour: m_Scopes: [] m_IsDefault: 1 m_Capabilities: 7 + m_ConfigSource: 0 m_UserSelectedRegistryName: m_UserAddingNewScopedRegistry: 0 m_RegistryInfoDraft: - m_ErrorMessage: - m_Original: - m_Id: - m_Name: - m_Url: - m_Scopes: [] - m_IsDefault: 0 - m_Capabilities: 0 m_Modified: 0 - m_Name: - m_Url: - m_Scopes: - - - m_SelectedScopeIndex: 0 + m_ErrorMessage: + m_UserModificationsInstanceId: -826 + m_OriginalInstanceId: -828 + m_LoadAssets: 0 diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index eabd6337..5a4e8e97 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ m_EditorVersion: 2021.3.15f1 -m_EditorVersionWithRevision: 2021.3.15f1 (e8e88683f834) +m_EditorVersionWithRevision: 2021.3.15f1 (e8e88683f834) \ No newline at end of file diff --git a/README.md b/README.md index b3c3d854..c8be7a4e 100644 --- a/README.md +++ b/README.md @@ -1,91 +1,43 @@ -# UnitySensors +## UnitySensors [![Acquire activation file](https://github.com/Field-Robotics-Japan/UnitySensorsROSAssets/workflows/Acquire%20activation%20file/badge.svg)](https://github.com/Field-Robotics-Japan/UnitySensorsROSAssets/actions?query=workflow%3A%22Acquire+activation+file%22) [![CI](https://github.com/Field-Robotics-Japan/UnitySensorsROSAssets/workflows/CI/badge.svg)](https://github.com/Field-Robotics-Japan/UnitySensorsROSAssets/actions?query=workflow%3ACI) ![unity_sensors_ros_assets](.image/unity_sensors_ros_assets.gif) + +## Overview Robot sensor packages available on Unity. You can communicate sensor info via ROS and ROS2 using ROSTCPConnector and ROSTCPEndpoint. The following sensors are added. -- 3D LiDAR (Velodyne VLP-16) -- 2D LiDAR (Hokuyo UST-30LX) -- RGB Camera (Logitech C910) +- Velodyne 3D LiDAR (Velodyne VLP-16, VLP-16-HiRes, VLP-32) +- Livox 3D LiDAR(Avia, Horizon, Mid40, Mid70, Tele) +- RGB Camera +- RGBD Camera - IMU +- GNSS +- (GroundTruth) +- (TF) There are several Prefab and Scene files available for testing each sensor. -Check [this directory](https://github.com/Field-Robotics-Japan/sensors_unity/tree/develop/Assets/Scenes). - -# How to use (ROS) -## 1. Launch ROS packages -### 1-1 ros_tcp_endpoint -Launch the `ros_tcp_endpoint` with following command. -```bash -roslaunch ros_tcp_endpoint endpoint.launch -``` - -### 1-2 Sensors -#### 1-2-1 velodyne_pointcloud -Launch the `velodyne_pointcloud` package with following launch file. -Please create launch file by copy and paste following script. -**The version of `velodyne_pointcloud` package should be later than 1.6.0.** -If you fail to lanch following launch file, please download velodyne package directoly into your workspace, build it, and launch them again. -https://github.com/ros-drivers/velodyne -```xml - - - - - - - - - - - - - - - - - -``` - -#### 1-2-2 RGB Camera -Image communication between Unity and ROS is done using JPEG compression. -You can set the topic name in the Unity inspector window. - -**Note** : -Message type is sensor_msgs/CompressedImage. -So, the topic name must end with "/compressed". -In order to handle sensor_msgs/CompressedImage, -launch the `image_transport` package with following launch file. -Please create launch file by copy and paste following script. - -```xml - - - - - - -``` -If you want to check the images in Rviz, you don't need to use the above launch file. +## Dependencies +- [RosTCPConnector](https://github.com/Unity-Technologies/ROS-TCP-Connector) (Appache 2.0 LICENSE) -#### 1-2-3 IMU -It can be used in the same way as a normal IMU sensor. +## Package Installation (For using UnitySensors in your project) +1. Using Unity 2021.3 or later, open the Package Manager from `Window` -> `Package Manager`. +2. In the Package Manager window, find and click the + button in the upper lefthand corner of the window. Select `Add package from git URL....` -## 2. Clone and run on Unity -1. Just to clone this repo with `git clone --recursive https://github.com/Field-Robotics-Japan/UnitySensorsROSAssets.git` command. -1. Then, open the project file with UnityHub. -1. Finally, run the scene file for the sensor you want to test. +3. Enter the git URL for the desired package. + 1. For the UnitySensors, enter `https://github.com/Field-Robotics-Japan/UnitySensors.git?path=/Assets/UnitySensors#v1.0b`. + 2. For the UnitySensorsROS, enter `https://github.com/Field-Robotics-Japan/UnitySensors.git?path=/Assets/UnitySensorsROS#v1.0b`. + __Note: UnitySensorsROS does not contain UnitySensors.__ +4. Click `Add`. -You can try all the sensors in the "`Sensors`" scene file. +## Documentation -# LICENSE +## LICENSE Copyright [2020-2021] Ryodo Tanaka groadpg@gmail.com Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at @@ -93,6 +45,3 @@ Licensed under the Apache License, Version 2.0 (the "License"); you may not use http://www.apache.org/licenses/LICENSE-2.0 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. - -## Dependencies -- [RosTCPConnector](https://github.com/Unity-Technologies/ROS-TCP-Connector) (Appache 2.0 LICENSE)