Add flag to ensure hesai pandar driver compatibility

tier4 · Sep 25, 2024 · e370b8b · e370b8b
1 parent 06a71ee
commit e370b8b
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 5 deletions.
diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
@@ -16,7 +16,6 @@
 using UnityEngine;
 using System.Collections.Generic;
 using System.Linq;
-using System.Net.NetworkInformation;
 using UnityEngine.Assertions;
 
 namespace RGLUnityPlugin
@@ -47,6 +46,9 @@ public class LidarUdpPublisher : MonoBehaviour
         [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
 
+        [Tooltip("Hesai Pandar ROS2 driver has some differences from the LiDAR manuals. This flag applies these changes to raw packets.")]
+        public bool ensureCompatibilityWithHesaiPandarDriver = false;
+
         private RGLNodeSequence rglSubgraphUdpPublishing;
 
         private const string udpPublishingNodeId = "UDP_PUBLISHING";
@@ -292,12 +294,16 @@ private RGLUdpOptions GetUdpOptions(LidarModel currentLidarModel)
                 Debug.LogWarning($"{name}: enableHesaiUpCloseBlockageDetection option is only available for Hesai QT128C2X LiDAR model. Disabling option...");
             }
 
+            // Other LiDAR models are compatible with Hesai Pandar Driver by default
+            bool enableHesaiPandarDriverCompatibilityForQt = currentLidarModel == LidarModel.HesaiPandarQT && ensureCompatibilityWithHesaiPandarDriver;
+
             // 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;
+            udpOptions += enableHesaiPandarDriverCompatibilityForQt ? (UInt32)RGLUdpOptions.RGL_UDP_FIT_QT64_TO_HESAI_PANDAR_DRIVER : 0;
 
             // Check if high resolution mode is enabled (available only on Hesai Pandar128E4X)
             if (currentLidarModel == LidarModel.HesaiPandar128E4X)

diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
@@ -143,10 +143,11 @@ public enum RGLRadarObjectClass : Int32
 
 	public enum RGLUdpOptions : UInt32
 	{
-		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,
+		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,
+		RGL_UDP_FIT_QT64_TO_HESAI_PANDAR_DRIVER = 1 << 3,
 	};
 
 	public enum RGLQosPolicyReliability