From f1fc4bfed6098126e94cca2b71a1ee84c283fc1f Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Sun, 19 Nov 2023 17:16:53 +0100 Subject: [PATCH 1/3] Add workaround to Hesai packets axis orientation --- .../Scripts/LidarUdpPublisher.cs | 63 ++++++++++++++++--- 1 file changed, 54 insertions(+), 9 deletions(-) diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs index c84fbfa01..9cd037aac 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs @@ -24,9 +24,19 @@ public class LidarUdpPublisher : MonoBehaviour public string sourceIP = "0.0.0.0"; public string destinationIP = "255.255.255.255"; public int destinationPort = 2368; + + private string sourceIPOnAwake; + private string destinationIPOnAwake; + private int destinationPortOnAwake; + 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 + 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; private RGLLidarModel currentRGLLidarModel = 0; // To be set when validating lidar model @@ -61,10 +71,14 @@ private void Awake() return; } + sourceIPOnAwake = sourceIP; + destinationIPOnAwake = destinationIP; + destinationPortOnAwake = destinationPort; + // Node parameters will be updated when validating lidar model rglSubgraphUdpPublishing = new RGLNodeSequence() .AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS, - sourceIP, destinationIP, destinationPort); + sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake); } private void Start() @@ -73,7 +87,7 @@ private void Start() lidarSensor.onLidarModelChange += ValidateLidarModel; // 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 dupicate transform nodes to have compacted and non-compacted point cloud in lidar frame. + // 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); ValidateLidarModel(); @@ -86,10 +100,38 @@ public void OnValidate() return; } + if (sourceIPOnAwake != sourceIP) + { + sourceIP = sourceIPOnAwake; + Debug.LogWarning("`sourceIP` parameter cannot be changed in simulation runtime"); + } + + if (destinationIPOnAwake != destinationIP) + { + destinationIP = destinationIPOnAwake; + Debug.LogWarning("`destinationIP` parameter cannot be changed in simulation runtime"); + } + + if (destinationPortOnAwake != destinationPort) + { + destinationPort = destinationPortOnAwake; + Debug.LogWarning("`destinationPort` parameter cannot be changed in simulation runtime"); + } + if (emitRawPackets != rglSubgraphUdpPublishing.IsActive(udpPublishingNodeId)) { 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(); } public void OnEnable() @@ -150,13 +192,16 @@ private void ValidateLidarModel() Debug.LogWarning($"Max range of lidar '{lidarSensor.name}' exceeds max range supported by Velodyne Legacy Packet Format (262.14m). Consider reducing its value to ensure proper work."); } - if (currentRGLLidarModel != UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value]) - { - currentRGLLidarModel = UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value]; - HandleUdpOptionsFlags(); - rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish( - udpPublishingNodeId, currentRGLLidarModel, currentRGLUdpOptions, sourceIP, destinationIP, destinationPort); - } + currentRGLLidarModel = UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value]; + UpdateRGLSubgraph(); + } + + private void UpdateRGLSubgraph() + { + HandleUdpOptionsFlags(); + rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish( + udpPublishingNodeId, currentRGLLidarModel, currentRGLUdpOptions, + sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake); } /// From a063a2e06f6a27b369ca336c63c06bf5f179d160 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Sun, 19 Nov 2023 17:18:13 +0100 Subject: [PATCH 2/3] Minor fix with warning for velodyne range exceeding --- Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs index 9cd037aac..6779616f1 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs @@ -186,8 +186,8 @@ private void ValidateLidarModel() return; } - // Currently, all of the supported models use Velodyne Legacy Packet Format - if (modelToValidate.maxRange > maxRangeForVelodyneLegacyPacketFormat) + // Currently, all of the supported Velodyne models use Legacy Packet Format + if (IsVelodyne((LidarModel)detectedUnityLidarModel) && modelToValidate.maxRange > maxRangeForVelodyneLegacyPacketFormat) { Debug.LogWarning($"Max range of lidar '{lidarSensor.name}' exceeds max range supported by Velodyne Legacy Packet Format (262.14m). Consider reducing its value to ensure proper work."); } @@ -256,5 +256,12 @@ private void HandleUdpOptionsFlags() currentRGLUdpOptions = (RGLUdpOptions)udpOptionsConstruction; } + + private bool IsVelodyne(LidarModel model) + { + return model == LidarModel.VelodyneVLP16 || + model == LidarModel.VelodyneVLP32C || + model == LidarModel.VelodyneVLS128; + } } } From 9cd6a41be5350bd5e6012f0560cb4fa2511fb638 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Sun, 19 Nov 2023 17:42:48 +0100 Subject: [PATCH 3/3] Fix startup --- Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs index 6779616f1..604ff7237 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs @@ -90,6 +90,11 @@ private void Start() // 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(); }