Add model specific options to the lidar sensor component

Author: FilipAlg

Diff for: AGXUnity/Sensor/LidarSensor.cs

@@ -8,6 +8,26 @@
 
 namespace AGXUnity.Sensor
 {
+  public interface IModelData { }
+
+  [Serializable]
+  public class OusterData : IModelData
+  {
+    public LidarModelOusterOS.ChannelCount ChannelCount = LidarModelOusterOS.ChannelCount.ch_32;
+    public LidarModelOusterOS.BeamSpacing BeamSpacing = LidarModelOusterOS.BeamSpacing.Uniform;
+    public LidarModelOusterOS.LidarMode LidarMode = LidarModelOusterOS.LidarMode.Mode_512x20;
+  }
+
+  [Serializable]
+  public class GenericSweepData : IModelData
+  {
+    public float Frequency = 10.0f;
+    public float HorizontalFoV = 360.0f;
+    public float VerticalFoV = 35.0f;
+    public float HorizontalResolution = 0.5f;
+    public float VerticalResolution = 0.5f;
+  }
+
   public enum LidarModelPreset
   {
     NONE,
@@ -34,7 +54,29 @@ public class LidarSensor : ScriptComponent
     * The Model, or preset, of this Lidar.
     * Changing this will assign Model specific properties to this Lidar.
     */
-    public LidarModelPreset LidarModelPreset = LidarModelPreset.LidarModelOusterOS1;
+    [SerializeField]
+    private LidarModelPreset m_lidarModelPreset = LidarModelPreset.LidarModelOusterOS1;
+
+    public LidarModelPreset LidarModelPreset
+    {
+      get => m_lidarModelPreset;
+      set
+      {
+        m_lidarModelPreset = value;
+        ModelData = value switch
+        {
+          LidarModelPreset.LidarModelOusterOS0 => new OusterData(),
+          LidarModelPreset.LidarModelOusterOS1 => new OusterData(),
+          LidarModelPreset.LidarModelOusterOS2 => new OusterData(),
+          LidarModelPreset.LidarModelGeneric360HorizontalSweep => new GenericSweepData(),
+          LidarModelPreset.NONE => null,
+        };
+      }
+    }
+
+
+    [field: SerializeReference]
+    public IModelData ModelData { get; private set; } = new OusterData();
 
     /**
     * The minimum and maximum range of the Lidar Sensor [m].
@@ -183,19 +225,27 @@ private LidarModel CreateLidarModel( LidarModelPreset preset )
 
       switch ( preset ) {
         case LidarModelPreset.LidarModelGeneric360HorizontalSweep:
-          lidarModel = new LidarModelGeneric360HorizontalSweep( 10f ); // TODO Default frequency for now, implement lidar settings
+          GenericSweepData sweepData = ModelData as GenericSweepData;
+          lidarModel = new LidarModelHorizontalSweep(
+            Mathf.Deg2Rad * new agx.Vec2( sweepData.HorizontalFoV, sweepData.VerticalFoV ),
+            Mathf.Deg2Rad * new agx.Vec2( sweepData.HorizontalResolution, sweepData.VerticalResolution ),
+            sweepData.Frequency
+          ); // TODO Default frequency for now, implement lidar settings
           break;
 
         case LidarModelPreset.LidarModelOusterOS0:
-          lidarModel = new LidarModelOusterOS0();
+          OusterData ousterData = ModelData as OusterData;
+          lidarModel = new LidarModelOusterOS0( ousterData.ChannelCount, ousterData.BeamSpacing, ousterData.LidarMode );
           break;
 
         case LidarModelPreset.LidarModelOusterOS1:
-          lidarModel = new LidarModelOusterOS1();
+          ousterData = ModelData as OusterData;
+          lidarModel = new LidarModelOusterOS1( ousterData.ChannelCount, ousterData.BeamSpacing, ousterData.LidarMode );
           break;
 
         case LidarModelPreset.LidarModelOusterOS2:
-          lidarModel = new LidarModelOusterOS2();
+          ousterData = ModelData as OusterData;
+          lidarModel = new LidarModelOusterOS2( ousterData.ChannelCount, ousterData.BeamSpacing, ousterData.LidarMode );
           break;
 
         case LidarModelPreset.NONE:

Diff for: AGXUnity/Sensor/SensorEnvironment.cs

@@ -294,20 +294,21 @@ protected override bool Initialize()
 
       FindValidComponents<MeshFilter>( true ).ForEach( RegisterMeshfilter );
 
-      FindValidComponents<DeformableTerrain>( true ).ForEach( c => m_agxComponents.Add( c, c.gameObject.activeInHierarchy ) );
-      FindValidComponents<DeformableTerrainPager>( true ).ForEach( c => m_agxComponents.Add( c, c.gameObject.activeInHierarchy ) );
-      FindValidComponents<Wire>( true ).ForEach( c => m_agxComponents.Add( c, c.gameObject.activeInHierarchy ) );
-      FindValidComponents<Cable>( true ).ForEach( c => m_agxComponents.Add( c, c.gameObject.activeInHierarchy ) );
-
-      foreach ( var entry in m_agxComponents ) {
-        if ( entry.Value )
-          AddAGXModel( entry.Key.GetInitialized() );
-      }
+      FindValidComponents<DeformableTerrain>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+      FindValidComponents<HeightField>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+      FindValidComponents<DeformableTerrainPager>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+      FindValidComponents<Wire>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+      FindValidComponents<Cable>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+      FindValidComponents<Track>( true ).ForEach( c => m_agxComponents.TryAdd( c, false ) );
+
+      UpdateEnvironment()
+
+      Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += UpdateEnvironment;
 
       return true;
     }
 
-    public void FixedUpdate()
+    public void UpdateEnvironment()
     {
       if ( Native == null )
         return;

Diff for: Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs

@@ -1113,5 +1113,44 @@ public static object UrdfElementDrawer( object[] objects, InvokeWrapper wrapper
 
       return null;
     }
+
+    public static void DrawOusterModelData( AGXUnity.Sensor.OusterData data )
+    {
+      data.ChannelCount = (agxSensor.LidarModelOusterOS.ChannelCount)EditorGUILayout.EnumPopup( "Channel Count", data.ChannelCount );
+      data.BeamSpacing  = (agxSensor.LidarModelOusterOS.BeamSpacing)EditorGUILayout.EnumPopup( "Beam Spacing", data.BeamSpacing );
+      data.LidarMode    = (agxSensor.LidarModelOusterOS.LidarMode)EditorGUILayout.EnumPopup( "Lidar Mode", data.LidarMode );
+    }
+
+    public static void DrawGenericSweepModelData( AGXUnity.Sensor.GenericSweepData data )
+    {
+      data.Frequency     = EditorGUILayout.FloatField( "Frequency", data.Frequency );
+      data.HorizontalFoV = EditorGUILayout.FloatField( "HorizontalFoV", data.HorizontalFoV );
+      data.VerticalFoV   = EditorGUILayout.FloatField( "VerticalFoV", data.VerticalFoV );
+      data.HorizontalResolution = EditorGUILayout.FloatField( "HorizontalResolution", data.HorizontalResolution );
+      data.VerticalResolution   = EditorGUILayout.FloatField( "VerticalResolution", data.VerticalResolution );
+    }
+
+    [InspectorDrawer( typeof( AGXUnity.Sensor.IModelData ) )]
+    public static object ModelDataDrawer( object[] objects, InvokeWrapper wrapper )
+    {
+      if ( objects.Length != 1 ) {
+        InspectorGUI.WarningLabel( "Multi-select of ModelData Elements isn't supported." );
+        return null;
+      }
+
+      var data = wrapper.Get<AGXUnity.Sensor.IModelData>( objects[0] );
+      using ( new InspectorGUI.IndentScope() ) {
+        switch ( data ) {
+          case AGXUnity.Sensor.OusterData ousterData:
+            DrawOusterModelData( ousterData );
+            break;
+          case AGXUnity.Sensor.GenericSweepData sweepData:
+            DrawGenericSweepModelData( sweepData );
+            break;
+        }
+      }
+
+      return null;
+    }
   }
 }