diff --git a/Assets/RGLUnityPlugin/NebulaCalibrations.meta b/Assets/RGLUnityPlugin/NebulaCalibrations.meta
new file mode 100644
index 000000000..23c7f0858
--- /dev/null
+++ b/Assets/RGLUnityPlugin/NebulaCalibrations.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 00ad0d8f94d922945b956a77237bbb67
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv b/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv
new file mode 100644
index 000000000..1e84a901b
--- /dev/null
+++ b/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv
@@ -0,0 +1,129 @@
+Laser id,Elevation,Azimuth
+1,14.985,0.186
+2,13.283,0.185
+3,11.758,1.335
+4,10.483,1.343
+5,9.836,0.148
+6,9.171,0.147
+7,8.496,0.146
+8,7.812,0.146
+9,7.462,1.335
+10,7.115,1.336
+11,6.767,1.337
+12,6.416,1.338
+13,6.064,1.339
+14,5.71,1.34
+15,5.355,1.341
+16,4.998,1.342
+17,4.643,0.128
+18,4.282,0.128
+19,3.921,0.127
+20,3.558,0.127
+21,3.194,0.107
+22,2.829,0.106
+23,2.463,0.105
+24,2.095,0.105
+25,1.974,-3.118
+26,1.854,1.315
+27,1.729,4.529
+28,1.609,-3.121
+29,1.487,1.316
+30,1.362,4.532
+31,1.242,-3.124
+32,1.12,1.317
+33,0.995,4.536
+34,0.875,-3.127
+35,0.75,1.317
+36,0.625,4.539
+37,0.5,-3.13
+38,0.375,1.318
+39,0.25,4.542
+40,0.125,-3.133
+41,0,0.103
+42,-0.125,2.935
+43,-0.25,-1.517
+44,-0.375,0.103
+45,-0.5,2.937
+46,-0.626,-1.519
+47,-0.751,0.103
+48,-0.876,2.939
+49,-1.001,-1.52
+50,-1.126,0.103
+51,-1.251,2.941
+52,-1.377,-1.521
+53,-1.502,0.102
+54,-1.627,2.943
+55,-1.751,-1.523
+56,-1.876,0.102
+57,-2.001,2.945
+58,-2.126,-1.524
+59,-2.251,0.102
+60,-2.376,2.946
+61,-2.501,-1.526
+62,-2.626,0.102
+63,-2.751,2.948
+64,-2.876,-1.526
+65,-3.001,1.324
+66,-3.126,4.57
+67,-3.251,-3.155
+68,-3.376,1.325
+69,-3.501,4.573
+70,-3.626,-3.157
+71,-3.751,1.326
+72,-3.876,4.575
+73,-4.001,-3.159
+74,-4.126,1.326
+75,-4.25,4.578
+76,-4.375,-3.161
+77,-4.501,1.327
+78,-4.626,4.581
+79,-4.751,-3.163
+80,-4.876,1.328
+81,-5.001,4.583
+82,-5.126,-3.165
+83,-5.252,1.329
+84,-5.377,4.586
+85,-5.502,-3.167
+86,-5.626,1.329
+87,-5.752,4.588
+88,-5.877,-3.168
+89,-6.002,0.102
+90,-6.378,0.103
+91,-6.754,0.103
+92,-7.13,0.103
+93,-7.507,0.104
+94,-7.882,0.104
+95,-8.257,0.104
+96,-8.632,0.104
+97,-9.003,1.337
+98,-9.376,1.337
+99,-9.749,1.338
+100,-10.121,1.339
+101,-10.493,1.34
+102,-10.864,1.341
+103,-11.234,1.341
+104,-11.603,1.342
+105,-11.975,0.108
+106,-12.343,0.108
+107,-12.709,0.109
+108,-13.075,0.109
+109,-13.439,0.13
+110,-13.803,0.131
+111,-14.164,0.131
+112,-14.525,0.132
+113,-14.879,1.384
+114,-15.237,1.384
+115,-15.593,1.385
+116,-15.948,1.385
+117,-16.299,1.386
+118,-16.651,1.386
+119,-17,1.387
+120,-17.347,1.387
+121,-17.701,0.151
+122,-18.386,0.153
+123,-19.063,0.154
+124,-19.73,0.156
+125,-20.376,1.388
+126,-21.653,1.408
+127,-23.044,0.196
+128,-24.765,0.286
diff --git a/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv.meta b/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv.meta
new file mode 100644
index 000000000..c49b9c7b5
--- /dev/null
+++ b/Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv.meta
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: f3365c80bbf7055559fee701f48c7dbb
+TextScriptImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv b/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv
new file mode 100644
index 000000000..1fc0b14f4
--- /dev/null
+++ b/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv
@@ -0,0 +1,129 @@
+Laser id,Elevation,Azimuth
+1,-52.62676282,10.10830596
+2,-51.0280939,9.719503673
+3,-49.51495392,9.384265827
+4,-48.07394795,9.091433335
+5,-46.69466297,8.832899657
+6,-45.36882812,8.602605729
+7,-44.08974439,8.395920495
+8,-42.85190128,8.209210679
+9,-41.65069769,8.03961095
+10,-40.48225401,7.8848012
+11,-39.34326358,7.742887867
+12,-38.23088092,7.612311759
+13,-37.14263654,7.491771683
+14,-36.07637316,7.380164213
+15,-35.03019446,7.276562397
+16,-34.00242585,7.180171603
+17,-32.99157758,7.090302002
+18,-31.99631045,7.006369095
+19,-31.01543179,6.927848973
+20,-30.04786472,6.854295551
+21,-29.09262707,6.785306938
+22,-28.14883195,6.720536921
+23,-27.21566989,6.659666871
+24,-26.29240356,6.602428464
+25,-25.37835081,6.548569874
+26,-24.47288318,6.497872808
+27,-23.57542315,6.450129948
+28,-22.68544071,6.405178951
+29,-21.80243688,6.362840261
+30,-20.92594329,6.32298504
+31,-20.05553602,6.285467298
+32,-19.19081372,6.25017485
+33,-18.33139172,-6.21699538
+34,-17.47691901,-6.18583344
+35,-16.62705442,-6.156599148
+36,-15.78149317,-6.129208192
+37,-14.93993032,-6.103581848
+38,-14.10208035,-6.079663981
+39,-13.26767188,-6.057381402
+40,-12.43645574,-6.036683527
+41,-11.60818201,-6.017519726
+42,-10.78261371,-5.999833659
+43,-9.959523504,-5.983597281
+44,-9.138697037,-5.968771181
+45,-8.319929599,-5.955310246
+46,-7.503005408,-5.943192006
+47,-6.687807162,-5.926719867
+48,-5.873926875,-5.922876604
+49,-5.061393513,-5.914628713
+50,-4.249948322,-5.907639071
+51,-3.43941536,-5.901885097
+52,-2.629617595,-5.897355529
+53,-1.820378325,-5.894039091
+54,-1.011522452,-5.891935825
+55,-0.202883525,-5.89103442
+56,0.605717878,-5.89132922
+57,1.414444374,-5.892831558
+58,2.223469054,-5.895541414
+59,3.032962884,-5.899458754
+60,3.8431104,-5.904600534
+61,4.654074511,-5.910966684
+62,5.466034723,-5.918579795
+63,6.279180222,-5.927451101
+64,7.093696848,-5.937608821
+65,7.909760474,5.949064148
+66,8.727579858,5.961845258
+67,9.54735011,5.97598597
+68,10.36927364,5.99150874
+69,11.19356846,6.008464331
+70,12.0204614,6.026875139
+71,12.85016831,6.046797524
+72,13.68294669,6.068282136
+73,14.51903314,6.091373918
+74,15.35869922,6.116134758
+75,16.20222125,6.142626487
+76,17.04988688,6.170927865
+77,17.90200196,6.201111915
+78,18.75890049,6.233268566
+79,19.62091911,6.267487655
+80,20.48842178,6.303870236
+81,21.36180725,6.342539884
+82,22.24149217,6.383614369
+83,23.12791172,6.427233938
+84,24.02155359,6.473555624
+85,24.92293181,6.52274755
+86,25.83260224,6.574994555
+87,26.75116558,6.63050945
+88,27.67927338,6.689521655
+89,28.61763387,6.752299732
+90,29.56702472,6.819128697
+91,30.52828971,6.890343815
+92,31.50236238,6.966319159
+93,32.49026442,7.0474731
+94,33.49313666,7.134285032
+95,34.51223682,7.227334588
+96,35.54897914,7.327222327
+97,19.19081372,-6.25017485
+98,20.05553602,-6.285467298
+99,20.92594329,-6.32298504
+100,21.80243688,-6.362840261
+101,22.68544071,-6.405178951
+102,23.57542315,-6.450129948
+103,24.47288318,-6.497872808
+104,25.37835081,-6.548569874
+105,26.29240356,-6.602428464
+106,27.21566989,-6.659666871
+107,28.14883195,-6.720536921
+108,29.09262707,-6.785306938
+109,30.04786472,-6.854295551
+110,31.01543179,-6.927848973
+111,31.99631045,-7.006369095
+112,32.99157758,-7.090302002
+113,34.00242585,-7.180171603
+114,35.03019446,-7.276562397
+115,36.07637316,-7.380164213
+116,37.14263654,-7.491771683
+117,38.23088092,-7.612311759
+118,39.34326358,-7.742887867
+119,40.48225401,-7.8848012
+120,41.65069769,-8.03961095
+121,42.85190128,-8.209210679
+122,44.08974439,-8.395920495
+123,45.36882812,-8.602605729
+124,46.69466297,-8.832899657
+125,48.07394795,-9.091433335
+126,49.51495392,-9.384265827
+127,51.0280939,-9.719503673
+128,52.62676282,-10.10830596
diff --git a/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv.meta b/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv.meta
new file mode 100644
index 000000000..084b7d4f8
--- /dev/null
+++ b/Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv.meta
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 4d935bc0aefccf84a88a4632d6999616
+TextScriptImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RGLUnityPlugin/Scripts/LidarModels/Laser.cs b/Assets/RGLUnityPlugin/Scripts/LidarModels/Laser.cs
index 210beecc0..d6cf7a9db 100644
--- a/Assets/RGLUnityPlugin/Scripts/LidarModels/Laser.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LidarModels/Laser.cs
@@ -12,8 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-using System;
-
namespace RGLUnityPlugin
{
///
@@ -22,7 +20,7 @@ namespace RGLUnityPlugin
/// Numbers are expressed in terms of Unity axes convention.
///
[System.Serializable]
- public struct Laser : IEquatable
+ public struct Laser
{
///
/// Rotation around Y-axis.
@@ -63,24 +61,5 @@ public struct Laser : IEquatable
/// Note: May be ignored for some `LidarConfiguration`s (e.g. `UniformRangeLidarConfiguration`)
///
public float maxRange;
-
- //// IEquatable interface
- public bool Equals(Laser other)
- {
- return this.horizontalAngularOffsetDeg == other.horizontalAngularOffsetDeg &&
- this.verticalAngularOffsetDeg == other.verticalAngularOffsetDeg &&
- this.verticalLinearOffsetMm == other.verticalLinearOffsetMm &&
- this.ringId == other.ringId &&
- this.timeOffset == other.timeOffset &&
- this.minRange == other.minRange &&
- this.maxRange == other.maxRange;
- }
-
- public override bool Equals(object obj)
- {
- return obj is Laser equatable && Equals(equatable);
- }
-
- public override int GetHashCode() => (horizontalAngularOffsetDeg, verticalAngularOffsetDeg, verticalLinearOffsetMm, ringId, timeOffset, minRange, maxRange).GetHashCode();
}
}
diff --git a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs
index dc01753ba..e29e15d09 100644
--- a/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LidarSensor.cs
@@ -160,6 +160,7 @@ public void OnValidate()
}
ApplyConfiguration(configuration);
validatedPreset = modelPreset;
+ onLidarModelChange?.Invoke();
}
private void ApplyConfiguration(BaseLidarConfiguration newConfig)
@@ -169,8 +170,6 @@ private void ApplyConfiguration(BaseLidarConfiguration newConfig)
return;
}
- onLidarModelChange?.Invoke();
-
rglGraphLidar.UpdateNodeRaysFromMat3x4f(lidarRaysNodeId, newConfig.GetRayPoses())
.UpdateNodeRaysSetRange(lidarRangeNodeId, newConfig.GetRayRanges())
.UpdateNodeRaysSetRingIds(lidarRingsNodeId, newConfig.GetRayRingIds())
diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
index 6b6d97634..588c3282f 100644
--- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
@@ -12,16 +12,20 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using System.Net.NetworkInformation;
+using UnityEngine.Assertions;
namespace RGLUnityPlugin
{
[RequireComponent(typeof(LidarSensor))]
public class LidarUdpPublisher : MonoBehaviour
{
+ public RGLReturnMode returnMode = RGLReturnMode.SingleReturnStrongest;
+
public string sourceIP = "0.0.0.0";
public string destinationIP = "255.255.255.255";
public int destinationPort = 2368;
@@ -30,18 +34,21 @@ public class LidarUdpPublisher : MonoBehaviour
private string destinationIPOnAwake;
private int destinationPortOnAwake;
+ public bool emitRawPackets = true;
+
+ [Header("Hesai LiDARs Flags")]
+
+ [Tooltip("Enable labeling the sequence number of Point Cloud UDP packets. It increases the packet size by an additional field.")]
public bool enableHesaiUdpSequence = false;
- public bool useDualReturnFormat = false; // Still single return data but packed in the dual return packet format
- // The second return will be the same as the first return
+ [Tooltip("Enable a workaround for the difference between the coordinate systems in the ROS2 driver and Hesai LiDAR manuals.")]
public bool ensureHesaiRosDriverOrientation = false; // When developing raw Hesai packets (based on LiDARs manuals),
// the difference between the coordinate systems in the ROS2 driver
// and LiDAR speciations was noted.
// Due to the use of the ROS driver on many real vehicles,
// it was decided to prepare a workaround in AWSIM.
- public bool emitRawPackets = true;
+ [Tooltip("Enable a feature that allows to distinguish point data between no laser emission and return signal rejection.")]
+ public bool enableHesaiUpCloseBlockageDetection = false; // Only supported for Hesai QT128C2X
- private RGLLidarModel currentRGLLidarModel = 0; // To be set when validating lidar model
- private RGLUdpOptions currentRGLUdpOptions = 0; // To be set when validating lidar model
private RGLNodeSequence rglSubgraphUdpPublishing;
private const string udpPublishingNodeId = "UDP_PUBLISHING";
@@ -56,9 +63,49 @@ public class LidarUdpPublisher : MonoBehaviour
{ LidarModel.VelodyneVLP32C, RGLLidarModel.RGL_VELODYNE_VLP32C },
{ LidarModel.VelodyneVLS128, RGLLidarModel.RGL_VELODYNE_VLS128 },
{ LidarModel.HesaiPandar40P, RGLLidarModel.RGL_HESAI_PANDAR_40P },
- { LidarModel.HesaiPandarQT, RGLLidarModel.RGL_HESAI_PANDAR_QT64 }
+ { LidarModel.HesaiPandarQT, RGLLidarModel.RGL_HESAI_PANDAR_QT64 },
+ { LidarModel.HesaiQT128C2X, RGLLidarModel.RGL_HESAI_QT128C2X },
+ { LidarModel.HesaiPandar128E4X, RGLLidarModel.RGL_HESAI_PANDAR_128E4X }
};
+ // Note: When selecting dual return mode, there will be still single return data but packed in the dual return packet format
+ // The second return will be the same as the first return
+ private static readonly Dictionary> SupportedLidarsAndReturnModes = new Dictionary>
+ {
+ { LidarModel.VelodyneVLP16, new List()
+ { RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
+ { LidarModel.VelodyneVLP32C, new List()
+ { RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
+ { LidarModel.VelodyneVLS128, new List()
+ { RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
+ { LidarModel.HesaiPandar40P, new List()
+ { RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnLastStrongest } },
+ { LidarModel.HesaiPandarQT, new List()
+ { RGLReturnMode.SingleReturnFirst, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnFirstLast } },
+ { LidarModel.HesaiQT128C2X, new List()
+ { RGLReturnMode.SingleReturnFirst, RGLReturnMode.SingleReturnSecond, RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast,
+ RGLReturnMode.DualReturnLastStrongest, RGLReturnMode.DualReturnFirstLast, RGLReturnMode.DualReturnFirstStrongest,
+ RGLReturnMode.DualReturnStrongestSecondStrongest, RGLReturnMode.DualReturnFirstSecond } },
+ { LidarModel.HesaiPandar128E4X, new List()
+ { RGLReturnMode.SingleReturnFirst, RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast,
+ RGLReturnMode.DualReturnLastStrongest, RGLReturnMode.DualReturnFirstLast, RGLReturnMode.DualReturnFirstStrongest } }
+ };
+
+ private bool IsVelodyne(LidarModel model)
+ {
+ return model == LidarModel.VelodyneVLP16 ||
+ model == LidarModel.VelodyneVLP32C ||
+ model == LidarModel.VelodyneVLS128;
+ }
+
+ private bool IsHesai(LidarModel model)
+ {
+ return model == LidarModel.HesaiPandar40P ||
+ model == LidarModel.HesaiPandarQT ||
+ model == LidarModel.HesaiQT128C2X ||
+ model == LidarModel.HesaiPandar128E4X;
+ }
+
// To be called when adding this component
private void Reset()
{
@@ -87,29 +134,25 @@ private void Awake()
// Node parameters will be updated when validating lidar model
rglSubgraphUdpPublishing = new RGLNodeSequence()
- .AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS,
+ .AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLReturnMode.SingleReturnStrongest, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS,
sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake);
}
private void Start()
{
lidarSensor = GetComponent();
- lidarSensor.onLidarModelChange += ValidateLidarModel;
+ lidarSensor.onLidarModelChange += HandleNewLidarModel;
// We can connect to world frame, because we need only DISTANCE field which is in lidar frame anyway.
// This way we don't need to duplicate transform nodes to have compacted and non-compacted point cloud in lidar frame.
lidarSensor.ConnectToWorldFrame(rglSubgraphUdpPublishing, false);
- if (ensureHesaiRosDriverOrientation)
- {
- OnValidate(); // Needed to handle this flag on startup
- }
-
- ValidateLidarModel();
+ HandleNewLidarModel();
}
public void OnValidate()
{
+ // `rglSubgraphUdpPublishing` is constructed on simulation startup. `OnValidate` works only in runtime.
if (rglSubgraphUdpPublishing == null)
{
return;
@@ -138,21 +181,13 @@ public void OnValidate()
rglSubgraphUdpPublishing.SetActive(udpPublishingNodeId, emitRawPackets);
}
- if (ensureHesaiRosDriverOrientation && (lidarSensor.configuration.minHAngle != -90.0f || lidarSensor.configuration.maxHAngle != 270.0f))
- {
- lidarSensor.configuration.minHAngle = -90.0f;
- lidarSensor.configuration.maxHAngle = 270.0f;
- lidarSensor.OnValidate();
- return; // UpdateRGLSubgraph() will be called when validating new LiDAR model configuration
- }
-
- UpdateRGLSubgraph();
+ HandleNewLidarModel();
}
public void OnEnable()
{
rglSubgraphUdpPublishing?.SetActive(udpPublishingNodeId, emitRawPackets);
- ValidateLidarModel();
+ HandleNewLidarModel();
}
public void OnDisable()
@@ -167,7 +202,7 @@ public void OnDestroy()
rglSubgraphUdpPublishing = null;
}
- private void ValidateLidarModel()
+ private void HandleNewLidarModel()
{
if (lidarSensor == null || !enabled)
{
@@ -175,52 +210,102 @@ private void ValidateLidarModel()
}
var modelToValidate = lidarSensor.configuration;
+ // Horizontal field of view must be 360 degrees
if (modelToValidate.maxHAngle - modelToValidate.minHAngle != 360)
{
- Debug.LogError("Lidar model configuration not supported for UDP publishing " +
+ Debug.LogError($"{name}: Lidar model configuration not supported for UDP publishing " +
"- horizontal field of view different than 360 degrees. Disabling component...");
OnDisable();
}
- LidarModel? detectedUnityLidarModel = null;
- foreach(var unityLidarModel in UnityToRGLLidarModelsMapping.Keys)
+ LidarModel currentLidarModel = lidarSensor.modelPreset;
+
+ if (!UnityToRGLLidarModelsMapping.ContainsKey(currentLidarModel))
{
- var unityLidarConfig = LidarConfigurationLibrary.ByModel[unityLidarModel]();
- if (modelToValidate.laserArray.lasers.SequenceEqual(unityLidarConfig.laserArray.lasers))
- {
- detectedUnityLidarModel = unityLidarModel;
- break;
- }
+ Debug.LogError($"{name}: Lidar model preset not supported for UDP publishing. " +
+ $"Please select one of: [{string.Join(", ", UnityToRGLLidarModelsMapping.Keys)}]. Disabling component...");
+ OnDisable();
+ return;
}
-
- if (detectedUnityLidarModel == null)
+ if (!SupportedLidarsAndReturnModes.ContainsKey(currentLidarModel) || SupportedLidarsAndReturnModes[currentLidarModel].Count == 0)
{
- Debug.LogError("Lidar model configuration not supported for UDP publishing " +
- $"- lasers doesn't match any supported Lidar models ({string.Join(", ", UnityToRGLLidarModelsMapping.Keys)}). Disabling component...");
+ Debug.LogError($"{name}: Lidar model preset doesn't have specification for supported return modes. It is most likely due to implementation error. " +
+ "Please contact to the project maintainers. Disabling component...");
OnDisable();
return;
}
// Check if lidar configuration doesn't exceed max range for Velodyne Legacy Packet Format
// Currently, all of the supported Velodyne models use this packet format
- if (IsVelodyne((LidarModel)detectedUnityLidarModel) && modelToValidate.GetRayRanges().Max(v => v.y) > maxRangeForVelodyneLegacyPacketFormat)
+ if (IsVelodyne(currentLidarModel) && modelToValidate.GetRayRanges().Max(v => v.y) > maxRangeForVelodyneLegacyPacketFormat)
{
Debug.LogWarning($"Max range of lidar '{lidarSensor.name}' exceeds max range supported by Velodyne Legacy Packet Format ({maxRangeForVelodyneLegacyPacketFormat}m). " +
"Consider reducing its value to ensure proper work.");
}
- currentRGLLidarModel = UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value];
- UpdateRGLSubgraph();
- }
+ // This is a workaround for the difference between the coordinate systems in the ROS2 driver and Hesai LiDAR manuals
+ // The order of the points is changed to be encoded to match ROS2 coordinate system
+ if (ensureHesaiRosDriverOrientation && IsHesai(currentLidarModel) && (lidarSensor.configuration.minHAngle != -90.0f || lidarSensor.configuration.maxHAngle != 270.0f))
+ {
+ lidarSensor.configuration.minHAngle = -90.0f;
+ lidarSensor.configuration.maxHAngle = 270.0f;
+ lidarSensor.OnValidate(); // This will trigger `HandleNewLidarModel()` again
+ return;
+ }
- private void UpdateRGLSubgraph()
- {
- HandleUdpOptionsFlags();
+ // Check if supported return mode is selected
+ if (!SupportedLidarsAndReturnModes[currentLidarModel].Contains(returnMode))
+ {
+ Debug.LogError($"{name}: Return mode for selected lidar model preset is not supported. " +
+ $"Please select one of: [{string.Join(", ", SupportedLidarsAndReturnModes[currentLidarModel])}]. " +
+ "Setting the first supported return mode...");
+ returnMode = SupportedLidarsAndReturnModes[currentLidarModel][0];
+ }
+
+ // Update RGL subgraph
rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish(
- udpPublishingNodeId, currentRGLLidarModel, currentRGLUdpOptions,
+ udpPublishingNodeId, UnityToRGLLidarModelsMapping[currentLidarModel], returnMode, GetUdpOptions(currentLidarModel),
sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake);
}
+ private RGLUdpOptions GetUdpOptions(LidarModel currentLidarModel)
+ {
+ // Validate current model and option flags
+ if (!IsHesai(currentLidarModel) && enableHesaiUdpSequence)
+ {
+ enableHesaiUdpSequence = false;
+ Debug.LogWarning($"{name}: enableHesaiUdpSequence option is not available for selected LiDAR model. Disabling option...");
+ }
+ if ((currentLidarModel == LidarModel.HesaiQT128C2X || currentLidarModel == LidarModel.HesaiPandar128E4X) && !enableHesaiUdpSequence)
+ {
+ enableHesaiUdpSequence = true;
+ Debug.LogWarning($"{name}: enableHesaiUdpSequence option must be enabled for selected LiDAR model. Enabling option...");
+ }
+ if (currentLidarModel != LidarModel.HesaiQT128C2X && enableHesaiUpCloseBlockageDetection)
+ {
+ enableHesaiUpCloseBlockageDetection = false;
+ Debug.LogWarning($"{name}: enableHesaiUpCloseBlockageDetection option is only available for Hesai QT128C2X LiDAR model. Disabling option...");
+ }
+
+ // Construct RGLUdpOptions
+ // We need to cast to the underlying type of the enum to be able to add multiple udp options
+ Assert.IsTrue(Enum.GetUnderlyingType(typeof(RGLUdpOptions)) == typeof(UInt32)); // Check if we are casting properly
+ UInt32 udpOptions = (UInt32)RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
+ udpOptions += enableHesaiUdpSequence ? (UInt32)RGLUdpOptions.RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE : 0;
+ udpOptions += enableHesaiUpCloseBlockageDetection ? (UInt32)RGLUdpOptions.RGL_UDP_UP_CLOSE_BLOCKAGE_DETECTION : 0;
+
+ // Check if high resolution mode is enabled (available only on Hesai Pandar128E4X)
+ if (currentLidarModel == LidarModel.HesaiPandar128E4X)
+ {
+ if (((HesaiPandar128E4XLidarConfiguration)lidarSensor.configuration).highResolutionModeEnabled)
+ {
+ udpOptions += (UInt32)RGLUdpOptions.RGL_UDP_HIGH_RESOLUTION_MODE;
+ }
+ }
+
+ return (RGLUdpOptions)udpOptions;
+ }
+
///
/// Returns true if component has been destroyed
///
@@ -231,7 +316,7 @@ private bool CheckAndDestroyComponentIfUnsupported()
return false;
}
- Debug.LogError($"Loaded RGL plugin does not include support for UDP Raw Packet, removing component");
+ Debug.LogError("Loaded RGL plugin does not include support for UDP Raw Packet, removing component");
if (Application.isEditor && !Application.isPlaying) // In edit mode
{
DestroyImmediate(this);
@@ -243,44 +328,6 @@ private bool CheckAndDestroyComponentIfUnsupported()
return true;
}
- private void HandleUdpOptionsFlags()
- {
- bool currentLidarIsHesai = currentRGLLidarModel == RGLLidarModel.RGL_HESAI_PANDAR_40P ||
- currentRGLLidarModel == RGLLidarModel.RGL_HESAI_PANDAR_QT64;
-
- if (!currentLidarIsHesai)
- {
- if (enableHesaiUdpSequence)
- {
- enableHesaiUdpSequence = false;
- currentRGLUdpOptions = RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
- Debug.LogWarning($"{name}: enableHesaiUdpSequence option is not available for selected LiDAR model. Disabling...");
- }
-
- if (useDualReturnFormat)
- {
- useDualReturnFormat = false;
- currentRGLUdpOptions = RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
- Debug.LogWarning($"{name}: useDualReturnFormat option is not available for selected LiDAR model. Disabling...");
- }
-
- return;
- }
-
- int udpOptionsConstruction = (int)RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
- udpOptionsConstruction += enableHesaiUdpSequence ? (int)RGLUdpOptions.RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE : 0;
- udpOptionsConstruction += useDualReturnFormat ? (int)RGLUdpOptions.RGL_UDP_DUAL_RETURN : 0;
-
- currentRGLUdpOptions = (RGLUdpOptions)udpOptionsConstruction;
- }
-
- private bool IsVelodyne(LidarModel model)
- {
- return model == LidarModel.VelodyneVLP16 ||
- model == LidarModel.VelodyneVLP32C ||
- model == LidarModel.VelodyneVLS128;
- }
-
private bool IsValidIpAddress(in string ip)
{
if (ip == "0.0.0.0") return true;
diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs
index ff85775d8..4d2772748 100644
--- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs
@@ -115,7 +115,7 @@ public static extern int rgl_node_points_ros2_publish_with_qos(
[DllImport("RobotecGPULidar")]
public static extern int rgl_node_points_udp_publish(
- ref IntPtr node, RGLLidarModel lidar_model, RGLUdpOptions udp_options, [MarshalAs(UnmanagedType.LPStr)] string device_ip,
+ ref IntPtr node, RGLLidarModel lidar_model, RGLReturnMode return_mode, RGLUdpOptions udp_options, [MarshalAs(UnmanagedType.LPStr)] string device_ip,
[MarshalAs(UnmanagedType.LPStr)] string dest_ip, int dest_port);
[DllImport("RobotecGPULidar")]
@@ -467,9 +467,9 @@ public static void NodePointsRos2PublishWithQos(
CheckErr(rgl_node_points_ros2_publish_with_qos(ref node, topicName, frameId, qos_reliability, qos_durability, qos_history, qos_depth));
}
- public static void NodePointsUdpPublish(ref IntPtr node, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
+ public static void NodePointsUdpPublish(ref IntPtr node, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
{
- CheckErr(rgl_node_points_udp_publish(ref node, lidarModel, udpOptions, deviceIp, destIp, destPort));
+ CheckErr(rgl_node_points_udp_publish(ref node, lidarModel, returnMode, udpOptions, deviceIp, destIp, destPort));
}
public static void NodeGaussianNoiseAngularRay(ref IntPtr node, float mean, float stDev)
diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
index 8c321a42e..5e7bf8d40 100644
--- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
@@ -88,13 +88,30 @@ public enum RGLLidarModel : Int32
RGL_VELODYNE_VLS128 = 3,
RGL_HESAI_PANDAR_40P = 4,
RGL_HESAI_PANDAR_QT64 = 5,
+ RGL_HESAI_QT128C2X = 6,
+ RGL_HESAI_PANDAR_128E4X = 7,
+ };
+
+ // Items have been renamed to be displayed in Unity nicer.
+ public enum RGLReturnMode : UInt32
+ {
+ /* RGL_RETURN_FIRST */ SingleReturnFirst = 1 << 29, // Three of the most significant bits encode the number of returns
+ /* RGL_RETURN_SECOND */ SingleReturnSecond,
+ /* RGL_RETURN_LAST */ SingleReturnLast,
+ /* RGL_RETURN_STRONGEST */ SingleReturnStrongest,
+ /* RGL_RETURN_LAST_STRONGEST */ DualReturnLastStrongest = 2 << 29,
+ /* RGL_RETURN_FIRST_LAST */ DualReturnFirstLast,
+ /* RGL_RETURN_FIRST_STRONGEST */ DualReturnFirstStrongest,
+ /* RGL_RETURN_STRONGEST_SECOND_STRONGEST */ DualReturnStrongestSecondStrongest,
+ /* RGL_RETURN_FIRST_SECOND */ DualReturnFirstSecond,
};
public enum RGLUdpOptions : UInt32
{
- RGL_UDP_NO_ADDITIONAL_OPTIONS = 0,
- RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE = 1 << 0,
- RGL_UDP_DUAL_RETURN = 1 << 1,
+ RGL_UDP_NO_ADDITIONAL_OPTIONS = 0,
+ RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE = 1 << 0,
+ RGL_UDP_HIGH_RESOLUTION_MODE = 1 << 1,
+ RGL_UDP_UP_CLOSE_BLOCKAGE_DETECTION = 1 << 2,
};
public enum RGLQosPolicyReliability
@@ -117,7 +134,7 @@ public enum RGLQosPolicyHistory
QOS_POLICY_HISTORY_KEEP_LAST = 1,
QOS_POLICY_HISTORY_KEEP_ALL = 2,
};
-
+
public enum RGLExtension : Int32
{
RGL_EXTENSION_PCL = 0,
diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
index 51c1928fe..feb699d5c 100644
--- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
+++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
@@ -226,11 +226,11 @@ public RGLNodeSequence AddNodePointsRos2Publish(
return this;
}
- public RGLNodeSequence AddNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
+ public RGLNodeSequence AddNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
{
CheckNodeNotExist(identifier);
RGLNodeHandle handle = new RGLNodeHandle();
- RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort);
+ RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, returnMode, udpOptions, deviceIp, destIp, destPort);
handle.Type = RGLNodeType.POINTS_UDP_PUBLISH;
handle.Identifier = identifier;
AddNode(handle);
@@ -376,10 +376,10 @@ public RGLNodeSequence UpdateNodePointsTemporalMerge(string identifier, RGLField
return this;
}
- public RGLNodeSequence UpdateNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
+ public RGLNodeSequence UpdateNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort)
{
RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_UDP_PUBLISH);
- RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort);
+ RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, returnMode, udpOptions, deviceIp, destIp, destPort);
return this;
}