diff --git a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta index d5811923d..622d77450 100644 --- a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta +++ b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanCasual.fbx.meta @@ -253,7 +253,7 @@ ModelImporter: maskType: 3 maskSource: {instanceID: 0} additiveReferencePoseFrame: 0 - isReadable: 0 + isReadable: 1 meshes: lODScreenPercentages: - 0.25 diff --git a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta index 9a61ac6e1..b6518d4c7 100644 --- a/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta +++ b/Assets/AWSIM/Models/NPCs/Pedestrians/Human/humanElegant.fbx.meta @@ -253,7 +253,7 @@ ModelImporter: maskType: 3 maskSource: {instanceID: 0} additiveReferencePoseFrame: 0 - isReadable: 0 + isReadable: 1 meshes: lODScreenPercentages: - 0.25 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab index e88a1e26d..dbf1b4a3f 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab @@ -280,10 +280,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 8 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab index 1077dc924..b0326872b 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab @@ -183,10 +183,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 11 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab index 112fe1339..d1a4da892 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab @@ -509,10 +509,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 6 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab index c9a003f26..ebe2715ef 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab @@ -412,10 +412,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 5 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab index d9fa0b322..df4802b4f 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab @@ -280,10 +280,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 9 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab index 6d8c72806..389423f5d 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab @@ -101,10 +101,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 10 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab index d5c330224..fbec3d587 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab @@ -100,10 +100,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 20 modelPreset: 7 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab index d36b981e6..40ca77d5b 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab @@ -101,10 +101,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 2 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab index 1b1df1104..f1441aa36 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab @@ -101,10 +101,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 3 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab index e12d9d657..51f05d0a6 100644 --- a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab +++ b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab @@ -509,10 +509,11 @@ MonoBehaviour: m_EditorClassIdentifier: AutomaticCaptureHz: 10 modelPreset: 4 - returnType: 0 + returnMode: 16777220 applyDistanceGaussianNoise: 1 applyAngularGaussianNoise: 1 applyVelocityDistortion: 0 + simulateBeamDivergence: 0 doValidateConfigurationOnStartup: 1 configuration: id: 0 diff --git a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so index 316bb5a5a..5bce7f6f5 100755 Binary files a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so and b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so differ diff --git a/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll b/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll index 5d7474240..5e3334e6f 100644 Binary files a/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll and b/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll differ diff --git a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs index 2b57731b8..fae5cd974 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs @@ -46,8 +46,8 @@ public class LidarSensor : MonoBehaviour [Tooltip("Allows to select one of built-in LiDAR models")] public LidarModel modelPreset = LidarModel.RangeMeter; - [Tooltip("Allows to select between not divergent beams and different multi-return modes")] - public RGLReturnType returnType = RGLReturnType.RGL_RETURN_TYPE_NOT_DIVERGENT; + [Tooltip("Allows to select between LiDAR return modes")] + public RGLReturnMode returnMode = RGLReturnMode.SingleReturnFirst; [Tooltip("Allows to quickly enable/disable distance gaussian noise")] public bool applyDistanceGaussianNoise = true; @@ -58,6 +58,9 @@ public class LidarSensor : MonoBehaviour [Tooltip("Allows to quickly enable/disable velocity distortion")] public bool applyVelocityDistortion = false; + [Tooltip("If disable, both beam divergence values are set to 0. Otherwise, they are set based on LiDAR configuration.")] + public bool simulateBeamDivergence = false; + [Tooltip( "If enabled, validates whether the configuration is the same as the manual for the selected model (only on startup)")] public bool doValidateConfigurationOnStartup = true; @@ -85,7 +88,6 @@ public class LidarSensor : MonoBehaviour private const string lidarPoseNodeId = "LIDAR_POSE"; private const string noiseLidarRayNodeId = "NOISE_LIDAR_RAY"; private const string lidarRaytraceNodeId = "LIDAR_RAYTRACE"; - private const string lidarMRNodeId = "LIDAR_MR"; private const string noiseHitpointNodeId = "NOISE_HITPOINT"; private const string noiseDistanceNodeId = "NOISE_DISTANCE"; private const string pointsCompactNodeId = "POINTS_COMPACT"; @@ -113,7 +115,6 @@ public void Awake() .AddNodeRaysTransform(lidarPoseNodeId, Matrix4x4.identity) .AddNodeGaussianNoiseAngularRay(noiseLidarRayNodeId, 0, 0) .AddNodeRaytrace(lidarRaytraceNodeId) - .AddNodeMultiReturnSwitch(lidarMRNodeId, RGLReturnType.RGL_RETURN_TYPE_NOT_DIVERGENT) .AddNodeGaussianNoiseAngularHitpoint(noiseHitpointNodeId, 0, 0) .AddNodeGaussianNoiseDistance(noiseDistanceNodeId, 0, 0, 0); @@ -129,7 +130,7 @@ public void Awake() public void Start() { - sceneManager = FindObjectOfType(); + sceneManager = SceneManager.Instance; if (sceneManager == null) { // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager @@ -144,8 +145,7 @@ public void Start() if (LidarSnowManager.Instance != null) { // Add deactivated node with some initial values. To be activated and updated when validating. - rglGraphLidar.AddNodePointsSimulateSnow(snowNodeId, 0.0f, 1.0f, 0.0001f, 0.0001f, 0.2f, 0.01f, 1, 0.01f, - false, 0.0f); + rglGraphLidar.AddNodePointsSimulateSnow(snowNodeId, 0.0f, 1.0f, 0.0001f, 0.0001f, 0.2f, 0.01f, 1, 0.01f, 0.0f); rglGraphLidar.SetActive(snowNodeId, false); LidarSnowManager.Instance.OnNewConfig += OnValidate; } @@ -199,10 +199,9 @@ private void ApplyConfiguration(BaseLidarConfiguration newConfig) newConfig.noiseParams.angularNoiseMean * Mathf.Deg2Rad, newConfig.noiseParams.angularNoiseStDev * Mathf.Deg2Rad) .UpdateNodeGaussianNoiseDistance(noiseDistanceNodeId, newConfig.noiseParams.distanceNoiseMean, - newConfig.noiseParams.distanceNoiseStDevBase, newConfig.noiseParams.distanceNoiseStDevRisePerMeter) - .UpdateMultiReturnSwitch(lidarMRNodeId, returnType); + newConfig.noiseParams.distanceNoiseStDevBase, newConfig.noiseParams.distanceNoiseStDevRisePerMeter); - if (returnType != RGLReturnType.RGL_RETURN_TYPE_NOT_DIVERGENT) + if (simulateBeamDivergence) { rglGraphLidar.ConfigureNodeRaytraceBeamDivergence(lidarRaytraceNodeId, Mathf.Deg2Rad * newConfig.horizontalBeamDivergence, @@ -212,6 +211,8 @@ private void ApplyConfiguration(BaseLidarConfiguration newConfig) { rglGraphLidar.ConfigureNodeRaytraceBeamDivergence(lidarRaytraceNodeId, 0.0f, 0.0f); } + + rglGraphLidar.ConfigureNodeRaytraceReturnMode(lidarRaytraceNodeId, returnMode); rglGraphLidar.SetActive(noiseDistanceNodeId, applyDistanceGaussianNoise); var angularNoiseType = newConfig.noiseParams.angularNoiseType; @@ -235,8 +236,11 @@ private void ApplyConfiguration(BaseLidarConfiguration newConfig) LidarSnowManager.Instance.Density, newConfig.laserArray.GetLaserRingIds().Length, newConfig.horizontalBeamDivergence * Mathf.Deg2Rad, - LidarSnowManager.Instance.DoSimulateEnergyLoss, - LidarSnowManager.Instance.SnowflakeOccupancyThreshold); + LidarSnowManager.Instance.OccupancyThreshold); + rglGraphLidar.UpdateNodePointsSnowDefaults(snowNodeId, + LidarSnowManager.Instance.SnowflakesId, + LidarSnowManager.Instance.FullBeamIntensity, + 0.0f); // Default, because it is not supported in AWSIM. } rglGraphLidar.SetActive(snowNodeId, LidarSnowManager.Instance.IsSnowEnabled); @@ -263,6 +267,11 @@ private void ApplyConfiguration(BaseLidarConfiguration newConfig) public void OnEnable() { activeSensors.Add(this); + // Sync timer with the active sensors to achieve the best performance. It minimizes number of scene updates. + if (activeSensors.Count > 0) + { + timer = activeSensors[0].timer; + } } public void OnDisable() diff --git a/Assets/RGLUnityPlugin/Scripts/LidarSnowManager.cs b/Assets/RGLUnityPlugin/Scripts/LidarSnowManager.cs index b66ad889f..8c0b08bae 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarSnowManager.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarSnowManager.cs @@ -29,6 +29,11 @@ public class LidarSnowManager : MonoBehaviour public delegate void OnNewConfigDelegate(); public OnNewConfigDelegate OnNewConfig; + [field: Header("Base Settings")] + + [field: SerializeField] + public bool IsSnowEnabled { get; private set; } = false; + // Snow model properties [field: SerializeField] [field: Tooltip("The precipitation rate for snow is expressed in rate of equivalent water depth in mm per hour")] @@ -51,15 +56,20 @@ public class LidarSnowManager : MonoBehaviour public float Density { get; private set; } = 0.07f; [field: SerializeField] - [field: Tooltip("If true, a more sophisticated method is used, which takes into account the energy loss of the lidar beam when hitting snowflakes")] - public bool DoSimulateEnergyLoss { get; private set; } = true; - - [field: SerializeField] - [field: Tooltip("Minimal snowflake occupancy (in fraction of ray beam angle) included in energy loss calculation")] + [field: Tooltip("Minimal beam aperture occupancy (ratio) that means a hit, both for snowflakes and for original hit")] [field: Range(0.0f, 1.0f)] - public float SnowflakeOccupancyThreshold { get; private set; } = 0.0f; + public float OccupancyThreshold { get; private set; } = 0.0f; - public bool IsSnowEnabled { get; private set; } = false; + [field: Header("Defaults")] + + [field: SerializeField] + [field: Tooltip("Entity ID that is assigned to cloud points resulting from snowflake hits")] + public int SnowflakesId { get; private set; } = 268435455; // Default RGL entity ID. + + [field: SerializeField] + [field: Tooltip("Initial intensity of each LiDAR laser beam, used to evaluate energy loss based on beam aperture occupancy")] + [field: Min(0.0f)] + public float FullBeamIntensity { get; private set; } = 1.0f; private void Awake() { diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs new file mode 100644 index 000000000..0a64fff9b --- /dev/null +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs @@ -0,0 +1,112 @@ +// Copyright 2024 Robotec.ai. +// +// 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 +// +// http://www.apache.org/licenses/LICENSE2.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. + +using System; +using System.Collections.Generic; +using UnityEngine; + +namespace RGLUnityPlugin +{ + /// + /// Since multiple skinned meshes can share the same skeleton, BonesPoseCacheManager improves the performance of the simulation + /// by caching the calculated pose of the skeletons for the given simulation step. + /// + public static class BonesPoseCacheManager + { + private class BonesPose + { + public BonesPose(SkinnedMeshRenderer smr) + { + this.smr = smr; + var bonesCount = smr.bones.Length; + pose = new Matrix4x4[bonesCount]; + rglPose = new float[bonesCount * 3 * 4]; // Mat3x4 == 12 floats + usageCount = 1; + } + + public SkinnedMeshRenderer smr; // needed to retrieve the current skeleton's pose + public Matrix4x4[] pose; // buffer for the skeleton's pose in the Unity representation + public float[] rglPose; // buffer for the skeleton's pose in the RGL representation + public int usageCount; // the counter on how many meshes use this skeleton + public int lastUpdateFrame = -1; // simulation frame + public int lastFixedUpdateFrame = -1; // physics cycle in the simulation frame + } + + // The bone root acts as the identifier of the skeleton. + private static Dictionary boneRootToBonesPose = new Dictionary(); + + public static void RegisterBonesPoseInstance(SkinnedMeshRenderer smr) + { + if (!boneRootToBonesPose.ContainsKey(smr.rootBone)) + { + boneRootToBonesPose.Add(smr.rootBone, new BonesPose(smr)); + } + else + { + boneRootToBonesPose[smr.rootBone].usageCount++; + } + } + + public static void UnregisterBonesPoseInstance(Transform rootBone) + { + if (!boneRootToBonesPose.ContainsKey(rootBone)) + { + Debug.LogWarning( + $"Trying to unregister absent in BonesPoseCacheManager rootBone: '{rootBone.name}', ignoring request"); + return; + } + + var bonesPose = boneRootToBonesPose[rootBone]; + bonesPose.usageCount--; + if (bonesPose.usageCount == 0) + { + boneRootToBonesPose.Remove(rootBone); + } + } + + public static float[] GetRglPose(Transform rootBone) + { + if (!boneRootToBonesPose.ContainsKey(rootBone)) + { + throw new NotSupportedException( + $"Trying to get RglPose from root bone ('{rootBone.name}') that is not registered in BonesPoseCacheManager"); + } + + var bonesPose = boneRootToBonesPose[rootBone]; + + if (SceneManager.Instance.LastUpdateFrame == bonesPose.lastUpdateFrame && + SceneManager.Instance.LastFixedUpdateFrame == bonesPose.lastFixedUpdateFrame) + { + return bonesPose.rglPose; + } + + var bones = bonesPose.smr.bones; + for (int i = 0; i < bonesPose.pose.Length; i++) + { + bonesPose.pose[i] = bones[i].localToWorldMatrix; + } + + RGLNativeAPI.IntoMat3x4f(bonesPose.pose, ref bonesPose.rglPose); + + bonesPose.lastUpdateFrame = SceneManager.Instance.LastUpdateFrame; + bonesPose.lastFixedUpdateFrame = SceneManager.Instance.LastFixedUpdateFrame; + return bonesPose.rglPose; + } + + public static void Clear() + { + boneRootToBonesPose.Clear(); + } + } +} diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta new file mode 100644 index 000000000..e16483dd9 --- /dev/null +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/BonesPoseCacheManager.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: f2d073eeb8da43b6be986a5e5b65fb51 +timeCreated: 1723636067 \ No newline at end of file diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs index 649a24a1e..9b342fb68 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs @@ -41,6 +41,5 @@ public enum RGLNodeType GAUSSIAN_NOISE_ANGULAR_HITPOINT, GAUSSIAN_NOISE_DISTANCE, PUBLISH_ROS2_RADARSCAN, - MULTI_RETURN_SWITCH, }; } diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs index 56cf6e4cd..32d20e0f0 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLMeshObject.cs @@ -79,15 +79,15 @@ public abstract class RGLObject : IRGLObject { private readonly string identifier; private RGLTexture rglTexture; - private IntPtr rglEntityPtr; + protected IntPtr rglEntityPtr; protected RGLMesh rglMesh; public GameObject RepresentedGO { get; } public int? CategoryId { get; private set; } public string CategoryName { get; private set; } - // There are different stratiegies for obtaining a RGLMesh so we have to also destroy it differently. + // There are different strategies for obtaining a RGLMesh so we have to also destroy it differently. protected abstract RGLMesh GetRGLMeshFrom(T meshSource); protected abstract void DestroyRGLMesh(); @@ -134,13 +134,9 @@ public virtual void DestroyInRGL() } } - public void Update() + public virtual void Update() { UpdateTransform(); - if (rglMesh is RGLSkinnedMesh rglSkinnedMesh) - { - rglSkinnedMesh.UpdateSkinnedMesh(); - } } protected virtual void UpdateTransform() @@ -159,7 +155,7 @@ protected virtual void UpdateTransform() fixed (float* pMatrix3x4 = matrix3x4) { RGLNativeAPI.CheckErr( - RGLNativeAPI.rgl_entity_set_pose(rglEntityPtr, (IntPtr) pMatrix3x4)); + RGLNativeAPI.rgl_entity_set_transform(rglEntityPtr, (IntPtr) pMatrix3x4)); } } } @@ -278,6 +274,8 @@ protected override void DestroyRGLMesh() public class RGLSkinnedMeshRendererObject : RGLObject { private readonly Transform skinnedMeshRendererTransform; + private readonly Transform rootBone; + private readonly int bonesCount; public RGLSkinnedMeshRendererObject(SkinnedMeshRenderer skinnedMeshRenderer) : base( @@ -287,6 +285,11 @@ public RGLSkinnedMeshRendererObject(SkinnedMeshRenderer skinnedMeshRenderer) : ) { skinnedMeshRendererTransform = skinnedMeshRenderer.transform; + rootBone = skinnedMeshRenderer.rootBone; // Identifier of the skeleton. + bonesCount = skinnedMeshRenderer.bones.Length; + // Multiple skinned meshes can share the same skeleton. + // To improve performance BonesPoseCacheManager will cache the last calculated pose of the skeletons. + BonesPoseCacheManager.RegisterBonesPoseInstance(skinnedMeshRenderer); } protected override RGLMesh GetRGLMeshFrom(SkinnedMeshRenderer skinnedMeshRenderer) @@ -295,18 +298,41 @@ protected override RGLMesh GetRGLMeshFrom(SkinnedMeshRenderer skinnedMeshRendere { throw new NotSupportedException($"Shared skinned mesh of '{skinnedMeshRenderer.gameObject}' is null"); } - // Skinned meshes cannot be shared by using RGLMeshSharingManager - return new RGLSkinnedMesh(skinnedMeshRenderer.gameObject.GetInstanceID(), skinnedMeshRenderer); + var outRglMesh = RGLMeshSharingManager.RegisterRGLMeshInstance(skinnedMeshRenderer.sharedMesh); + // Bone weights and bindposes need to be uploaded to perform skeleton animation. + outRglMesh.UploadBoneWeights(); + outRglMesh.UploadBindposes(); + return outRglMesh; + } + + // Override base class Update() method that sets world's transform. + // Instead, set the pose of the skeleton in the world coordinates. RGL will perform skeleton animation on GPU. + public override void Update() + { + unsafe + { + fixed (float* pPoseFloats = BonesPoseCacheManager.GetRglPose(rootBone)) + { + RGLNativeAPI.CheckErr( + RGLNativeAPI.rgl_entity_set_pose_world(rglEntityPtr, (IntPtr)pPoseFloats, bonesCount)); + } + } } protected override Matrix4x4 GetLocalToWorld() { return skinnedMeshRendererTransform.localToWorldMatrix; } - + + public override void DestroyInRGL() + { + base.DestroyInRGL(); + BonesPoseCacheManager.UnregisterBonesPoseInstance(rootBone); + } + protected override void DestroyRGLMesh() { - rglMesh.DestroyInRGL(); + RGLMeshSharingManager.UnregisterRGLMeshInstance(rglMesh); } } @@ -536,38 +562,44 @@ public void UploadUVs() } } } - } - /// - /// Some objects (such as NPC) use skinned meshes, which needs to be constantly updated by the Unity side. - /// - public class RGLSkinnedMesh : RGLMesh - { - private readonly SkinnedMeshRenderer skinnedMeshRenderer; - - public RGLSkinnedMesh(int identifier, SkinnedMeshRenderer smr) + public void UploadBoneWeights() { - Identifier = identifier; - Mesh = new Mesh(); - skinnedMeshRenderer = smr; - skinnedMeshRenderer.BakeMesh(Mesh, true); - UploadToRGL(); + var boneWeights = Mesh.boneWeights; + bool boneWeightsOK = boneWeights != null && boneWeights.Length > 0; + if (!boneWeightsOK) + { + throw new NotSupportedException( + $"Could not get bone weights from Mesh '{Mesh.name}'. The mesh may be not adapted for animation."); + } + + unsafe + { + fixed (BoneWeight* pBoneWeights = boneWeights) + { + RGLNativeAPI.CheckErr( + RGLNativeAPI.rgl_mesh_set_bone_weights(RGLMeshPtr, (IntPtr) pBoneWeights, Mesh.boneWeights.Length)); + } + } } - public void UpdateSkinnedMesh() + public void UploadBindposes() { - skinnedMeshRenderer.BakeMesh(Mesh, true); + var bindposes = Mesh.bindposes; + bool bindposesOK = bindposes != null && bindposes.Length > 0; + if (!bindposesOK) + { + throw new NotSupportedException( + $"Could not get bindposes from Mesh '{Mesh.name}'. The mesh may be not adapted for animation."); + } + + var bindposesFloats = RGLNativeAPI.IntoMat3x4f(bindposes); unsafe { - // Accessing .vertices perform a CPU copy! - // TODO: This could be optimized using Vulkan-CUDA interop and Unity NativePluginInterface. Expect difficulties. - // https://docs.unity3d.com/ScriptReference/Mesh.GetNativeVertexBufferPtr.html - // https://docs.unity3d.com/Manual/NativePluginInterface.html - // https://github.com/NVIDIA/cuda-samples/tree/master/Samples/5_Domain_Specific/simpleVulkan - fixed (Vector3* pVertices = Mesh.vertices) + fixed (float* pBindposesFloats = bindposesFloats) { RGLNativeAPI.CheckErr( - RGLNativeAPI.rgl_mesh_update_vertices(RGLMeshPtr, (IntPtr) pVertices, Mesh.vertices.Length)); + RGLNativeAPI.rgl_mesh_set_restposes(RGLMeshPtr, (IntPtr) pBindposesFloats, Mesh.bindposes.Length)); } } } diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs index 7b6068e46..9c050e9c2 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs @@ -40,10 +40,13 @@ public static class RGLNativeAPI public static extern int rgl_mesh_set_texture_coords(IntPtr mesh, IntPtr uvs, int uvCount); [DllImport("RobotecGPULidar")] - public static extern int rgl_mesh_destroy(IntPtr mesh); + public static extern int rgl_mesh_set_bone_weights(IntPtr mesh, IntPtr bone_weights, int bone_weights_count); + + [DllImport("RobotecGPULidar")] + public static extern int rgl_mesh_set_restposes(IntPtr mesh, IntPtr restposes, int bones_count); [DllImport("RobotecGPULidar")] - public static extern int rgl_mesh_update_vertices(IntPtr mesh, IntPtr vertices, int vertex_count); + public static extern int rgl_mesh_destroy(IntPtr mesh); [DllImport("RobotecGPULidar")] public static extern int rgl_entity_create(out IntPtr entity, IntPtr scene, IntPtr mesh); @@ -52,7 +55,10 @@ public static class RGLNativeAPI public static extern int rgl_entity_destroy(IntPtr entity); [DllImport("RobotecGPULidar")] - public static extern int rgl_entity_set_pose(IntPtr entity, IntPtr local_to_world_tf); + public static extern int rgl_entity_set_transform(IntPtr entity, IntPtr local_to_world_tf); + + [DllImport("RobotecGPULidar")] + public static extern int rgl_entity_set_pose_world(IntPtr entity, IntPtr pose, int bones_count); [DllImport("RobotecGPULidar")] public static extern int rgl_entity_set_id(IntPtr entity, int id); @@ -60,6 +66,9 @@ public static class RGLNativeAPI [DllImport("RobotecGPULidar")] public static extern int rgl_entity_set_intensity_texture(IntPtr entity, IntPtr texture); + [DllImport("RobotecGPULidar")] + public static extern int rgl_entity_apply_external_animation(IntPtr entity, IntPtr vertices, int vertex_count); + [DllImport("RobotecGPULidar")] public static extern int rgl_texture_create(out IntPtr texture, IntPtr texels, int width, int height); @@ -102,6 +111,9 @@ public static class RGLNativeAPI [DllImport("RobotecGPULidar")] public static extern int rgl_node_raytrace_configure_beam_divergence(IntPtr node, float horizontal_divergence, float vertical_divergence); + [DllImport("RobotecGPULidar")] + public static extern int rgl_node_raytrace_configure_return_mode(IntPtr node, RGLReturnMode return_mode); + [DllImport("RobotecGPULidar")] public static extern int rgl_node_points_format(ref IntPtr node, IntPtr fields, int field_count); @@ -140,10 +152,6 @@ public static extern int rgl_node_points_udp_publish( [DllImport("RobotecGPULidar")] public static extern int rgl_node_gaussian_noise_distance(ref IntPtr node, float mean, float st_dev, float st_dev_rise_per_meter); - - [DllImport("RobotecGPULidar")] - public static extern int rgl_node_multi_return_switch(ref IntPtr node, RGLReturnType return_type); - [DllImport("RobotecGPULidar")] public static extern int rgl_node_points_filter_ground(ref IntPtr node, IntPtr sensor_up_vector, float ground_angle_threshold); @@ -168,7 +176,11 @@ public static extern int rgl_node_publish_udp_objectlist(ref IntPtr node, [Marsh [DllImport("RobotecGPULidar")] public static extern int rgl_node_points_simulate_snow(ref IntPtr node, float min_range, float max_range, float rain_rate, float mean_snowflake_diameter, float terminal_velocity, float density, Int32 num_channels, float beam_divergence, - bool simulate_energy_loss, float snowflake_occupancy_threshold); + float occupancy_threshold); + + [DllImport("RobotecGPULidar")] + public static extern int rgl_node_points_simulate_snow_configure_defaults(IntPtr node, int snowflakes_id, float full_beam_intensity, + float snowflakes_laser_retro); [DllImport("RobotecGPULidar")] public static extern int rgl_graph_run(IntPtr node); @@ -322,6 +334,16 @@ public static float[] IntoVec3f(Vector3 vec) public static float[] IntoMat3x4f(Matrix4x4[] mats) { var matFloats = new float[mats.Length * 12]; + IntoMat3x4f(mats, ref matFloats); + return matFloats; + } + + public static void IntoMat3x4f(Matrix4x4[] mats, ref float[] outFloats) + { + if (outFloats == null || outFloats.Length != mats.Length * 12) + { + outFloats = new float[mats.Length * 12]; + } for (int i = 0; i < mats.Length; ++i) { @@ -330,12 +352,10 @@ public static float[] IntoMat3x4f(Matrix4x4[] mats) for (int col = 0; col < 4; col++) { int idx = 12 * i + 4 * row + col; - matFloats[idx] = mats[i][row, col]; + outFloats[idx] = mats[i][row, col]; } } } - - return matFloats; } public static float[] IntoMat3x4f(Matrix4x4 mat) @@ -468,6 +488,11 @@ public static void NodeRaytraceConfigureBeamDivergence(IntPtr node, float horizo CheckErr(rgl_node_raytrace_configure_beam_divergence(node, horizontalDivergence, verticalDivergence)); } + public static void NodeRaytraceConfigureReturnMode(IntPtr node, RGLReturnMode returnMode) + { + CheckErr(rgl_node_raytrace_configure_return_mode(node, returnMode)); + } + public static void NodePointsFormat(ref IntPtr node, RGLField[] fields) { unsafe @@ -545,11 +570,6 @@ public static void NodeGaussianNoiseDistance(ref IntPtr node, float mean, float CheckErr(rgl_node_gaussian_noise_distance(ref node, mean, stDev, stDevRisePerMeter)); } - public static void NodeMultiReturnSwitch(ref IntPtr node, RGLReturnType return_type) - { - CheckErr(rgl_node_multi_return_switch(ref node, return_type)); - } - public static void NodePointsFilterGround(ref IntPtr node, float groundAngleThreshold) { var upVector = IntoVec3f(new Vector3(0, 1, 0)); @@ -620,10 +640,16 @@ public static void NodePublishUdpObjectList(ref IntPtr node, string deviceIp, st public static void NodePointsSimulateSnow(ref IntPtr node, float minRange, float maxRange, float rainRate, float meanSnowflakeDiameter, float terminalVelocity, float density, Int32 numChannels, float beamDivergence, - bool doSimulateEnergyLoss, float snowflakeOccupancyThreshold) + float occupancyThreshold) { CheckErr(rgl_node_points_simulate_snow(ref node, minRange, maxRange, rainRate, - meanSnowflakeDiameter, terminalVelocity, density, numChannels, beamDivergence, doSimulateEnergyLoss, snowflakeOccupancyThreshold)); + meanSnowflakeDiameter, terminalVelocity, density, numChannels, beamDivergence, occupancyThreshold)); + } + + public static void NodePointsSimulateSnowConfigureDefaults(IntPtr node, int snowflakesId, float fullBeamIntensity, + float snowflakesLaserRetro) + { + CheckErr(rgl_node_points_simulate_snow_configure_defaults(node, snowflakesId, fullBeamIntensity, snowflakesLaserRetro)); } public static void GraphRun(IntPtr node) diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs index fa405fab7..80ff042e7 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs @@ -99,18 +99,33 @@ public enum RGLLidarModel : Int32 RGL_HESAI_PANDAR_XT32 = 8, }; + public enum RGLReturnType : Int32 + { + ReturnStrongest = 1, + ReturnLast = 2, + ReturnSecond = 3, + ReturnFirst = 4, + ReturnSecondStrongest = 5, + } + + public enum RGLReturnCount : Int32 + { + SingleReturn = 1 << 24, + DualReturn = 2 << 24, + } + // Items have been renamed to be displayed in Unity nicer. - public enum RGLReturnMode : UInt32 + public enum RGLReturnMode : Int32 { - /* RGL_RETURN_FIRST */ SingleReturnFirst = 1 << 29, // Three of the most significant bits encode the number of returns - /* RGL_RETURN_SECOND */ SingleReturnSecond, - /* RGL_RETURN_LAST */ SingleReturnLast, - /* RGL_RETURN_STRONGEST */ SingleReturnStrongest, - /* RGL_RETURN_LAST_STRONGEST */ DualReturnLastStrongest = 2 << 29, - /* RGL_RETURN_FIRST_LAST */ DualReturnFirstLast, - /* RGL_RETURN_FIRST_STRONGEST */ DualReturnFirstStrongest, - /* RGL_RETURN_STRONGEST_SECOND_STRONGEST */ DualReturnStrongestSecondStrongest, - /* RGL_RETURN_FIRST_SECOND */ DualReturnFirstSecond, + /* RGL_RETURN_FIRST */ SingleReturnFirst = RGLReturnCount.SingleReturn | RGLReturnType.ReturnFirst, + /* RGL_RETURN_SECOND */ SingleReturnSecond = RGLReturnCount.SingleReturn | RGLReturnType.ReturnSecond, + /* RGL_RETURN_LAST */ SingleReturnLast = RGLReturnCount.SingleReturn | RGLReturnType.ReturnLast, + /* RGL_RETURN_STRONGEST */ SingleReturnStrongest = RGLReturnCount.SingleReturn | RGLReturnType.ReturnStrongest, + /* RGL_RETURN_LAST_STRONGEST */ DualReturnLastStrongest = RGLReturnCount.DualReturn | RGLReturnType.ReturnLast | (RGLReturnType.ReturnStrongest << 8), + /* RGL_RETURN_FIRST_LAST */ DualReturnFirstLast = RGLReturnCount.DualReturn | RGLReturnType.ReturnFirst | (RGLReturnType.ReturnLast << 8), + /* RGL_RETURN_FIRST_STRONGEST */ DualReturnFirstStrongest = RGLReturnCount.DualReturn | RGLReturnType.ReturnFirst | (RGLReturnType.ReturnStrongest << 8), + /* RGL_RETURN_STRONGEST_SECOND_STRONGEST */ DualReturnStrongestSecondStrongest = RGLReturnCount.DualReturn | RGLReturnType.ReturnStrongest | (RGLReturnType.ReturnSecondStrongest << 8), + /* RGL_RETURN_FIRST_SECOND */ DualReturnFirstSecond = RGLReturnCount.DualReturn | RGLReturnType.ReturnFirst | (RGLReturnType.ReturnSecond << 8), }; // Items have been renamed to be displayed in Unity nicer. @@ -163,11 +178,4 @@ public enum RGLExtension : Int32 RGL_EXTENSION_SNOW = 3, RGL_EXTENSION_COUNT }; - - public enum RGLReturnType : Int32 - { - RGL_RETURN_TYPE_NOT_DIVERGENT = 0, - RGL_RETURN_TYPE_FIRST = 1, - RGL_RETURN_TYPE_LAST = 2, - } } diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs index 1043f9597..6a8ac1cd6 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs @@ -338,30 +338,18 @@ public RGLNodeSequence AddNodePublishUdpObjectList(string identifier, string dev public RGLNodeSequence AddNodePointsSimulateSnow(string identifier, float minRange, float maxRange, float rainRate, float meanSnowflakeDiameter, float terminalVelocity, float density, Int32 numChannels, float beamDivergence, - bool doSimulateEnergyLoss, float snowflakeOccupancyThreshold) + float occupancyThreshold) { CheckNodeNotExist(identifier); RGLNodeHandle handle = new RGLNodeHandle(); - RGLNativeAPI.NodePointsSimulateSnow(ref handle.Node, minRange, maxRange, rainRate, - meanSnowflakeDiameter, terminalVelocity, density, numChannels, beamDivergence, doSimulateEnergyLoss, - snowflakeOccupancyThreshold); + RGLNativeAPI.NodePointsSimulateSnow(ref handle.Node, minRange, maxRange, rainRate, meanSnowflakeDiameter, + terminalVelocity, density, numChannels, beamDivergence, occupancyThreshold); handle.Type = RGLNodeType.POINTS_SIMULATE_SNOW; handle.Identifier = identifier; AddNode(handle); return this; } - public RGLNodeSequence AddNodeMultiReturnSwitch(string identifier, RGLReturnType returnType) - { - CheckNodeNotExist(identifier); - RGLNodeHandle handle = new RGLNodeHandle(); - RGLNativeAPI.NodeMultiReturnSwitch(ref handle.Node, returnType); - handle.Type = RGLNodeType.MULTI_RETURN_SWITCH; - handle.Identifier = identifier; - AddNode(handle); - return this; - } - //// UPDATE NODES //// public RGLNodeSequence UpdateNodeRaysFromMat3x4f(string identifier, Matrix4x4[] rays) { @@ -483,19 +471,18 @@ public RGLNodeSequence UpdateNodePointsRadarTrackObjects(string identifier, floa public RGLNodeSequence UpdateNodePointsSimulateSnow(string identifier, float minRange, float maxRange, float rainRate, float meanSnowflakeDiameter, float terminalVelocity, float density, Int32 numChannels, float beamDivergence, - bool doSimulateEnergyLoss, float snowflakeOccupancyThreshold) + float occupancyThreshold) { RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_SIMULATE_SNOW); - RGLNativeAPI.NodePointsSimulateSnow(ref handle.Node, minRange, maxRange, rainRate, - meanSnowflakeDiameter, terminalVelocity, density, numChannels, beamDivergence, doSimulateEnergyLoss, - snowflakeOccupancyThreshold); + RGLNativeAPI.NodePointsSimulateSnow(ref handle.Node, minRange, maxRange, rainRate, meanSnowflakeDiameter, + terminalVelocity, density, numChannels, beamDivergence, occupancyThreshold); return this; } - public RGLNodeSequence UpdateMultiReturnSwitch(string identifier, RGLReturnType returnType) + public RGLNodeSequence UpdateNodePointsSnowDefaults(string identifier, int snowflakesId, float fullBeamIntensity, float snowflakesLaserRetro) { - RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.MULTI_RETURN_SWITCH); - RGLNativeAPI.NodeMultiReturnSwitch(ref handle.Node, returnType); + RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_SIMULATE_SNOW); + RGLNativeAPI.NodePointsSimulateSnowConfigureDefaults(handle.Node, snowflakesId, fullBeamIntensity, snowflakesLaserRetro); return this; } @@ -522,6 +509,13 @@ public RGLNodeSequence ConfigureNodeRaytraceBeamDivergence(string identifier, fl return this; } + public RGLNodeSequence ConfigureNodeRaytraceReturnMode(string identifier, RGLReturnMode returnMode) + { + RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.RAYTRACE); + RGLNativeAPI.NodeRaytraceConfigureReturnMode(handle.Node, returnMode); + return this; + } + public RGLNodeSequence SetNodeRadarClasses(string identifier, int[] entityIds, RGLRadarObjectClass[] objectClasses) { RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_RADAR_TRACK_OBJECTS); diff --git a/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs b/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs index 4174109a0..226febd32 100644 --- a/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/RadarSensor.cs @@ -98,7 +98,7 @@ public void Awake() public void Start() { - sceneManager = FindObjectOfType(); + sceneManager = SceneManager.Instance; if (sceneManager == null) { // TODO(prybicki): this is too tedious, implement automatic instantiation of RGL Scene Manager diff --git a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs index ec8a04631..d78f65a95 100644 --- a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs +++ b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs @@ -46,6 +46,9 @@ public enum MeshSource RegularMeshesAndSkinnedMeshes } + // Singleton pattern + public static SceneManager Instance { get; private set; } + [SerializeField] private MeshSource meshSource = MeshSource.RegularMeshesAndSkinnedMeshes; @@ -66,8 +69,11 @@ public enum MeshSource // Since categoryId can be changed in the runtime, this is filled only on object removal / simulation end. private readonly Dictionary semanticDict = new Dictionary(); - private int lastUpdateFrame = -1; - private int lastFixedUpdateFrame = -1; + // Last frame of the simulation in which there was an update. + public int LastUpdateFrame { get; private set; } = -1; + + // Last FixedUpdate (physics cycle) in the `LastUpdateFrame` in which there was an update. + public int LastFixedUpdateFrame { get; private set; } = -1; private void OnDisable() { @@ -81,6 +87,14 @@ private void OnValidate() private void Awake() { + if (Instance != null && Instance != this) + { + Debug.LogError("SceneManager is already on the scene. Removing this component"); + Destroy(this); + return; + } + Instance = this; + if (IntoRGLObjects == null) { UpdateMeshSource(); @@ -108,8 +122,9 @@ private void UpdateMeshSource() /// /// Find out what changes happened on the scene since the last update and update RGL data. /// - /// Indicates fixed update frame number to enable updating when FixedUpdate is called more frequently than Update. - public void DoUpdate(int fixedUpdateFrame = 0) + /// Indicates which in turn the FixedUpdate call in the current frame it is. + /// It enables updating when FixedUpdate is called more frequently than Update. + public void DoUpdate(int fixedUpdateInCurrentFrame = 0) { /* TODO(prybicki): * Placing this code in Update() might cause an artifact - only undefined subset of @@ -127,13 +142,13 @@ public void DoUpdate(int fixedUpdateFrame = 0) // The problem was that lidars updated their position every raytrace execution (fixedUpdate) while RGL scene did not update because it was the same frame of the simulation. // In this case, we are raytracing on the same scene changing only lidars position which may overlap with the body of not-updated objects. // Now, lidars track its FixedUpdate cycles in the currect frame and pass it as fixedUpdateFrame. - if (lastUpdateFrame == Time.frameCount && lastFixedUpdateFrame == fixedUpdateFrame) + if (LastUpdateFrame == Time.frameCount && LastFixedUpdateFrame == fixedUpdateInCurrentFrame) { return; // Already done in this frame and fixed update (physics cycle) } - lastFixedUpdateFrame = fixedUpdateFrame; - lastUpdateFrame = Time.frameCount; + LastFixedUpdateFrame = fixedUpdateInCurrentFrame; + LastUpdateFrame = Time.frameCount; SynchronizeSceneTime(); @@ -210,6 +225,7 @@ private void Clear() RGLMeshSharingManager.Clear(); RGLTextureSharingManager.Clear(); + BonesPoseCacheManager.Clear(); uploadedRGLObjects.Clear(); lastFrameGameObjects.Clear(); Debug.Log("RGLSceneManager: cleared"); diff --git a/Assets/Tests/PlayMode/Sensors/SensorsTest.cs b/Assets/Tests/PlayMode/Sensors/SensorsTest.cs index fdc8847e4..c1983b887 100644 --- a/Assets/Tests/PlayMode/Sensors/SensorsTest.cs +++ b/Assets/Tests/PlayMode/Sensors/SensorsTest.cs @@ -641,6 +641,60 @@ void GetMessageCallback(sensor_msgs.msg.PointCloud2 msg) } } + /// + /// Test Outline: + /// - Validate multi-return modes for LiDAR. + /// - LiDAR sensor is placed inside a 10m radius sphere. + /// Test Target: + /// - Check the number of points in the cloud for LiDAR return modes. + /// Expected Result: + /// - The number of points matches the number of LiDAR rays multipled by the number of returns. + /// + [UnityTest] + public IEnumerator LidarVLP16_MultiReturn() + { + string testScenario = "LidarVLP16_Sphere10m"; + yield return SetupEnvironment(testScenario); + yield return new WaitForFixedUpdate(); + + RGLUnityPlugin.LidarSensor lidarSensor = GameObject.FindObjectOfType(); + Assert.NotNull(lidarSensor); + lidarSensor.simulateBeamDivergence = false; + + // remove noise in lidar + lidarSensor.applyDistanceGaussianNoise = false; + lidarSensor.applyAngularGaussianNoise = false; + lidarSensor.applyVelocityDistortion = false; + + // remove output restrictions + lidarSensor.outputRestriction.applyRestriction = false; + + // configure graph yield + rglSubgraphYieldOutput = new RGLUnityPlugin.RGLNodeSequence() + .AddNodePointsYield(yieldOutputNodeId, RGLUnityPlugin.RGLField.XYZ_VEC3_F32); + rglSubgraphYieldOutput.SetPriority(yieldOutputNodeId, 1); + rglSubgraphYieldOutput.SetActive(yieldOutputNodeId, true); + lidarSensor.ConnectToWorldFrame(rglSubgraphYieldOutput); + + lidarSensor.returnMode = RGLUnityPlugin.RGLReturnMode.SingleReturnFirst; + int numberOfReturns = 1; + lidarSensor.OnValidate(); + + lidarSensor.onNewData += OnNewLidarData; + yield return new WaitForSeconds(testDuration / 2); + lidarSensor.returnMode = RGLUnityPlugin.RGLReturnMode.DualReturnFirstLast; + numberOfReturns = 2; + lidarSensor.OnValidate(); + yield return new WaitForSeconds(testDuration / 2); + lidarSensor.onNewData -= OnNewLidarData; + + void OnNewLidarData() + { + rglSubgraphYieldOutput.GetResultData(ref onlyHits); + Assert.IsTrue(onlyHits.Count() == numberOfReturns * lidarSensor.configuration.GetRayPoses().Count()); + } + } + /// /// Method for converting RGL QoS to ROS2 QoS diff --git a/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md b/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md index 491dd9bca..dfbe4408a 100644 --- a/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md +++ b/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md @@ -94,10 +94,21 @@ The pipeline consists of: #### Elements configurable from the editor level - `Automatic Capture Hz` - the rate of sensor processing (default: `10Hz`) - `Model Preset` - allows selecting one of the built-in *LiDAR* models (default: `RangeMeter`) -- `Return Type` - allows selecting multi-return mode (note: this requires more computation). Modes other than "not divergent" require positive beam divergence. +- `Return Mode` - allows selecting LiDAR return mode (default: `Single Return First`). Both single and multi return modes may be selected, including various return types. Return types may be base on distance from LiDAR (e.g. first or last) or on instance [intensity](#intensity-texture) (strongest and second strongest). Note that dual return modes are reasonable only when beam divergence simulation is enabled (see parameter below). If beam divergence simulation is disabled and one of dual return modes is selected, then the same hit point will be returned for each return type (two exactly same hit points for each LiDAR ray). Available return modes are: + - `Single Return First` + - `Single Return Second` + - `Single Return Last` + - `Single Return Strongest` + - `Dual Return Last Strongest` + - `Dual Return First Last` + - `Dual Return First Strongest` + - `Dual Return Strongest Second Strongest` + - `Dual Return First Second` - `Apply Distance Gaussian Noise` - enable/disable distance *Gaussian* noise (default: `true`) - `Apply Angular Gaussian Noise` - enable/disable angular *Gaussian* noise (default: `true`) - `Apply Velocity Distortion` - enable/disable velocity distortion (default: `false`) +- `Simulate Beam Divergence` - enable/disable beam divergence simulation (default: `false`). Setting this to `false` effectively cause RGL to ignore +horizontal and vertical beam divergence values (as they would be set to 0). Note that simulating beam divergence requires significantly more processing power - if you observe any issues with the framerate, it is suggested to disable beam divergence simulation. - *Configuration*: - `Laser Array` - geometry description of lidar's array of lasers, should be prepared on the basis of the manual for a given model of *LiDAR* (default: loaded from `LaserArrayLibrary`) - `Horizontal Resolution` - the horiontal resolution of laser array firings diff --git a/docs/Components/Sensors/LiDARSensor/LiDARSensor/script.png b/docs/Components/Sensors/LiDARSensor/LiDARSensor/script.png index 44349bd38..a1c925a4a 100644 Binary files a/docs/Components/Sensors/LiDARSensor/LiDARSensor/script.png and b/docs/Components/Sensors/LiDARSensor/LiDARSensor/script.png differ