Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for Hesai 40P and QT LiDAR packets #216

Merged
merged 3 commits into from
Nov 9, 2023
Merged

Add support for Hesai 40P and QT LiDAR packets #216

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bd8dceb
Add support for Hesai 40P and QT LiDAR packets
msz-rai Oct 31, 2023
1e5f9ff
Review changes
msz-rai Oct 31, 2023
ddb4bc1
Add dual return format handle
msz-rai Nov 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 48 additions & 8 deletions Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -35,11 +39,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 +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()
Expand Down Expand Up @@ -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);
}
}

Expand All @@ -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;
}
}
}
2 changes: 1 addition & 1 deletion Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLHelperTypes.cs
View file
Open in desktop
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
Open in desktop
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
21 changes: 16 additions & 5 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -26,7 +28,7 @@ public enum RGLStatus
INTERNAL_EXCEPTION = 500,
};

public enum RGLField
public enum RGLField : Int32
{
UNKNOWN = -1,
XYZ_F32 = 1,
Expand All @@ -48,7 +50,7 @@ public enum RGLField
DYNAMIC_FORMAT = 13842,
}

public enum RGLLogLevel
public enum RGLLogLevel : Int32
{
ALL = 0,
TRACE = 0,
Expand All @@ -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
Expand Down
12 changes: 6 additions & 6 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
View file
Open in desktop
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
Loading