diff --git a/Assets/AWSIM/Scenes/Main/PointCloudMapping/Scripts/Mapper/RGLMappingAdapter.cs b/Assets/AWSIM/Scenes/Main/PointCloudMapping/Scripts/Mapper/RGLMappingAdapter.cs index 4d2b42a0e..6c024f814 100644 --- a/Assets/AWSIM/Scenes/Main/PointCloudMapping/Scripts/Mapper/RGLMappingAdapter.cs +++ b/Assets/AWSIM/Scenes/Main/PointCloudMapping/Scripts/Mapper/RGLMappingAdapter.cs @@ -71,7 +71,7 @@ public void Initialize(Vector3 worldOriginROS, string outputPcdFilePath) rglSubgraphMapping = new RGLNodeSequence() .AddNodePointsTransform(rosWorldTransformNodeId, worldTransform) .AddNodePointsDownsample(downsampleNodeId, new Vector3(leafSize, leafSize, leafSize)) - .AddNodePointsTemporalMerge(temporalMergeNodeId, new RGLField[1] {RGLField.XYZ_F32}); + .AddNodePointsTemporalMerge(temporalMergeNodeId, new RGLField[1] {RGLField.XYZ_VEC3_F32}); rglSubgraphMapping.SetActive(downsampleNodeId, enableDownsampling); diff --git a/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormats.cs b/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormats.cs index c225373bb..760c63571 100644 --- a/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormats.cs +++ b/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormats.cs @@ -29,7 +29,7 @@ public static RGLField[] GetRGLFields() { return new[] { - RGLField.XYZ_F32, + RGLField.XYZ_VEC3_F32, RGLField.PADDING_32, RGLField.INTENSITY_F32, RGLField.RING_ID_U16, @@ -47,7 +47,7 @@ public static RGLField[] GetRGLFields() { return new[] { - RGLField.XYZ_F32, + RGLField.XYZ_VEC3_F32, RGLField.PADDING_32, RGLField.INTENSITY_F32, RGLField.RING_ID_U16, @@ -72,7 +72,7 @@ public static RGLField[] GetRGLFields() { return new[] { - RGLField.XYZ_F32, + RGLField.XYZ_VEC3_F32, RGLField.ENTITY_ID_I32, RGLField.INTENSITY_F32 }; diff --git a/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs b/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs index dca5e674e..4e8e3da5a 100644 --- a/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs +++ b/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +using System; using UnityEngine; -using AWSIM; using AWSIM.PointCloudFormats; using RGLUnityPlugin; @@ -51,8 +51,11 @@ private void Awake() { rglSubgraphUnity2Ros = new RGLNodeSequence() .AddNodePointsTransform("UNITY_TO_ROS", ROS2.Transformations.Unity2RosMatrix4x4()); + } - if (publishPCL24) + private void OnEnable() + { + if (publishPCL24 && rglSubgraphPcl24 == null) { rglSubgraphPcl24 = new RGLNodeSequence() .AddNodePointsFormat("PCL24_FORMAT", FormatPCL24.GetRGLFields()) @@ -60,7 +63,7 @@ private void Awake() RGLNodeSequence.Connect(rglSubgraphUnity2Ros, rglSubgraphPcl24); } - if (publishPCL48) + if (publishPCL48 && rglSubgraphPcl48 == null) { rglSubgraphPcl48 = new RGLNodeSequence() .AddNodePointsFormat("PCL48_FORMAT", FormatPCL48.GetRGLFields()) @@ -68,7 +71,7 @@ private void Awake() RGLNodeSequence.Connect(rglSubgraphUnity2Ros, rglSubgraphPcl48); } - if (publishInstanceId) + if (publishInstanceId && rglSubgraphInstanceId == null) { rglSubgraphInstanceId = new RGLNodeSequence() .AddNodePointsFormat("ML_FORMAT", FormatMLInstanceSegmentation.GetRGLFields()) @@ -84,9 +87,6 @@ private void Start() Debug.LogWarning("All lidar message formats are disabled. Nothing to publish!"); } - // Synchronize RGL time with the same TimeSource as AWSIM - SceneManager.TimeSource = SimulatorROS2Node.TimeSource; - lidarSensor = GetComponent(); lidarSensor.ConnectToLidarFrame(rglSubgraphUnity2Ros); @@ -95,6 +95,11 @@ private void Start() public void OnValidate() { + if (!enabled) + { + return; + } + ApplySubgraphState(ref rglSubgraphPcl24, publishPCL24); ApplySubgraphState(ref rglSubgraphPcl48, publishPCL48); ApplySubgraphState(ref rglSubgraphInstanceId, publishInstanceId); @@ -102,31 +107,24 @@ public void OnValidate() private void OnDisable() { - publishPCL24 = false; - publishPCL48 = false; - publishInstanceId = false; - OnValidate(); + ApplySubgraphState(ref rglSubgraphPcl24, false); + ApplySubgraphState(ref rglSubgraphPcl48, false); + ApplySubgraphState(ref rglSubgraphInstanceId, false); } private void OnDestroy() { - if (rglSubgraphPcl24 != null) + Action destroySubgraphIfNotNull = subgraph => { + if (rglSubgraphPcl24 == null) return; rglSubgraphPcl24.Clear(); rglSubgraphPcl24 = null; - } - - if (rglSubgraphPcl48 != null) - { - rglSubgraphPcl48.Clear(); - rglSubgraphPcl48 = null; - } + }; - if (rglSubgraphInstanceId != null) - { - rglSubgraphInstanceId.Clear(); - rglSubgraphInstanceId = null; - } + destroySubgraphIfNotNull(rglSubgraphPcl24); + destroySubgraphIfNotNull(rglSubgraphPcl48); + destroySubgraphIfNotNull(rglSubgraphInstanceId); + destroySubgraphIfNotNull(rglSubgraphUnity2Ros); } private void ApplySubgraphState(ref RGLNodeSequence subgraph, bool activateState) @@ -155,4 +153,4 @@ private void ApplySubgraphState(ref RGLNodeSequence subgraph, bool activateState } } } -} \ No newline at end of file +} diff --git a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so index f54eb0764..ffadf8f98 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 881f86813..2a117fbe1 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 c76b42a02..a067fe7f0 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs @@ -12,13 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -using System; using System.Collections.Generic; -using Unity.VisualScripting; using UnityEngine; -using UnityEngine.SceneManagement; using UnityEngine.Serialization; -using Object = System.Object; namespace RGLUnityPlugin { @@ -79,17 +75,17 @@ public class LidarSensor : MonoBehaviour private RGLNodeSequence rglSubgraphToLidarFrame; private SceneManager sceneManager; - private readonly string lidarRaysNodeId = "LIDAR_RAYS"; - private readonly string lidarRangeNodeId = "LIDAR_RANGE"; - private readonly string lidarRingsNodeId = "LIDAR_RINGS"; - private readonly string lidarTimeOffsetsNodeId = "LIDAR_OFFSETS"; - private readonly string lidarPoseNodeId = "LIDAR_POSE"; - private readonly string noiseLidarRayNodeId = "NOISE_LIDAR_RAY"; - private readonly string lidarRaytraceNodeId = "LIDAR_RAYTRACE"; - private readonly string noiseHitpointNodeId = "NOISE_HITPOINT"; - private readonly string noiseDistanceNodeId = "NOISE_DISTANCE"; - private readonly string pointsCompactNodeId = "POINTS_COMPACT"; - private readonly string toLidarFrameNodeId = "TO_LIDAR_FRAME"; + private const string lidarRaysNodeId = "LIDAR_RAYS"; + private const string lidarRangeNodeId = "LIDAR_RANGE"; + private const string lidarRingsNodeId = "LIDAR_RINGS"; + private const string lidarTimeOffsetsNodeId = "LIDAR_OFFSETS"; + private const string lidarPoseNodeId = "LIDAR_POSE"; + private const string noiseLidarRayNodeId = "NOISE_LIDAR_RAY"; + private const string lidarRaytraceNodeId = "LIDAR_RAYTRACE"; + private const string noiseHitpointNodeId = "NOISE_HITPOINT"; + private const string noiseDistanceNodeId = "NOISE_DISTANCE"; + private const string pointsCompactNodeId = "POINTS_COMPACT"; + private const string toLidarFrameNodeId = "TO_LIDAR_FRAME"; private LidarModel? validatedPreset; private float timer; @@ -100,6 +96,8 @@ public class LidarSensor : MonoBehaviour private int fixedUpdatesInCurrentFrame = 0; private int lastUpdateFrame = -1; + private static List activeSensors = new List(); + public void Awake() { rglGraphLidar = new RGLNodeSequence() @@ -159,10 +157,7 @@ private void ApplyConfiguration(LidarConfiguration newConfig) return; } - if (onLidarModelChange != null) - { - onLidarModelChange.Invoke(); - } + onLidarModelChange?.Invoke(); rglGraphLidar.UpdateNodeRaysFromMat3x4f(lidarRaysNodeId, newConfig.GetRayPoses()) .UpdateNodeRaysSetRange(lidarRangeNodeId, newConfig.GetRayRanges()) @@ -189,7 +184,49 @@ private void ApplyConfiguration(LidarConfiguration newConfig) } } + public void OnEnable() + { + activeSensors.Add(this); + } + + public void OnDisable() + { + activeSensors.Remove(this); + } + public void FixedUpdate() + { + // One LidarSensor triggers FixedUpdateLogic for all of active LidarSensors on the scene + // This is an optimization to take full advantage of asynchronous RGL graph execution + // First, all RGL graphs are run which enqueue the most priority graph branches (e.g., visualization for Unity) properly + // Then, `onNewData` delegate is called to notify other components about new data available + // This way, the most important (Unity blocking) computations for all of the sensors are performed first + // Non-blocking operations (e.g., ROS2 publishing) are performed next + if (activeSensors[0] != this) + { + return; + } + + var triggeredSensorsIndexes = new List(); + for (var i = 0; i < activeSensors.Count; i++) + { + if (activeSensors[i].FixedUpdateLogic()) + { + triggeredSensorsIndexes.Add(i); + } + } + + foreach (var idx in triggeredSensorsIndexes) + { + activeSensors[idx].NotifyNewData(); + } + } + + /// + /// Performs fixed update logic. + /// Returns true if sensor was triggered (raytracing was performed) + /// + private bool FixedUpdateLogic() { if (lastUpdateFrame != Time.frameCount) { @@ -200,7 +237,7 @@ public void FixedUpdate() if (AutomaticCaptureHz == 0.0f) { - return; + return false; } timer += Time.deltaTime; @@ -210,15 +247,17 @@ public void FixedUpdate() var interval = 1.0f / AutomaticCaptureHz; if (timer + 0.00001f < interval) - return; + return false; timer = 0; Capture(); - if (onNewData != null) - { - onNewData.Invoke(); - } + return true; + } + + private void NotifyNewData() + { + onNewData?.Invoke(); } /// @@ -262,13 +301,13 @@ public void Capture() rglGraphLidar.Run(); } - public void UpdateTransforms() + private void UpdateTransforms() { lastTransform = currentTransform; currentTransform = gameObject.transform.localToWorldMatrix; } - public void SetVelocityToRaytrace() + private void SetVelocityToRaytrace() { // Calculate delta transform of lidar. // Velocities must be in sensor-local coordinate frame. diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs index c84fbfa01..b09fbdcd3 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs @@ -33,7 +33,7 @@ public class LidarUdpPublisher : MonoBehaviour private RGLUdpOptions currentRGLUdpOptions = 0; // To be set when validating lidar model private RGLNodeSequence rglSubgraphUdpPublishing; - private readonly string udpPublishingNodeId = "UDP_PUBLISHING"; + private const string udpPublishingNodeId = "UDP_PUBLISHING"; private LidarSensor lidarSensor; diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs index 3e54c59b6..197a4a282 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs @@ -137,7 +137,7 @@ public static extern int rgl_node_points_udp_publish( public static extern int rgl_graph_write_pcd_file(IntPtr node, [MarshalAs(UnmanagedType.LPStr)] string file_path); [DllImport("RobotecGPULidar")] - public static extern int rgl_graph_get_result_size(IntPtr node, RGLField field, out Int64 outCount, out Int64 outSizeOf); + public static extern int rgl_graph_get_result_size(IntPtr node, RGLField field, out Int32 outCount, out Int32 outSizeOf); [DllImport("RobotecGPULidar")] public static extern int rgl_graph_get_result_data(IntPtr node, RGLField field, IntPtr data); @@ -148,6 +148,12 @@ public static extern int rgl_node_points_udp_publish( [DllImport("RobotecGPULidar")] public static extern int rgl_graph_node_remove_child(IntPtr parent, IntPtr child); + [DllImport("RobotecGPULidar")] + public static extern int rgl_graph_node_set_priority(IntPtr node, Int32 priority); + + [DllImport("RobotecGPULidar")] + public static extern int rgl_graph_node_get_priority(IntPtr node, out Int32 priority); + [DllImport("RobotecGPULidar")] public static extern int rgl_tape_record_begin([MarshalAs(UnmanagedType.LPStr)] string path); @@ -187,7 +193,7 @@ static RGLNativeAPI() public static void CheckVersion() { int expectedMajor = 0; - int expectedMinor = 15; + int expectedMinor = 16; int expectedPatch = 0; CheckErr(rgl_get_version_info(out var major, out var minor, out var patch)); if (major != expectedMajor || minor < expectedMinor || (minor == expectedMinor && patch < expectedPatch)) @@ -471,16 +477,16 @@ public static void GraphRun(IntPtr node) public static int GraphGetResultSize(IntPtr node, RGLField field) { - Int64 pointCount = 0; - Int64 pointSize = 0; + Int32 pointCount = 0; + Int32 pointSize = 0; CheckErr(rgl_graph_get_result_size(node, field, out pointCount, out pointSize)); return (int) pointCount; } public static int GraphGetResult(IntPtr node, RGLField field, ref T[] data, int expectedPointSize) where T : unmanaged { - Int64 pointCount = 0; - Int64 pointSize = 0; + Int32 pointCount = 0; + Int32 pointSize = 0; CheckErr(rgl_graph_get_result_size(node, field, out pointCount, out pointSize)); unsafe { @@ -524,6 +530,17 @@ public static void GraphNodeRemoveChild(IntPtr parent, IntPtr child) CheckErr(rgl_graph_node_remove_child(parent, child)); } + public static void GraphNodeSetPriority(IntPtr node, Int32 priority) + { + CheckErr(rgl_graph_node_set_priority(node, priority)); + } + + public static int GraphNodeGetPriority(IntPtr node) + { + CheckErr(rgl_graph_node_get_priority(node, out var outPriority)); + return outPriority; + } + public static void GraphDestroy(IntPtr node) { CheckErr(rgl_graph_destroy(node)); diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs index b844944b1..60dcf67ad 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs @@ -23,7 +23,12 @@ public enum RGLStatus : Int32 INVALID_STATE, LOGGING_ERROR, INVALID_API_OBJECT, + INVALID_FILE_PATH, + TAPE_ERROR, + UDP_ERROR, + ROS2_ERROR, INVALID_PIPELINE, + INITIALIZATION_ERROR, NOT_IMPLEMENTED = 404, INTERNAL_EXCEPTION = 500, }; @@ -31,7 +36,7 @@ public enum RGLStatus : Int32 public enum RGLField : Int32 { UNKNOWN = -1, - XYZ_F32 = 1, + XYZ_VEC3_F32 = 1, INTENSITY_F32, IS_HIT_I32, RAY_IDX_U32, @@ -106,7 +111,7 @@ public enum RGLQosPolicyHistory QOS_POLICY_HISTORY_KEEP_ALL = 2, }; - public enum RGLExtension + public enum RGLExtension : Int32 { RGL_EXTENSION_PCL = 0, RGL_EXTENSION_ROS2 = 1, diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs index 01548cf8e..03e3e7c34 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs @@ -385,7 +385,7 @@ public int GetResultDataRaw(ref byte[] data, int expectedPointSize) return RGLNativeAPI.GraphGetResult(handle.Node, handle.OutputField, ref data, expectedPointSize); } - public int GetPointCloudCount(string identifier = null, RGLField field = RGLField.XYZ_F32) + public int GetPointCloudCount(string identifier = null, RGLField field = RGLField.XYZ_VEC3_F32) { RGLNodeHandle handle = identifier == null ? GetLastNodeOrNull(true) : ValidateNode(identifier); if (handle == null) @@ -456,6 +456,12 @@ public void SetActive(string identifier, bool active) } } + public void SetPriority(string identifier, int priority) + { + RGLNodeHandle node = ValidateNode(identifier); + RGLNativeAPI.GraphNodeSetPriority(node.Node, priority); + } + //// PRIVATE HELPERS //// // Throws an exception when node with provided identifier already exists in this NodeSequence private void CheckNodeNotExist(string identifier) diff --git a/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs b/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs index 76014bea0..985f17db3 100644 --- a/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs +++ b/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +using System; using System.Collections.Generic; -using System.Linq; -using UnityEngine.Serialization; using UnityEngine; namespace RGLUnityPlugin @@ -30,7 +29,7 @@ public enum PointShape Pyramid = 2 } - static private readonly List rainbowColors = new List { + private static readonly List rainbowColors = new List { Color.red, new Color(1, 0.5f, 0, 1), // orange Color.yellow, @@ -64,14 +63,20 @@ public enum PointShape private Mesh mesh; + private Vector3[] onlyHits = Array.Empty(); + private int pointCount = 0; + private int[] indices = Array.Empty(); + private LidarSensor lidarSensor; private RGLNodeSequence rglSubgraphVisualizationOutput; - private readonly string visualizationOutputNodeId = "OUT_VISUALIZATION"; + private const string visualizationOutputNodeId = "OUT_VISUALIZATION"; public void Awake() { rglSubgraphVisualizationOutput = new RGLNodeSequence() - .AddNodePointsYield(visualizationOutputNodeId, RGLField.XYZ_F32); + .AddNodePointsYield(visualizationOutputNodeId, RGLField.XYZ_VEC3_F32); + + rglSubgraphVisualizationOutput.SetPriority(visualizationOutputNodeId, 1); lidarSensor = GetComponent(); } @@ -127,17 +132,18 @@ public void OnValidate() public void SetPoints(Vector3[] points) { - // TODO: easy, low-prio optimization here - int[] indicies = new int[points.Length]; - - for (int i = 0; i < points.Length; ++i) + if (indices.Length < points.Length) { - indicies[i] = i; + indices = new int[points.Length]; + for (int i = 0; i < points.Length; ++i) + { + indices[i] = i; + } } mesh.Clear(); mesh.vertices = points; - mesh.SetIndices(indicies, MeshTopology.Points, 0); + mesh.SetIndices(indices, 0, pointCount, MeshTopology.Points, 0); if (autoComputeColoringHeights) { @@ -151,14 +157,17 @@ public void SetPoints(Vector3[] points) public void Update() { + if (!lidarSensor.enabled) + { + mesh.Clear(); + } Graphics.DrawMesh(mesh, Vector3.zero, Quaternion.identity, material, visualizationLayerID); } private void OnNewLidarData() { - Vector3[] onlyHits = new Vector3[0]; - rglSubgraphVisualizationOutput.GetResultData(ref onlyHits); + pointCount = rglSubgraphVisualizationOutput.GetResultData(ref onlyHits); SetPoints(onlyHits); } } -} \ No newline at end of file +} diff --git a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs index 7e9d7167f..d151a1a64 100644 --- a/Assets/RGLUnityPlugin/Scripts/SceneManager.cs +++ b/Assets/RGLUnityPlugin/Scripts/SceneManager.cs @@ -66,7 +66,8 @@ 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(); - public static ITimeSource TimeSource { get; set; } = new UnityTimeSource(); + // Using the same TimeSource as AWSIM does + public static ITimeSource TimeSource { get; } = AWSIM.SimulatorROS2Node.TimeSource; private int lastUpdateFrame = -1; private int lastFixedUpdateFrame = -1;