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

Jump to bottom

Support multi-return modes in UDP publishing #356

Open
wants to merge 2 commits into
base: feature/rgl-24q3
Choose a base branch
from
+27 −23
Open

Support multi-return modes in UDP publishing #356

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
06a71ee
Support multi-return modes in UDP publishing
msz-rai Sep 4, 2024
e370b8b
Add flag to ensure hesai pandar driver compatibility
msz-rai Sep 25, 2024
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
27 changes: 15 additions & 12 deletions Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,13 @@
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;
Expand All @@ -49,6 +46,9 @@ public class LidarUdpPublisher : MonoBehaviour
[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

[Tooltip("Hesai Pandar ROS2 driver has some differences from the LiDAR manuals. This flag applies these changes to raw packets.")]
public bool ensureCompatibilityWithHesaiPandarDriver = false;

private RGLNodeSequence rglSubgraphUdpPublishing;

private const string udpPublishingNodeId = "UDP_PUBLISHING";
Expand All @@ -74,11 +74,11 @@ public class LidarUdpPublisher : MonoBehaviour
private static readonly Dictionary<LidarModel, List<RGLReturnMode>> SupportedLidarsAndReturnModes = new Dictionary<LidarModel, List<RGLReturnMode>>
{
{ LidarModel.VelodyneVLP16, new List<RGLReturnMode>()
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnLastStrongest } },
{ LidarModel.VelodyneVLP32C, new List<RGLReturnMode>()
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnLastStrongest } },
{ LidarModel.VelodyneVLS128, new List<RGLReturnMode>()
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast } },
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnLastStrongest } },
{ LidarModel.HesaiPandar40P, new List<RGLReturnMode>()
{ RGLReturnMode.SingleReturnStrongest, RGLReturnMode.SingleReturnLast, RGLReturnMode.DualReturnLastStrongest } },
{ LidarModel.HesaiPandarQT, new List<RGLReturnMode>()
Expand Down Expand Up @@ -138,7 +138,7 @@ private void Awake()

// Node parameters will be updated when validating lidar model
rglSubgraphUdpPublishing = new RGLNodeSequence()
.AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLReturnMode.SingleReturnStrongest, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS,
.AddNodePointsUdpPublish(udpPublishingNodeId, RGLLidarModel.RGL_VELODYNE_VLP16, RGLUdpOptions.RGL_UDP_NO_ADDITIONAL_OPTIONS,
sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake);
}

Expand Down Expand Up @@ -258,17 +258,16 @@ private void HandleNewLidarModel()
}

// Check if supported return mode is selected
if (!SupportedLidarsAndReturnModes[currentLidarModel].Contains(returnMode))
if (!SupportedLidarsAndReturnModes[currentLidarModel].Contains(lidarSensor.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];
$"Please select one of: [{string.Join(", ", SupportedLidarsAndReturnModes[currentLidarModel])}]. Disabling component...");
OnDisable();
}

// Update RGL subgraph
rglSubgraphUdpPublishing.UpdateNodePointsUdpPublish(
udpPublishingNodeId, UnityToRGLLidarModelsMapping[currentLidarModel], returnMode, GetUdpOptions(currentLidarModel),
udpPublishingNodeId, UnityToRGLLidarModelsMapping[currentLidarModel], GetUdpOptions(currentLidarModel),
sourceIPOnAwake, destinationIPOnAwake, destinationPortOnAwake);
}

Expand All @@ -295,12 +294,16 @@ private RGLUdpOptions GetUdpOptions(LidarModel currentLidarModel)
Debug.LogWarning($"{name}: enableHesaiUpCloseBlockageDetection option is only available for Hesai QT128C2X LiDAR model. Disabling option...");
}

// Other LiDAR models are compatible with Hesai Pandar Driver by default
bool enableHesaiPandarDriverCompatibilityForQt = currentLidarModel == LidarModel.HesaiPandarQT && ensureCompatibilityWithHesaiPandarDriver;

// 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;
udpOptions += enableHesaiPandarDriverCompatibilityForQt ? (UInt32)RGLUdpOptions.RGL_UDP_FIT_QT64_TO_HESAI_PANDAR_DRIVER : 0;

// Check if high resolution mode is enabled (available only on Hesai Pandar128E4X)
if (currentLidarModel == LidarModel.HesaiPandar128E4X)
Expand Down
6 changes: 3 additions & 3 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeAPI.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ public static extern int rgl_node_publish_ros2_radarscan(ref IntPtr node, [Marsh

[DllImport("RobotecGPULidar")]
public static extern int rgl_node_points_udp_publish(
ref IntPtr node, RGLLidarModel lidar_model, RGLReturnMode return_mode, RGLUdpOptions udp_options, [MarshalAs(UnmanagedType.LPStr)] string device_ip,
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 @@ -550,9 +550,9 @@ public static void NodePublishRos2RadarScan(
CheckErr(rgl_node_publish_ros2_radarscan(ref node, topicName, frameId, qos_reliability, qos_durability, qos_history, qos_depth));
}

public static void NodePointsUdpPublish(ref IntPtr node, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, 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(ref node, lidarModel, returnMode, udpOptions, 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
9 changes: 5 additions & 4 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,10 +143,11 @@ public enum RGLRadarObjectClass : Int32

public enum RGLUdpOptions : UInt32
{
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,
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,
RGL_UDP_FIT_QT64_TO_HESAI_PANDAR_DRIVER = 1 << 3,
};

public enum RGLQosPolicyReliability
Expand Down
8 changes: 4 additions & 4 deletions Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNodeSequence.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -253,11 +253,11 @@ public RGLNodeSequence AddNodePublishRos2RadarScan(
return this;
}

public RGLNodeSequence AddNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, 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.NodePointsUdpPublish(ref handle.Node, lidarModel, returnMode, udpOptions, deviceIp, destIp, destPort);
RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort);
handle.Type = RGLNodeType.POINTS_UDP_PUBLISH;
handle.Identifier = identifier;
AddNode(handle);
Expand Down Expand Up @@ -407,10 +407,10 @@ public RGLNodeSequence UpdateNodePointsTemporalMerge(string identifier, RGLField
return this;
}

public RGLNodeSequence UpdateNodePointsUdpPublish(string identifier, RGLLidarModel lidarModel, RGLReturnMode returnMode, RGLUdpOptions udpOptions, 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);
RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, returnMode, udpOptions, deviceIp, destIp, destPort);
RGLNativeAPI.NodePointsUdpPublish(ref handle.Node, lidarModel, udpOptions, deviceIp, destIp, destPort);
return this;
}

Expand Down
Loading