HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM
2026-03-02 07:50:30 +00:00
parent ff8a90cbaa
commit 06c2d01b4a
61 changed files with 2934 additions and 1012 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
examples/NavigationExample/TF3API.cs Normal file
View File

@@ -0,0 +1,133 @@
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;
}
}
}