diff --git a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs index 5d858f943..c84fbfa01 100644 --- a/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs +++ b/Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs @@ -24,9 +24,13 @@ public class LidarUdpPublisher : MonoBehaviour public string sourceIP = "0.0.0.0"; public string destinationIP = "255.255.255.255"; public int destinationPort = 2368; + 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 emitRawPackets = true; - private RGLVelodyneModel currentRGLLidarModel = RGLVelodyneModel.RGL_VELODYNE_VLP16; + 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 readonly string udpPublishingNodeId = "UDP_PUBLISHING"; @@ -35,11 +39,13 @@ public class LidarUdpPublisher : MonoBehaviour private static readonly float maxRangeForVelodyneLegacyPacketFormat = 262.14f; - private static readonly Dictionary UnityToRGLLidarModelsMapping = new Dictionary + private static readonly Dictionary UnityToRGLLidarModelsMapping = new Dictionary { - { LidarModel.VelodyneVLP16, RGLVelodyneModel.RGL_VELODYNE_VLP16 }, - { LidarModel.VelodyneVLP32C, RGLVelodyneModel.RGL_VELODYNE_VLP32C }, - { LidarModel.VelodyneVLS128, RGLVelodyneModel.RGL_VELODYNE_VLS128 } + { LidarModel.VelodyneVLP16, RGLLidarModel.RGL_VELODYNE_VLP16 }, + { 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 } }; // To be called when adding this component @@ -55,9 +61,10 @@ private void Awake() return; } - // Velodyne model will be updated when validating lidar model + // Node parameters will be updated when validating lidar model rglSubgraphUdpPublishing = new RGLNodeSequence() - .AddNodePointsUdpPublishVelodyne(udpPublishingNodeId, currentRGLLidarModel, sourceIP, destinationIP, destinationPort); + .AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS, + sourceIP, destinationIP, destinationPort); } private void Start() @@ -146,7 +153,9 @@ private void ValidateLidarModel() if (currentRGLLidarModel != UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value]) { currentRGLLidarModel = UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value]; - rglSubgraphUdpPublishing.UpdateNodePointsUdpPublishVelodyne(udpPublishingNodeId, currentRGLLidarModel, sourceIP, destinationIP, destinationPort); + HandleUdpOptionsFlags(); + rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish( + udpPublishingNodeId, currentRGLLidarModel, currentRGLUdpOptions, sourceIP, destinationIP, destinationPort); } } @@ -171,5 +180,36 @@ 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; + } } } diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs index 0ac542dfd..2a4f9ce3c 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs @@ -30,7 +30,7 @@ public enum RGLNodeType POINTS_DOWNSAMPLE, POINTS_TEMPORAL_MERGE, POINTS_ROS2_PUBLISH, - POINTS_UDP_PUBLISH_VELODYNE, + POINTS_UDP_PUBLISH, RAYTRACE, GAUSSIAN_NOISE_ANGULAR_RAY, GAUSSIAN_NOISE_ANGULAR_HITPOINT, diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs index 271162f8f..3e54c59b6 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs @@ -114,8 +114,8 @@ public static extern int rgl_node_points_ros2_publish_with_qos( RGLQosPolicyReliability qos_reliability, RGLQosPolicyDurability qos_durability, RGLQosPolicyHistory qos_history, int qos_depth); [DllImport("RobotecGPULidar")] - public static extern int rgl_node_points_udp_publish_velodyne( - ref IntPtr node, RGLVelodyneModel velodyne_model, [MarshalAs(UnmanagedType.LPStr)] string device_ip, + public static extern int rgl_node_points_udp_publish( + ref IntPtr node, RGLLidarModel lidar_model, RGLUdpOptions udp_options, [MarshalAs(UnmanagedType.LPStr)] string device_ip, [MarshalAs(UnmanagedType.LPStr)] string dest_ip, int dest_port); [DllImport("RobotecGPULidar")] @@ -444,9 +444,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 NodePointsUdpPublishVelodyne(ref IntPtr node, RGLVelodyneModel velodyneModel, string deviceIp, string destIp, int destPort) + public static void NodePointsUdpPublish(ref IntPtr node, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort) { - CheckErr(rgl_node_points_udp_publish_velodyne(ref node, velodyneModel, deviceIp, destIp, destPort)); + CheckErr(rgl_node_points_udp_publish(ref node, lidarModel, 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 e0596b422..b844944b1 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +using System; + namespace RGLUnityPlugin { - public enum RGLStatus + public enum RGLStatus : Int32 { SUCCESS = 0, INVALID_ARGUMENT, @@ -26,7 +28,7 @@ public enum RGLStatus INTERNAL_EXCEPTION = 500, }; - public enum RGLField + public enum RGLField : Int32 { UNKNOWN = -1, XYZ_F32 = 1, @@ -48,7 +50,7 @@ public enum RGLField DYNAMIC_FORMAT = 13842, } - public enum RGLLogLevel + public enum RGLLogLevel : Int32 { ALL = 0, TRACE = 0, @@ -60,18 +62,27 @@ public enum RGLLogLevel OFF = 6, }; - public enum RGLAxis + public enum RGLAxis : Int32 { RGL_AXIS_X = 1, RGL_AXIS_Y = 2, RGL_AXIS_Z = 3, }; - public enum RGLVelodyneModel + public enum RGLLidarModel : Int32 { RGL_VELODYNE_VLP16 = 1, RGL_VELODYNE_VLP32C = 2, RGL_VELODYNE_VLS128 = 3, + RGL_HESAI_PANDAR_40P = 4, + RGL_HESAI_PANDAR_QT64 = 5, + }; + + public enum RGLUdpOptions : UInt32 + { + RGL_UDP_NO_ADDITIONAL_OPTIONS = 0, + RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE = 1 << 0, + RGL_UDP_DUAL_RETURN = 1 << 1, }; public enum RGLQosPolicyReliability diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs index fdea09cfb..01548cf8e 100644 --- a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs +++ b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs @@ -226,12 +226,12 @@ public RGLNodeSequence AddNodePointsRos2Publish( return this; } - public RGLNodeSequence AddNodePointsUdpPublishVelodyne(string identifier, RGLVelodyneModel velodyneModel, string deviceIp, string destIp, int destPort) + public RGLNodeSequence AddNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort) { CheckNodeNotExist(identifier); RGLNodeHandle handle = new RGLNodeHandle(); - RGLNativeAPI.NodePointsUdpPublishVelodyne(ref handle.Node, velodyneModel, deviceIp, destIp, destPort); - handle.Type = RGLNodeType.POINTS_UDP_PUBLISH_VELODYNE; + RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort); + handle.Type = RGLNodeType.POINTS_UDP_PUBLISH; handle.Identifier = identifier; AddNode(handle); return this; @@ -341,10 +341,10 @@ public RGLNodeSequence UpdateNodePointsTemporalMerge(string identifier, RGLField return this; } - public RGLNodeSequence UpdateNodePointsUdpPublishVelodyne(string identifier, RGLVelodyneModel velodyneModel, string deviceIp, string destIp, int destPort) + public RGLNodeSequence UpdateNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLUdpOptions udpOptions, string deviceIp, string destIp, int destPort) { - RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_UDP_PUBLISH_VELODYNE); - RGLNativeAPI.NodePointsUdpPublishVelodyne(ref handle.Node, velodyneModel, deviceIp, destIp, destPort); + RGLNodeHandle handle = ValidateNode(identifier, RGLNodeType.POINTS_UDP_PUBLISH); + RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort); return this; }