Skip to content

Commit d0c0f4a

Browse files
msz-raimsz-rai
committed
Add option to validate LiDAR configuration on startup
1 parent d5d74d6 commit d0c0f4a

File tree

5 files changed

+149
-21
lines changed

5 files changed

+149
-21
lines changed

‎Assets/RGLUnityPlugin/Scripts/LidarModels/Laser.cs

+23-1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
using System;
16+
1517
namespace RGLUnityPlugin
1618
{
1719
/// <summary>
@@ -20,7 +22,7 @@ namespace RGLUnityPlugin
2022
/// Numbers are expressed in terms of Unity axes convention.
2123
/// </summary>
2224
[System.Serializable]
23-
public struct Laser
25+
public struct Laser : IEquatable<Laser>
2426
{
2527
/// <summary>
2628
/// Rotation around Y-axis.
@@ -61,5 +63,25 @@ public struct Laser
6163
/// Note: May be ignored for some `LidarConfiguration`s (e.g. `UniformRangeLidarConfiguration`)
6264
/// </summary>
6365
public float maxRange;
66+
67+
//// IEquatable interface
68+
public bool Equals(Laser other)
69+
{
70+
return this.horizontalAngularOffsetDeg == other.horizontalAngularOffsetDeg &&
71+
this.verticalAngularOffsetDeg == other.verticalAngularOffsetDeg &&
72+
this.verticalLinearOffsetMm == other.verticalLinearOffsetMm &&
73+
this.ringId == other.ringId &&
74+
this.timeOffset == other.timeOffset &&
75+
this.minRange == other.minRange &&
76+
this.maxRange == other.maxRange;
77+
}
78+
79+
public override bool Equals(object obj)
80+
{
81+
return obj is Laser equatable && Equals(equatable);
82+
}
83+
84+
public override int GetHashCode() =>
85+
(horizontalAngularOffsetDeg, verticalAngularOffsetDeg, verticalLinearOffsetMm, ringId, timeOffset, minRange, maxRange).GetHashCode();
6486
}
6587
}

‎Assets/RGLUnityPlugin/Scripts/LidarModels/LaserArray.cs

+27-2
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ namespace RGLUnityPlugin
2222
/// Describes a LiDAR's array of lasers.
2323
/// </summary>
2424
[System.Serializable]
25-
public struct LaserArray
25+
public struct LaserArray : IEquatable<LaserArray>
2626
{
2727
// |
2828
// .---------------------.
@@ -126,5 +126,30 @@ private static float mmToMeters(float mm)
126126
{
127127
return mm / 1000.0f;
128128
}
129+
130+
//// IEquatable interface
131+
public bool Equals(LaserArray other)
132+
{
133+
if (lasers.Length != other.lasers.Length)
134+
{
135+
return false;
136+
}
137+
for (int i = 0; i < lasers.Length; i++)
138+
{
139+
if (!lasers[i].Equals(other.lasers[i]))
140+
return false;
141+
}
142+
143+
return this.centerOfMeasurementLinearOffsetMm.Equals(other.centerOfMeasurementLinearOffsetMm) &&
144+
this.focalDistanceMm == other.focalDistanceMm;
145+
}
146+
147+
public override bool Equals(object obj)
148+
{
149+
return obj is LaserArray equatable && Equals(equatable);
150+
}
151+
152+
public override int GetHashCode() =>
153+
(centerOfMeasurementLinearOffsetMm, focalDistanceMm, lasers.GetHashCode()).GetHashCode();
129154
}
130-
}
155+
}

‎Assets/RGLUnityPlugin/Scripts/LidarModels/LidarConfiguration.cs

+61
Original file line numberDiff line numberDiff line change
@@ -125,6 +125,29 @@ public Matrix4x4 GetLidarOriginTransfrom()
125125
{
126126
return Matrix4x4.Translate(laserArray.centerOfMeasurementLinearOffsetMm / 1000.0f);
127127
}
128+
129+
/// <summary>
130+
/// Validates if the configuration is the same as the default configuration for the given model.
131+
/// Parameters that can be modified in the real-world LiDARs are omitted from the comparison.
132+
/// </summary>
133+
public virtual bool ValidateWithModel(LidarModel model)
134+
{
135+
return ValidateWithModel(LidarConfigurationLibrary.ByModel[model]());
136+
}
137+
138+
// Need to create separate method with gold config parameter for derived classes.
139+
// In some cases (e.g. in `HesaiPandar128E4XLidarConfiguration`) gold must be configured additionally.
140+
protected bool ValidateWithModel(BaseLidarConfiguration gold)
141+
{
142+
return laserArrayCycleTime == gold.laserArrayCycleTime &&
143+
beamDivergence == gold.beamDivergence &&
144+
noiseParams.Equals(gold.noiseParams) &&
145+
laserArray.Equals(gold.laserArray);
146+
// Omitted values
147+
// horizontalResolution == gold.horizontalResolution
148+
// minHAngle == gold.minHAngle
149+
// maxHAngle == gold.maxHAngle
150+
}
128151
}
129152

130153
/// <summary>
@@ -151,6 +174,15 @@ public override Vector2[] GetRayRanges()
151174
{
152175
return new Vector2[1] {new Vector2(minRange, maxRange)};
153176
}
177+
178+
public override bool ValidateWithModel(LidarModel model)
179+
{
180+
var gold = LidarConfigurationLibrary.ByModel[model]();
181+
return base.ValidateWithModel(gold) &&
182+
gold is UniformRangeLidarConfiguration goldTyped &&
183+
minRange == goldTyped.minRange &&
184+
maxRange == goldTyped.maxRange;
185+
}
154186
}
155187

156188
/// <summary>
@@ -183,6 +215,13 @@ public override Vector2[] GetRayRanges()
183215
}
184216
return rayRanges;
185217
}
218+
219+
public override bool ValidateWithModel(LidarModel model)
220+
{
221+
var gold = LidarConfigurationLibrary.ByModel[model]();
222+
return base.ValidateWithModel(gold) &&
223+
gold is HesaiAT128LidarConfiguration;
224+
}
186225
}
187226

188227
/// <summary>
@@ -222,6 +261,13 @@ public override Matrix4x4[] GetRayPoses()
222261
}
223262
return rayPoses;
224263
}
264+
265+
public override bool ValidateWithModel(LidarModel model)
266+
{
267+
var gold = LidarConfigurationLibrary.ByModel[model]();
268+
return base.ValidateWithModel(gold) &&
269+
gold is HesaiQT128C2XLidarConfiguration;
270+
}
225271
}
226272

227273
/// <summary>
@@ -281,5 +327,20 @@ public override Matrix4x4[] GetRayPoses()
281327
}
282328
return rayPoses;
283329
}
330+
331+
public override bool ValidateWithModel(LidarModel model)
332+
{
333+
var gold = LidarConfigurationLibrary.ByModel[model]() as HesaiPandar128E4XLidarConfiguration;
334+
if (gold == null)
335+
{
336+
return false;
337+
}
338+
339+
// Set the same high resolution mode flag to the gold config
340+
// Laser array changes for standard and high resolution mode
341+
gold.highResolutionModeEnabled = highResolutionModeEnabled;
342+
343+
return base.ValidateWithModel(gold);
344+
}
284345
}
285346
}

‎Assets/RGLUnityPlugin/Scripts/LidarModels/LidarNoiseParams.cs

+21-2
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
using System;
1516
using UnityEngine;
1617

1718
namespace RGLUnityPlugin
@@ -23,7 +24,7 @@ public enum AngularNoiseType
2324
}
2425

2526
[System.Serializable]
26-
public struct LidarNoiseParams
27+
public struct LidarNoiseParams : IEquatable<LidarNoiseParams>
2728
{
2829
[Tooltip("Angular noise type")]
2930
public AngularNoiseType angularNoiseType;
@@ -62,5 +63,23 @@ public struct LidarNoiseParams
6263
angularNoiseMean = 0.0f,
6364
distanceNoiseMean = 0.0f,
6465
};
66+
67+
//// IEquatable interface
68+
public bool Equals(LidarNoiseParams other)
69+
{
70+
return this.angularNoiseType == other.angularNoiseType &&
71+
this.angularNoiseStDev == other.angularNoiseStDev &&
72+
this.angularNoiseMean == other.angularNoiseMean &&
73+
this.distanceNoiseStDevBase == other.distanceNoiseStDevBase &&
74+
this.distanceNoiseStDevRisePerMeter == other.distanceNoiseStDevRisePerMeter &&
75+
this.distanceNoiseMean == other.distanceNoiseMean;
76+
}
77+
78+
public override bool Equals(object obj)
79+
{
80+
return obj is LidarNoiseParams equatable && Equals(equatable);
81+
}
82+
83+
public override int GetHashCode() => (angularNoiseType, angularNoiseStDev, angularNoiseMean, distanceNoiseStDevBase, distanceNoiseStDevRisePerMeter, distanceNoiseMean).GetHashCode();
6584
}
66-
}
85+
}

‎Assets/RGLUnityPlugin/Scripts/LidarSensor.cs

+17-16
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,7 @@ namespace RGLUnityPlugin
2323
/// </summary>
2424
public class LidarSensor : MonoBehaviour
2525
{
26-
/// <summary>
27-
/// Sensor processing and callbacks are automatically called in this hz.
28-
/// </summary>
26+
[Tooltip("Sensor processing and callbacks are automatically called in this hz")]
2927
[FormerlySerializedAs("OutputHz")]
3028
[Range(0, 50)] public int AutomaticCaptureHz = 10;
3129

@@ -44,27 +42,21 @@ public class LidarSensor : MonoBehaviour
4442
/// </summary>
4543
public OnNewDataDelegate onLidarModelChange;
4644

47-
/// <summary>
48-
/// Allows to select one of built-in LiDAR models.
49-
/// Defaults to a range meter to ensure the choice is conscious.
50-
/// </summary>
45+
[Tooltip("Allows to select one of built-in LiDAR models")]
5146
public LidarModel modelPreset = LidarModel.RangeMeter;
5247

53-
/// <summary>
54-
/// Allows to quickly enable/disable distance gaussian noise.
55-
/// </summary>
48+
[Tooltip("Allows to quickly enable/disable distance gaussian noise")]
5649
public bool applyDistanceGaussianNoise = true;
5750

58-
/// <summary>
59-
/// Allows to quickly enable/disable angular gaussian noise.
60-
/// </summary>
51+
[Tooltip("Allows to quickly enable/disable angular gaussian noise")]
6152
public bool applyAngularGaussianNoise = true;
6253

63-
/// <summary>
64-
/// Allows to quickly enable/disable velocity distortion
65-
/// </summary>
54+
[Tooltip("Allows to quickly enable/disable velocity distortion")]
6655
public bool applyVelocityDistortion = false;
6756

57+
[Tooltip("If enabled, validates whether the configuration is the same as the manual for the selected model (only on startup)")]
58+
public bool doValidateConfigurationOnStartup = true;
59+
6860
/// <summary>
6961
/// Encapsulates description of a point cloud generated by a LiDAR and allows for fine-tuning.
7062
/// </summary>
@@ -146,6 +138,15 @@ public void Start()
146138
}
147139

148140
OnValidate();
141+
142+
if (doValidateConfigurationOnStartup)
143+
{
144+
if (!configuration.ValidateWithModel(modelPreset))
145+
{
146+
Debug.LogWarning($"{name}: the configuration of the selected model preset ({modelPreset.ToString()}) is modified. " +
147+
"Ignore this warning if you have consciously changed them.");
148+
}
149+
}
149150
}
150151

151152
public void OnValidate()

0 commit comments

Comments
 (0)