Upgrade RGL to v16

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);
 

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
             };

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,24 +51,27 @@ 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())
                     .AddNodePointsRos2Publish("PCL24_PUB", pcl24Topic, frameID, reliabilityPolicy, durabilityPolicy, historyPolicy, historyDepth);
                 RGLNodeSequence.Connect(rglSubgraphUnity2Ros, rglSubgraphPcl24);
             }
 
-            if (publishPCL48)
+            if (publishPCL48 && rglSubgraphPcl48 == null)
             {
                 rglSubgraphPcl48 = new RGLNodeSequence()
                     .AddNodePointsFormat("PCL48_FORMAT", FormatPCL48.GetRGLFields())
                     .AddNodePointsRos2Publish("PCL48_PUB", pcl48Topic, frameID, reliabilityPolicy, durabilityPolicy, historyPolicy, historyDepth);
                 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>();
             lidarSensor.ConnectToLidarFrame(rglSubgraphUnity2Ros);
 
@@ -95,38 +95,36 @@ private void Start()
 
         public void OnValidate()
         {
+            if (!enabled)
+            {
+                return;
+            }
+
             ApplySubgraphState(ref rglSubgraphPcl24, publishPCL24);
             ApplySubgraphState(ref rglSubgraphPcl48, publishPCL48);
             ApplySubgraphState(ref rglSubgraphInstanceId, publishInstanceId);
         }
 
         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<RGLNodeSequence> 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
             }
         }
     }
-}
+}

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<LidarSensor> activeSensors = new List<LidarSensor>();
+
         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<int>();
+            for (var i = 0; i < activeSensors.Count; i++)
+            {
+                if (activeSensors[i].FixedUpdateLogic())
+                {
+                    triggeredSensorsIndexes.Add(i);
+                }
+            }
+
+            foreach (var idx in triggeredSensorsIndexes)
+            {
+                activeSensors[idx].NotifyNewData();
+            }
+        }
+
+        /// <summary>
+        /// Performs fixed update logic.
+        /// Returns true if sensor was triggered (raytracing was performed)
+        /// </summary>
+        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();
         }
 
         /// <summary>
@@ -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.

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;
 

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<T>(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));