133 lines
5.4 KiB
C#
133 lines
5.4 KiB
C#
using System;
|
|
using System.Runtime.InteropServices;
|
|
|
|
namespace NavigationExample
|
|
{
|
|
|
|
// ============================================================================
|
|
// TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
|
|
// ============================================================================
|
|
public static class TF3API
|
|
{
|
|
private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib
|
|
|
|
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
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
|
|
public struct TF3_Transform
|
|
{
|
|
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;
|
|
public double translation_x;
|
|
public double translation_y;
|
|
public double translation_z;
|
|
public double rotation_x;
|
|
public double rotation_y;
|
|
public double rotation_z;
|
|
public double rotation_w;
|
|
}
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr tf3_buffer_create(int cache_time_sec);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_buffer_destroy(IntPtr buffer);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_set_transform(
|
|
IntPtr buffer,
|
|
ref TF3_Transform transform,
|
|
string authority,
|
|
[MarshalAs(UnmanagedType.I1)] bool is_static);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_lookup_transform(
|
|
IntPtr buffer,
|
|
string target_frame,
|
|
string source_frame,
|
|
long time_sec,
|
|
long time_nsec,
|
|
out TF3_Transform transform,
|
|
out TF3_ErrorCode error_code);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_lookup_transform_full(
|
|
IntPtr buffer,
|
|
string target_frame,
|
|
long target_time_sec,
|
|
long target_time_nsec,
|
|
string source_frame,
|
|
long source_time_sec,
|
|
long source_time_nsec,
|
|
string fixed_frame,
|
|
out TF3_Transform transform,
|
|
out TF3_ErrorCode error_code);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_can_transform(
|
|
IntPtr buffer,
|
|
string target_frame,
|
|
string source_frame,
|
|
long time_sec,
|
|
long time_nsec,
|
|
System.Text.StringBuilder error_msg,
|
|
int error_msg_len);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_clear(IntPtr buffer);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf3_get_current_time(out long sec, out long nsec);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);
|
|
|
|
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr tf3_get_version();
|
|
|
|
/// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
|
|
public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
|
|
double tx = 0, double ty = 0, double tz = 0,
|
|
double qx = 0, double qy = 0, double qz = 0, double qw = 1)
|
|
{
|
|
var t = new TF3_Transform();
|
|
t.timestamp_sec = 0;
|
|
t.timestamp_nsec = 0;
|
|
t.frame_id = parentFrame ?? "";
|
|
t.child_frame_id = childFrame ?? "";
|
|
t.translation_x = tx;
|
|
t.translation_y = ty;
|
|
t.translation_z = tz;
|
|
t.rotation_x = qx;
|
|
t.rotation_y = qy;
|
|
t.rotation_z = qz;
|
|
t.rotation_w = qw;
|
|
return t;
|
|
}
|
|
}
|
|
} |