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

14
srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/RobotNet10.RobotApp.SickScanXdApi.csproj Normal file
View File

@@ -0,0 +1,14 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net10.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="../../../Shared/RobotNet10.Shared/RobotNet10.Shared.csproj" />
</ItemGroup>
</Project>

155
srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiClient.cs Normal file
View File

@@ -0,0 +1,155 @@
using System.Text;
namespace RobotNet10.RobotApp.SickScanXdApi;
/// <summary>
/// High-level C# wrapper for sick_scan_xd API
/// </summary>
public class SickScanApiClient : IDisposable
{
private IntPtr _apiHandle;
private bool _disposed;
private readonly object _lock = new();
public bool IsInitialized => _apiHandle != IntPtr.Zero;
public SickScanApiClient()
{
_apiHandle = IntPtr.Zero;
}
/// <summary>
/// Initialize scanner with launchfile-style arguments
/// </summary>
public async Task<bool> InitializeAsync(string launchfileArgs, CancellationToken ct = default)
{
await Task.Yield(); // Make async
lock (_lock)
{
if (_apiHandle != IntPtr.Zero)
throw new InvalidOperationException("Already initialized");
// Create API handle
_apiHandle = SickScanApiNative.SickScanApiCreate(0, IntPtr.Zero);
if (_apiHandle == IntPtr.Zero)
return false;
// Split launch args by spaces and convert to argc/argv format
// Format: "scanner_type:=sick_tim_7xx hostname:=192.168.254.11 port:=2112 ..."
var argsList = new List<string>();
argsList.Add("sick_scan_xd_api"); // argv[0] = program name
argsList.AddRange(launchfileArgs.Split(' ', StringSplitOptions.RemoveEmptyEntries));
int argc = argsList.Count;
IntPtr[] argvPtrs = new IntPtr[argc];
IntPtr argv = IntPtr.Zero;
try
{
// Allocate argv array
argv = System.Runtime.InteropServices.Marshal.AllocHGlobal(argc * IntPtr.Size);
// Marshal each string argument
for (int i = 0; i < argc; i++)
{
argvPtrs[i] = System.Runtime.InteropServices.Marshal.StringToHGlobalAnsi(argsList[i]);
}
// Copy pointers to argv
System.Runtime.InteropServices.Marshal.Copy(argvPtrs, 0, argv, argc);
// Initialize with CLI args
var result = SickScanApiNative.SickScanApiInitByCli(_apiHandle, argc, argv);
if (result != SickScanApiNative.SICK_SCAN_API_SUCCESS)
{
SickScanApiNative.SickScanApiRelease(_apiHandle);
_apiHandle = IntPtr.Zero;
return false;
}
return true;
}
finally
{
// Free all allocated memory
for (int i = 0; i < argc && i < argvPtrs.Length; i++)
{
if (argvPtrs[i] != IntPtr.Zero)
System.Runtime.InteropServices.Marshal.FreeHGlobal(argvPtrs[i]);
}
if (argv != IntPtr.Zero)
System.Runtime.InteropServices.Marshal.FreeHGlobal(argv);
}
}
}
/// <summary>
/// Poll for next polar point cloud (blocking with timeout)
/// </summary>
public SickScanApiNative.SickScanPointCloudMsg? PollPolarPointCloud(double timeoutSec = 1.0)
{
if (_apiHandle == IntPtr.Zero)
throw new InvalidOperationException("Not initialized");
var msg = new SickScanApiNative.SickScanPointCloudMsg
{
header = new SickScanApiNative.SickScanHeader
{
frame_id = new byte[256]
},
fields = new SickScanApiNative.SickScanPointFieldArray(),
data = new SickScanApiNative.SickScanUint8Array(),
topic = new byte[256]
};
var result = SickScanApiNative.SickScanApiWaitNextPolarPointCloudMsg(
_apiHandle, ref msg, timeoutSec);
if (result == SickScanApiNative.SICK_SCAN_API_SUCCESS)
return msg;
if (result == SickScanApiNative.SICK_SCAN_API_TIMEOUT)
return null;
throw new Exception($"Failed to poll point cloud: error code {result}");
}
/// <summary>
/// Free point cloud message memory
/// </summary>
public void FreePointCloudMsg(ref SickScanApiNative.SickScanPointCloudMsg msg)
{
if (_apiHandle == IntPtr.Zero)
return;
SickScanApiNative.SickScanApiFreePointCloudMsg(_apiHandle, ref msg);
}
/// <summary>
/// Close scanner connection
/// </summary>
public void Close()
{
lock (_lock)
{
if (_apiHandle == IntPtr.Zero)
return;
SickScanApiNative.SickScanApiClose(_apiHandle);
SickScanApiNative.SickScanApiRelease(_apiHandle);
_apiHandle = IntPtr.Zero;
}
}
public void Dispose()
{
if (_disposed)
return;
Close();
_disposed = true;
GC.SuppressFinalize(this);
}
}

124
srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiNative.cs Normal file
View File

@@ -0,0 +1,124 @@
using System.Runtime.InteropServices;
namespace RobotNet10.RobotApp.SickScanXdApi;
/// <summary>
/// P/Invoke declarations for sick_scan_xd C API
/// </summary>
public static class SickScanApiNative
{
private const string LibraryName = "sick_scan_xd_shared_lib"; // libsick_scan_xd_shared_lib.so
// Structs matching C API (exact field names with snake_case)
[StructLayout(LayoutKind.Sequential)]
public struct SickScanHeader
{
public uint seq;
public uint timestamp_sec;
public uint timestamp_nsec;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
public byte[] frame_id;
}
[StructLayout(LayoutKind.Sequential)]
public struct SickScanUint8Array
{
public ulong capacity;
public ulong size;
public IntPtr buffer; // uint8_t*
}
[StructLayout(LayoutKind.Sequential)]
public struct SickScanPointFieldMsg
{
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
public byte[] name;
public uint offset;
public byte datatype;
public uint count;
}
[StructLayout(LayoutKind.Sequential)]
public struct SickScanPointFieldArray
{
public ulong capacity;
public ulong size;
public IntPtr buffer; // SickScanPointFieldMsg*
}
[StructLayout(LayoutKind.Sequential)]
public struct SickScanPointCloudMsg
{
public SickScanHeader header;
public uint height;
public uint width;
public SickScanPointFieldArray fields;
public byte is_bigendian;
public uint point_step;
public uint row_step;
public SickScanUint8Array data;
public byte is_dense;
public int num_echos;
public int segment_idx;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
public byte[] topic;
}
// API Functions
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr SickScanApiCreate(int argc, IntPtr argv);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiRelease(IntPtr apiHandle);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiInitByCli(
IntPtr apiHandle,
int argc,
IntPtr argv);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiInitByLaunchfile(
IntPtr apiHandle,
IntPtr launchfileArgs);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiClose(IntPtr apiHandle);
// Callback registration
public delegate void PointCloudMsgCallback(IntPtr apiHandle, ref SickScanPointCloudMsg msg);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiRegisterPolarPointCloudMsg(
IntPtr apiHandle,
PointCloudMsgCallback callback);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiDeregisterPolarPointCloudMsg(
IntPtr apiHandle,
PointCloudMsgCallback callback);
// Polling function
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiWaitNextPolarPointCloudMsg(
IntPtr apiHandle,
ref SickScanPointCloudMsg msg,
double timeoutSec);
[DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
public static extern int SickScanApiFreePointCloudMsg(
IntPtr apiHandle,
ref SickScanPointCloudMsg msg);
// Error codes
public const int SICK_SCAN_API_SUCCESS = 0;
public const int SICK_SCAN_API_ERROR = 1;
public const int SICK_SCAN_API_NOT_LOADED = 2;
public const int SICK_SCAN_API_NOT_INITIALIZED = 3;
public const int SICK_SCAN_API_NOT_IMPLEMENTED = 4;
public const int SICK_SCAN_API_TIMEOUT = 5;
}