diff --git a/Assets/RGLUnityPlugin/README.md b/Assets/RGLUnityPlugin/README.md index f974e521e..2b238a42a 100644 --- a/Assets/RGLUnityPlugin/README.md +++ b/Assets/RGLUnityPlugin/README.md @@ -11,11 +11,19 @@ The native RGL library needs a once-per-scene preparation to access models on th 1. Create an empty object 2. Attach script `LidarSensor.cs`. -3. `PointCloudVisualization.cs` will be added automatically, however, you can disable it. -4. Now you can add a callback from another script to receive a notification when data is ready: +3. (Optional) Attach script `PointCloudVisualization.cs` for visualization purposes. +4. To obtain point cloud data from another script you have to create a new `RGLNodeSequence` and connect it to `LidarSensor`: ```cs + rglOutSubgraph = new RGLNodeSequence().AddNodePointsYield("OUT_XYZ", RGLField.XYZ_F32); lidarSensor = GetComponent(); - lidarSensor.OnOutputData += HandleLidarDataMethod; + lidarSensor.ConnectToWorldFrame(rglOutSubgraph); // you can also connect to Lidar frame using ConnectToLidarFrame + // You can add a callback to receive a notification when new data is ready + lidarSensor.onNewData += HandleLidarDataMethod; + ``` +5. To get data from `RGLNodeSequence` call `GetResultData`: + ```cs + Vector3[] xyz = new Vector3[0]; + rglOutSubgraph.GetResultData(ref xyz); ``` ## Adding new lidar models diff --git a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs index 73bc796f8..77ee9877c 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs @@ -25,7 +25,6 @@ namespace RGLUnityPlugin /// /// Encapsulates all non-ROS components of a RGL-based Lidar. /// - [RequireComponent(typeof(PointCloudVisualization))] public class LidarSensor : MonoBehaviour { /// @@ -37,7 +36,6 @@ public class LidarSensor : MonoBehaviour /// /// Delegate used in callbacks. /// - /// Data output for each hz public delegate void OnNewDataDelegate(); /// @@ -74,7 +72,6 @@ public class LidarSensor : MonoBehaviour private RGLNodeSequence rglGraphLidar; private RGLNodeSequence rglSubgraphCompact; private RGLNodeSequence rglSubgraphToLidarFrame; - private RGLNodeSequence rglSubgraphVisualizationOutput; private SceneManager sceneManager; private readonly string lidarRaysNodeId = "LIDAR_RAYS"; @@ -86,7 +83,6 @@ public class LidarSensor : MonoBehaviour private readonly string noiseDistanceNodeId = "NOISE_DISTANCE"; private readonly string pointsCompactNodeId = "POINTS_COMPACT"; private readonly string toLidarFrameNodeId = "TO_LIDAR_FRAME"; - private readonly string visualizationOutputNodeId = "OUT_VISUALIZATION"; private LidarModel? validatedPreset; private float timer; @@ -108,12 +104,8 @@ public void Awake() rglSubgraphToLidarFrame = new RGLNodeSequence() .AddNodePointsTransform(toLidarFrameNodeId, Matrix4x4.identity); - rglSubgraphVisualizationOutput = new RGLNodeSequence() - .AddNodePointsYield(visualizationOutputNodeId, RGLField.XYZ_F32); - RGLNodeSequence.Connect(rglGraphLidar, rglSubgraphCompact); RGLNodeSequence.Connect(rglSubgraphCompact, rglSubgraphToLidarFrame); - RGLNodeSequence.Connect(rglSubgraphCompact, rglSubgraphVisualizationOutput); } public void Start() @@ -226,14 +218,6 @@ public void Capture() rglSubgraphToLidarFrame.UpdateNodePointsTransform(toLidarFrameNodeId, lidarPose.inverse); rglGraphLidar.Run(); - - // Could be moved to PointCloudVisualization - if (GetComponent().isActiveAndEnabled) - { - Vector3[] onlyHits = new Vector3[0]; - rglSubgraphVisualizationOutput.GetResultData(ref onlyHits); - GetComponent().SetPoints(onlyHits); - } } } } diff --git a/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs b/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs index 1ce60d6af..76014bea0 100644 --- a/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs +++ b/Assets/RGLUnityPlugin/Scripts/PointCloudVisualization.cs @@ -20,6 +20,7 @@ namespace RGLUnityPlugin { [System.Serializable] + [RequireComponent(typeof(LidarSensor))] public class PointCloudVisualization : MonoBehaviour { public enum PointShape @@ -63,8 +64,22 @@ public enum PointShape private Mesh mesh; + private LidarSensor lidarSensor; + private RGLNodeSequence rglSubgraphVisualizationOutput; + private readonly string visualizationOutputNodeId = "OUT_VISUALIZATION"; + + public void Awake() + { + rglSubgraphVisualizationOutput = new RGLNodeSequence() + .AddNodePointsYield(visualizationOutputNodeId, RGLField.XYZ_F32); + + lidarSensor = GetComponent(); + } + public void Start() { + lidarSensor.ConnectToWorldFrame(rglSubgraphVisualizationOutput); + mesh = new Mesh(); if (!material) { @@ -79,6 +94,18 @@ public void Start() mesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32; } + public void OnEnable() + { + rglSubgraphVisualizationOutput.SetActive(visualizationOutputNodeId, true); + lidarSensor.onNewData += OnNewLidarData; + } + + public void OnDisable() + { + rglSubgraphVisualizationOutput.SetActive(visualizationOutputNodeId, false); + lidarSensor.onNewData -= OnNewLidarData; + } + public void OnValidate() { if (!material) @@ -126,5 +153,12 @@ public void Update() { Graphics.DrawMesh(mesh, Vector3.zero, Quaternion.identity, material, visualizationLayerID); } + + private void OnNewLidarData() + { + Vector3[] onlyHits = new Vector3[0]; + rglSubgraphVisualizationOutput.GetResultData(ref onlyHits); + SetPoints(onlyHits); + } } } \ No newline at end of file diff --git a/docs/Components/Sensors/LiDARSensor/AddNewLiDAR/index.md b/docs/Components/Sensors/LiDARSensor/AddNewLiDAR/index.md index 2094b2c5f..99b244417 100644 --- a/docs/Components/Sensors/LiDARSensor/AddNewLiDAR/index.md +++ b/docs/Components/Sensors/LiDARSensor/AddNewLiDAR/index.md @@ -35,7 +35,7 @@ To add a new *LiDAR* model, perform the following steps: 1. Create an empty object and name it appropriately according to the *LiDAR* model. 1. Attach script `LidarSensor.cs` to created object. 1. Set the new added *LiDAR* model in `Model Preset` field, check if the configuration loads correctly. You can now customize it however you like. -1. `PointCloudVisualization.cs` will be added automatically, however, you can disable it. +1. (Optional) Attach script `PointCloudVisualization.cs` for visualization purposes. 1. For publishing point cloud via *ROS2* attach script `RglLidarPublisher.cs` script to created object. 1. Set the topics on which you want the data to be published and their frame. 2. Save the prefab in the project.