QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

416
srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Parsers/LmdScandataParser.cs Normal file
View File

@@ -0,0 +1,416 @@
using System.Text;
namespace Sick.ColaB.Parsers;
/// <summary>
/// Parser for SICK scanner LMDscandata telegram
/// Supports both binary (ColaB) and ASCII (ColaA) formats
/// </summary>
public static class LmdScandataParser
{
// Constants for validation
private const int MinReasonableDistanceCount = 50;
private const int MaxReasonableDistanceCount = 5000;
/// <summary>
/// Parse LMDscandata from binary ColaB format
/// This is the primary parser for binary protocol data
/// </summary>
public static ScanDataResult? ParseBinary(ReadOnlySpan<byte> commandData)
{
try
{
// Binary LMDscandata format:
// Starts with ASCII header: "sRA LMDscandata " or "sSN LMDscandata "
// Then binary data follows:
// - Various fields (version, device number, etc.)
// - "DIST1" (5 bytes ASCII) followed by:
// - Scale factor (4 bytes double)
// - Scale offset (4 bytes double)
// - Starting angle (4 bytes signed int, units: 1/10000 degree)
// - Angular step (2 bytes unsigned short, units: 1/10000 degree)
// - Number of items (2 bytes, big endian uint16)
// - Distance values (each 2 bytes, big endian uint16, units: mm)
// - "RSSI1" (5 bytes ASCII) followed by:
// - Number of items (2 bytes, big endian uint16)
// - RSSI values (each 1 or 2 bytes depending on resolution)
if (commandData.Length < 50)
{
return null;
}
// Check if it contains "LMDscandata"
var commandStart = Encoding.ASCII.GetString(commandData[..Math.Min(50, commandData.Length)]);
if (!commandStart.Contains("LMDscandata", StringComparison.Ordinal))
{
return null;
}
double startAngleDeg = 0.0;
double angularStepDeg = 0.0;
double scaleFactor = 1.0;
double scaleOffset = 0.0;
// Find "DIST1" in the data
var dist1Pattern = Encoding.ASCII.GetBytes("DIST1");
var dist1Index = FindPattern(commandData, dist1Pattern);
if (dist1Index == -1)
{
return null;
}
// Parse header fields at fixed offsets after DIST1
int headerStart = dist1Index + 5; // Right after "DIST1"
// Scale factor (4 bytes double, big endian) at offset headerStart
if (headerStart + 4 <= commandData.Length)
{
uint scaleFactorInt = ReadUInt32BigEndian(commandData, headerStart);
scaleFactor = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
}
// Scale offset (4 bytes double, big endian) at offset headerStart + 4
if (headerStart + 8 <= commandData.Length)
{
uint scaleOffsetInt = ReadUInt32BigEndian(commandData, headerStart + 4);
scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
}
// Starting angle (4 bytes signed int, big endian) at offset headerStart + 8
if (headerStart + 12 <= commandData.Length)
{
int startAngleInt = (int)ReadUInt32BigEndian(commandData, headerStart + 8);
startAngleDeg = startAngleInt / 10000.0;
}
// Angular step width (2 bytes unsigned short, big endian) at offset headerStart + 12
if (headerStart + 14 <= commandData.Length)
{
ushort angularStepInt = ReadUInt16BigEndian(commandData, headerStart + 12);
angularStepDeg = angularStepInt / 10000.0;
}
// Find distance count - try multiple offsets
if (!TryFindDistanceCount(commandData, headerStart, out ushort distCount, out int distCountOffset))
{
return null;
}
// Read distance values (each 2 bytes, big endian, units: mm)
var distValuesOffset = distCountOffset + 2;
if (distValuesOffset + (distCount * 2) > commandData.Length)
{
return null;
}
var ranges = new List<double>();
for (int i = 0; i < distCount; i++)
{
var offset = distValuesOffset + (i * 2);
if (offset + 2 > commandData.Length)
{
break;
}
// Read uint16 big endian
ushort distValue = ReadUInt16BigEndian(commandData, offset);
// Apply scale factor and convert from mm to meters
double distanceM = (distValue * scaleFactor + scaleOffset) / 1000.0;
ranges.Add(distanceM);
}
// Find "RSSI1" for intensities (optional)
var rssi1Pattern = Encoding.ASCII.GetBytes("RSSI1");
var rssi1Index = FindPattern(commandData, rssi1Pattern, rssiSearchStart: distValuesOffset + (distCount * 2));
var intensities = new List<double>();
if (rssi1Index != -1)
{
// Read RSSI count
var rssiCountOffset = rssi1Index + 5;
if (rssiCountOffset + 2 <= commandData.Length)
{
ushort rssiCount = ReadUInt16BigEndian(commandData, rssiCountOffset);
// Read RSSI values (usually 1 byte each, but can be 2 bytes for 16-bit resolution)
var rssiValuesOffset = rssiCountOffset + 2;
// Try to determine if RSSI is 8-bit or 16-bit
bool use16BitRssi = (rssiValuesOffset + (rssiCount * 2) <= commandData.Length) &&
(rssiCount == ranges.Count);
if (use16BitRssi)
{
// 16-bit RSSI values (big endian)
for (int i = 0; i < rssiCount && i < ranges.Count; i++)
{
ushort rssiValue = ReadUInt16BigEndian(commandData, rssiValuesOffset + (i * 2));
// Normalize to 0-1 range (assuming max value is 65535)
intensities.Add(rssiValue);
}
}
else if (rssiValuesOffset + rssiCount <= commandData.Length)
{
// 8-bit RSSI values
for (int i = 0; i < rssiCount && i < ranges.Count; i++)
{
// Normalize to 0-1 range (assuming max value is 255)
intensities.Add(commandData[rssiValuesOffset + i]);
}
}
else
{
for (int i = 0; i < ranges.Count; i++)
{
intensities.Add(1.0);
}
}
}
}
if (ranges.Count == 0)
{
return null;
}
return new ScanDataResult
{
Ranges = [.. ranges],
Intensities = intensities.Count > 0 ? [.. intensities] : null,
StartAngleDeg = startAngleDeg,
AngularStepDeg = angularStepDeg,
ScaleFactor = scaleFactor,
ScaleOffset = scaleOffset,
Timestamp = DateTime.UtcNow
};
}
catch
{
return null;
}
}
/// <summary>
/// Parse LMDscandata from ASCII format (via ColaB decoding)
/// This is a fallback parser when binary parsing fails
/// </summary>
public static ScanDataResult? ParseAscii(ReadOnlySpan<byte> commandData)
{
try
{
// Decode command string to check if it's scan data
var commandString = ColaBHelper.DecodeCommand(commandData);
// Check if this is scan data telegram
if (!commandString.Contains("LMDscandata", StringComparison.Ordinal))
{
return null;
}
return ParseAsciiString(commandString);
}
catch
{
return null;
}
}
/// <summary>
/// Parse LMDscandata from ASCII string format
/// Format: "sSN LMDscandata <version> <device> ... DIST1 <count> <distances> ... RSSI1 <count> <intensities> ..."
/// </summary>
public static ScanDataResult? ParseAsciiString(string telegram)
{
try
{
// Split telegram into fields (space-separated)
var fields = telegram.Split([' '], StringSplitOptions.RemoveEmptyEntries);
if (fields.Length < 20)
{
return null;
}
var ranges = new List<double>();
var intensities = new List<double>();
int distIndex = -1;
int rssiIndex = -1;
// Find DIST1 field
for (int i = 0; i < fields.Length; i++)
{
if (fields[i].StartsWith("DIST", StringComparison.Ordinal))
{
distIndex = i;
break;
}
}
// Find RSSI1 field
for (int i = 0; i < fields.Length; i++)
{
if (fields[i].StartsWith("RSSI", StringComparison.Ordinal))
{
rssiIndex = i;
break;
}
}
// Parse distances
if (distIndex >= 0 && distIndex + 1 < fields.Length)
{
// Next field after DIST1 is the count (in hex)
if (int.TryParse(fields[distIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int distCount))
{
// Parse distance values (in hex, units are mm, convert to meters)
for (int i = 0; i < distCount && distIndex + 2 + i < fields.Length; i++)
{
if (int.TryParse(fields[distIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int distValue))
{
// Convert from mm to meters
ranges.Add(distValue / 1000.0);
}
}
}
}
// Parse intensities (RSSI)
if (rssiIndex >= 0 && rssiIndex + 1 < fields.Length)
{
// Next field after RSSI1 is the count (in hex)
if (int.TryParse(fields[rssiIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int rssiCount))
{
// Parse RSSI values (in hex)
for (int i = 0; i < rssiCount && rssiIndex + 2 + i < fields.Length; i++)
{
if (int.TryParse(fields[rssiIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int rssiValue))
{
intensities.Add(rssiValue);
}
}
}
}
if (ranges.Count == 0)
{
return null;
}
return new ScanDataResult
{
Ranges = [.. ranges],
Intensities = intensities.Count > 0 ? [.. intensities] : null,
StartAngleDeg = 0.0, // ASCII format doesn't include angle info
AngularStepDeg = 0.0,
ScaleFactor = 1.0,
ScaleOffset = 0.0,
Timestamp = DateTime.UtcNow
};
}
catch
{
return null;
}
}
#region Helper Methods
private static int FindPattern(ReadOnlySpan<byte> data, byte[] pattern, int rssiSearchStart = 0)
{
int searchStart = rssiSearchStart;
for (int i = searchStart; i <= data.Length - pattern.Length; i++)
{
bool found = true;
for (int j = 0; j < pattern.Length; j++)
{
if (data[i + j] != pattern[j])
{
found = false;
break;
}
}
if (found)
{
return i;
}
}
return -1;
}
private static uint ReadUInt32BigEndian(ReadOnlySpan<byte> data, int offset)
{
if (offset + 4 > data.Length)
return 0;
return (uint)((data[offset] << 24) | (data[offset + 1] << 16) |
(data[offset + 2] << 8) | data[offset + 3]);
}
private static ushort ReadUInt16BigEndian(ReadOnlySpan<byte> data, int offset)
{
if (offset + 2 > data.Length)
return 0;
return (ushort)((data[offset] << 8) | data[offset + 1]);
}
private static bool TryFindDistanceCount(ReadOnlySpan<byte> commandData, int headerStart,
out ushort distCount, out int distCountOffset)
{
distCount = 0;
distCountOffset = -1;
// Try offset +14 first (matches actual data)
int tryOffset1 = headerStart + 14;
if (TryReadDistanceCount(commandData, tryOffset1, out distCount, out distCountOffset))
return true;
// Try offset +19 (per C++ reference code)
int tryOffset2 = headerStart + 19;
if (TryReadDistanceCount(commandData, tryOffset2, out distCount, out distCountOffset))
return true;
// Search for valid count
int searchStart = headerStart;
int searchEnd = Math.Min(headerStart + 30, commandData.Length - 2);
for (int offset = searchStart; offset <= searchEnd; offset++)
{
if (TryReadDistanceCount(commandData, offset, out distCount, out distCountOffset))
return true;
}
return false;
}
private static bool TryReadDistanceCount(ReadOnlySpan<byte> commandData, int offset,
out ushort count, out int countOffset)
{
count = 0;
countOffset = -1;
if (offset + 2 > commandData.Length)
return false;
ushort testCount = ReadUInt16BigEndian(commandData, offset);
if (testCount >= MinReasonableDistanceCount && testCount <= MaxReasonableDistanceCount)
{
int requiredBytes = offset + 2 + (testCount * 2);
if (requiredBytes <= commandData.Length)
{
count = testCount;
countOffset = offset;
return true;
}
}
return false;
}
#endregion
}