139 lines
4.0 KiB
C#
139 lines
4.0 KiB
C#
using System.Runtime.InteropServices;
|
|
|
|
namespace RobotNet10.RobotApp.TF3;
|
|
|
|
/// <summary>
|
|
/// C# P/Invoke wrapper for TF3 (Transform Framework) C API
|
|
/// Provides coordinate transform management for robotics applications
|
|
/// </summary>
|
|
|
|
/// <summary>
|
|
/// Opaque handle to TF3 BufferCore (TF3_BufferCore in C API)
|
|
/// </summary>
|
|
public struct TF3_BufferCore
|
|
{
|
|
public IntPtr Handle;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform structure for TF3 (matches TF3_Transform in C API)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct TF3_Transform
|
|
{
|
|
// Header
|
|
public long timestamp_sec;
|
|
public long timestamp_nsec;
|
|
|
|
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
|
|
public string frame_id;
|
|
|
|
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
|
|
public string child_frame_id;
|
|
|
|
// Translation
|
|
public double translation_x;
|
|
public double translation_y;
|
|
public double translation_z;
|
|
|
|
// Rotation (quaternion)
|
|
public double rotation_x;
|
|
public double rotation_y;
|
|
public double rotation_z;
|
|
public double rotation_w;
|
|
}
|
|
|
|
/// <summary>
|
|
/// TF3 error codes (matches TF3_ErrorCode in C API)
|
|
/// </summary>
|
|
public enum TF3_ErrorCode
|
|
{
|
|
TF3_OK = 0,
|
|
TF3_ERROR_LOOKUP = 1,
|
|
TF3_ERROR_CONNECTIVITY = 2,
|
|
TF3_ERROR_EXTRAPOLATION = 3,
|
|
TF3_ERROR_INVALID_ARGUMENT = 4,
|
|
TF3_ERROR_TIMEOUT = 5,
|
|
TF3_ERROR_UNKNOWN = 99
|
|
}
|
|
|
|
/// <summary>
|
|
/// P/Invoke declarations for libtf3.so
|
|
/// </summary>
|
|
public static class TF3NativeInterface
|
|
{
|
|
private const string LibraryPath = "/usr/local/lib/libtf3.so";
|
|
|
|
/// <summary>
|
|
/// Create TF3 buffer with cache time
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr tf3_buffer_create(int cacheTimeSec);
|
|
|
|
/// <summary>
|
|
/// Destroy TF3 buffer
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_buffer_destroy(IntPtr buffer);
|
|
|
|
/// <summary>
|
|
/// Set a transform in the buffer
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern bool tf3_set_transform(
|
|
IntPtr buffer,
|
|
ref TF3_Transform transform,
|
|
string authority,
|
|
bool isStatic);
|
|
|
|
/// <summary>
|
|
/// Lookup transform between two frames
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern bool tf3_lookup_transform(
|
|
IntPtr buffer,
|
|
string targetFrame,
|
|
string sourceFrame,
|
|
long timeSec,
|
|
long timeNsec,
|
|
ref TF3_Transform transform,
|
|
ref TF3_ErrorCode errorCode);
|
|
|
|
/// <summary>
|
|
/// Check if transform can be computed
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern bool tf3_can_transform(
|
|
IntPtr buffer,
|
|
string targetFrame,
|
|
string sourceFrame,
|
|
long timeSec,
|
|
long timeNsec,
|
|
IntPtr errorMsg,
|
|
int errorMsgLen);
|
|
|
|
/// <summary>
|
|
/// Clear all transforms
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_clear(IntPtr buffer);
|
|
|
|
/// <summary>
|
|
/// Get current time
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_get_current_time(ref long sec, ref long nsec);
|
|
|
|
/// <summary>
|
|
/// Get version string
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr tf3_get_version();
|
|
|
|
// [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
// public static extern int tf3_get_all_frame_names(
|
|
// TF3_BufferCore buffer,
|
|
// IntPtr frames,
|
|
// int frames_len);
|
|
}
|