From d898777d9e1608552e7fa081f61e305d796d040c Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 6 Feb 2024 11:37:40 +0100 Subject: [PATCH 1/3] Add raw packets support for Hesai QT128 and Hesai Pandar128E4X --- .../Scripts/LidarModels/Laser.cs | 23 +- .../Scripts/LidarUdpPublisher.cs | 217 +++++++++++------- .../Scripts/LowLevelWrappers/RGLNativeAPI.cs | 6 +- .../LowLevelWrappers/RGLNativeTypes.cs | 25 +- .../LowLevelWrappers/RGLNodeSequence.cs | 8 +- 5 files changed, 161 insertions(+), 118 deletions(-) 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/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; } From 60485c41dfe43f555adc8c2a54d28b83eea2c35b Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 6 Feb 2024 11:50:31 +0100 Subject: [PATCH 2/3] Add calibration files for nebula driver --- Assets/RGLUnityPlugin/NebulaCalibrations.meta | 8 ++ .../NebulaCalibrations/Pandar128E4X.csv | 129 ++++++++++++++++++ .../NebulaCalibrations/Pandar128E4X.csv.meta | 7 + .../NebulaCalibrations/PandarQT128.csv | 129 ++++++++++++++++++ .../NebulaCalibrations/PandarQT128.csv.meta | 7 + 5 files changed, 280 insertions(+) create mode 100644 Assets/RGLUnityPlugin/NebulaCalibrations.meta create mode 100644 Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv create mode 100644 Assets/RGLUnityPlugin/NebulaCalibrations/Pandar128E4X.csv.meta create mode 100644 Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv create mode 100644 Assets/RGLUnityPlugin/NebulaCalibrations/PandarQT128.csv.meta 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: From 1a3ff95ff324c7abb4b6616b67a0513eef47ed01 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Wed, 7 Feb 2024 16:20:30 +0100 Subject: [PATCH 3/3] Fix onLidarModelChange invocation --- Assets/RGLUnityPlugin/Scripts/LidarSensor.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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())