Skip to content

Commit

Permalink
Add support for Hesai 40P and QT LiDAR packets
Browse files Browse the repository at this point in the history
  • Loading branch information
@msz-rai
msz-rai committed Oct 31, 2023
1 parent 3655578 commit bd8dceb
Show file tree
Hide file tree
Showing 5 changed files with 51 additions and 20 deletions.
39 changes: 31 additions & 8 deletions Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,11 @@ 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 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";
Expand All @@ -35,11 +37,13 @@ public class LidarUdpPublisher : MonoBehaviour

private static readonly float maxRangeForVelodyneLegacyPacketFormat = 262.14f;

private static readonly Dictionary<LidarModel, RGLVelodyneModel> UnityToRGLLidarModelsMapping = new Dictionary<LidarModel, RGLVelodyneModel>
private static readonly Dictionary<LidarModel, RGLLidarModel> UnityToRGLLidarModelsMapping = new Dictionary<LidarModel, RGLLidarModel>
{
{ 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
Expand All @@ -55,9 +59,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()
Expand Down Expand Up @@ -146,7 +151,9 @@ private void ValidateLidarModel()
if (currentRGLLidarModel != UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value])
{
currentRGLLidarModel = UnityToRGLLidarModelsMapping[detectedUnityLidarModel.Value];
rglSubgraphUdpPublishing.UpdateNodePointsUdpPublishVelodyne(udpPublishingNodeId, currentRGLLidarModel, sourceIP, destinationIP, destinationPort);
HandleHesaiUdpSequenceFlag();
rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish(
udpPublishingNodeId, currentRGLLidarModel, currentRGLUdpOptions, sourceIP, destinationIP, destinationPort);
}
}

Expand All @@ -171,5 +178,21 @@ private bool CheckAndDestroyComponentIfUnsupported()
}
return true;
}

private void HandleHesaiUdpSequenceFlag()
{
if (currentRGLLidarModel == RGLLidarModel.RGL_HESAI_PANDAR_40P || currentRGLLidarModel == RGLLidarModel.RGL_HESAI_PANDAR_QT64)
{
currentRGLUdpOptions = enableHesaiUdpSequence ? RGLUdpOptions.RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE
: RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
return;
}
if (enableHesaiUdpSequence)
{
enableHesaiUdpSequence = false;
currentRGLUdpOptions = RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS;
Debug.LogWarning($"{name}: enableHesaiUdpSequence option is not available for selected LiDAR model. Disabling...");
}
}
}
}
2 changes: 1 addition & 1 deletion Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
8 changes: 4 additions & 4 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")]
Expand Down Expand Up @@ -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)
Expand Down
10 changes: 9 additions & 1 deletion Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,11 +67,19 @@ public enum RGLAxis
RGL_AXIS_Z = 3,
};

public enum RGLVelodyneModel
public enum RGLLidarModel
{
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
{
RGL_UDP_NO_ADDITIONAL_OPTIONS = 0,
RGL_UDP_ENABLE_HESAI_UDP_SEQUENCE = 1,
};

public enum RGLQosPolicyReliability
Expand Down
12 changes: 6 additions & 6 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down

0 comments on commit bd8dceb

Please sign in to comment.