Skip to content

Commit

Permalink
Merge pull request #356 from tier4/feature/rgl-multi-return-udp
Browse files Browse the repository at this point in the history 
Support multi-return modes in UDP publishing
  • Loading branch information
@mackierx111
mackierx111 authored Oct 1, 2024
2 parents 26268a0 + e370b8b commit 17c5863
Show file tree
Hide file tree
Showing 4 changed files with 27 additions and 23 deletions.
27 changes: 15 additions & 12 deletions Assets/RGLUnityPlugin/Scripts/LidarUdpPublisher.cs
View file
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
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
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
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

0 comments on commit 17c5863

Please sign in to comment.