QUYVN/I150
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-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

398
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/TF3/TF3BufferManager.cs Normal file
View File

@@ -0,0 +1,398 @@
using RobotNet10.RobotApp.TF3;
using Microsoft.Extensions.Hosting;
namespace RobotNet10.RobotApp.TF3;
/// <summary>
/// Manages TF3 buffer initialization and static transform publishing.
/// This service MUST be initialized first before other components that depend on TF3.
/// Implements IHostedService to ensure it initializes when the application starts.
/// The TF3 buffer can then be passed to XlocClient, navigation, and other components.
/// </summary>
public class TF3BufferManager : IHostedService, IDisposable
{
private IntPtr _tfBuffer = IntPtr.Zero;
private readonly object _lock = new object();
private bool _disposed = false;
private readonly ILogger? _logger;
private bool _initialized = false;
/// <summary>
/// Get the TF3 buffer pointer for use in other components
/// </summary>
public IntPtr TfBuffer
{
get
{
lock (_lock)
{
if (_tfBuffer == IntPtr.Zero)
{
throw new InvalidOperationException("TF3 buffer not initialized. Call Initialize() first.");
}
return _tfBuffer;
}
}
}
/// <summary>
/// Check if TF3 buffer is initialized
/// </summary>
public bool IsInitialized
{
get
{
lock (_lock)
{
return _initialized;
}
}
}
/// <summary>
/// Initialize TF3BufferManager with optional logger
/// </summary>
public TF3BufferManager(ILogger? logger = null)
{
_logger = logger;
}
/// <summary>
/// Initialize TF3 buffer and publish static transforms
/// Should be called once at startup, before initializing XlocClient or navigation components
/// </summary>
public void Initialize()
{
lock (_lock)
{
if (_initialized)
{
_logger?.LogWarning("TF3BufferManager already initialized");
return;
}
try
{
_logger?.LogInformation("[TF3] Initializing TF3 buffer manager...");
// Create TF3 buffer (10 second cache)
_tfBuffer = TF3NativeInterface.tf3_buffer_create(10);
if (_tfBuffer == IntPtr.Zero)
{
throw new InvalidOperationException("Failed to create TF3 buffer");
}
_logger?.LogInformation("[TF3] TF3 buffer created successfully: {Buffer}", _tfBuffer);
// Publish static transforms
PublishStaticTransforms();
_initialized = true;
_logger?.LogInformation("[TF3] TF3BufferManager initialized successfully");
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] Exception during initialization: {Message}", ex.Message);
// Clean up on error
if (_tfBuffer != IntPtr.Zero)
{
try
{
TF3NativeInterface.tf3_buffer_destroy(_tfBuffer);
}
catch { }
_tfBuffer = IntPtr.Zero;
}
throw;
}
}
}
/// <summary>
/// Publish static transforms to TF3 buffer.
/// Sets up the robot's coordinate frame tree.
/// Must be called after buffer initialization.
/// </summary>
private void PublishStaticTransforms()
{
try
{
// _logger?.LogInformation("[TF3] ═══════════════════════════════════════════");
// _logger?.LogInformation("[TF3] Publishing static transforms to TF3 buffer...");
// _logger?.LogInformation("[TF3] Buffer handle: {Buffer}", _tfBuffer);
var now = DateTime.UtcNow;
long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
// base_link → imu_link (IMU at robot center, colocated with base_link)
var baseToImu = new TF3_Transform
{
timestamp_sec = sec,
timestamp_nsec = nsec,
frame_id = "base_link",
child_frame_id = "imu",
translation_x = 0.0,
translation_y = 0.0,
translation_z = 0.0,
rotation_x = 1.0,
rotation_y = 0.0,
rotation_z = 0.0,
rotation_w = 0.0
};
_logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
baseToImu.frame_id, baseToImu.child_frame_id);
bool result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToImu, "tf3_buffer_manager", true);
// base_link → scan_1 (LIDAR at front, 15cm backward from base_link)
var baseToLidar1 = new TF3_Transform
{
timestamp_sec = sec,
timestamp_nsec = nsec,
frame_id = "base_link",
child_frame_id = "scan_1",
translation_x = 0.2985,
translation_y = 0.0,
translation_z = 0.0,
rotation_x = 0.0,
rotation_y = 0.0,
rotation_z = 0.0,
rotation_w = 1.0
};
_logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
baseToLidar1.frame_id, baseToLidar1.child_frame_id);
result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar1, "tf3_buffer_manager", true);
// base_link → scan_2 (LIDAR 2 at front-right corner, -45° rotation)
var baseToLidar2 = new TF3_Transform
{
timestamp_sec = sec,
timestamp_nsec = nsec,
frame_id = "base_link",
child_frame_id = "scan_2",
translation_x = -0.2985,
translation_y = 0.0,
translation_z = 0.0,
rotation_x = 0.0,
rotation_y = 0.0,
rotation_z = 1.0,
rotation_w = 0.0
};
_logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
baseToLidar2.frame_id, baseToLidar2.child_frame_id);
result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar2, "tf3_buffer_manager", true);
// base_link → scan_3 (LIDAR 3 at front-left corner, +45° rotation)
var baseToLidar3 = new TF3_Transform
{
timestamp_sec = sec,
timestamp_nsec = nsec,
frame_id = "base_link",
child_frame_id = "scan_3",
translation_x = 0.325,
translation_y = -0.325,
translation_z = 0.0,
rotation_x = 0.0,
rotation_y = 0.0,
rotation_z = 0.0,
rotation_w = 1.0
};
_logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
baseToLidar3.frame_id, baseToLidar3.child_frame_id);
result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar3, "tf3_buffer_manager", true);
// Publish initial map → odom transform (DYNAMIC - not static!)
// This is required for costmap to initialize and detect the 'map' frame.
// Navigation will NOT hang waiting for this transform to exist.
// XlocClient will UPDATE this transform when localization starts with actual pose data.
// We publish as isStatic=false so it can be dynamically updated by XlocClient.
// var mapToOdom = new TF3_Transform
// {
// timestamp_sec = sec,
// timestamp_nsec = nsec,
// frame_id = "map",
// child_frame_id = "odom",
// translation_x = 0.0,
// translation_y = 0.0,
// translation_z = 0.0,
// rotation_x = 0.0,
// rotation_y = 0.0,
// rotation_z = 0.0,
// rotation_w = 1.0
// };
// _logger?.LogInformation("[TF3] Publishing INITIAL transform (dummy): {ParentFrame} → {ChildFrame} [Will be updated by XLOC]",
// mapToOdom.frame_id, mapToOdom.child_frame_id);
// result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref mapToOdom, "tf3_buffer_manager", false);
// Publish odom → base_footprint frame
// var odomToFootprint = new TF3_Transform
// {
// timestamp_sec = sec,
// timestamp_nsec = nsec,
// frame_id = "odom",
// child_frame_id = "base_footprint",
// translation_x = 0.0,
// translation_y = 0.0,
// translation_z = 0.0,
// rotation_x = 0.0,
// rotation_y = 0.0,
// rotation_z = 0.0,
// rotation_w = 1.0
// };
// _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
// odomToFootprint.frame_id, odomToFootprint.child_frame_id);
// result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref odomToFootprint, "tf3_buffer_manager", false);
// var footPrintTobaseLink = new TF3_Transform
// {
// timestamp_sec = sec,
// timestamp_nsec = nsec,
// frame_id = "base_footprint",
// child_frame_id = "base_link",
// translation_x = 0.0,
// translation_y = 0.0,
// translation_z = 0.0,
// rotation_x = 0.0,
// rotation_y = 0.0,
// rotation_z = 0.0,
// rotation_w = 1.0
// };
// _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
// footPrintTobaseLink.frame_id, footPrintTobaseLink.child_frame_id);
// result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref footPrintTobaseLink, "tf3_buffer_manager", true);
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] ❌ Error publishing static transforms: {Message}", ex.Message);
throw;
}
}
/// <summary>
/// Publish a custom transform to the TF3 buffer
/// </summary>
public void PublishTransform(TF3_Transform transform, bool isStatic = false)
{
lock (_lock)
{
if (!_initialized)
{
throw new InvalidOperationException("TF3BufferManager not initialized. Call Initialize() first.");
}
try
{
TF3NativeInterface.tf3_set_transform(_tfBuffer, ref transform, "tf3_buffer_manager", isStatic);
_logger?.LogDebug("[TF3] Published transform: {FrameId} → {ChildFrameId}",
transform.frame_id, transform.child_frame_id);
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] Error publishing transform: {Message}", ex.Message);
throw;
}
}
}
/// <summary>
/// Get a transform from the TF3 buffer
/// </summary>
public TF3_Transform? GetTransform(string sourceFrame, string targetFrame)
{
lock (_lock)
{
if (!_initialized)
{
throw new InvalidOperationException("TF3BufferManager not initialized. Call Initialize() first.");
}
try
{
// Call the TF3 native interface to get the transform
// This is a simplified version - you may need to add the actual native method
_logger?.LogDebug("[TF3] Querying transform: {SourceFrame} → {TargetFrame}", sourceFrame, targetFrame);
return null; // Return null for now - implement based on actual TF3 API
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] Error getting transform: {Message}", ex.Message);
return null;
}
}
}
/// <summary>
/// IHostedService implementation - initializes TF3 buffer when application starts
/// This ensures TF3 is ready before XlocIntegrationService and NavigationIntegrationService
/// </summary>
public Task StartAsync(CancellationToken cancellationToken)
{
try
{
Initialize();
return Task.CompletedTask;
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] Failed to start TF3BufferManager: {Message}", ex.Message);
throw;
}
}
/// <summary>
/// IHostedService implementation - stops TF3 buffer when application shuts down
/// </summary>
public Task StopAsync(CancellationToken cancellationToken)
{
Dispose();
return Task.CompletedTask;
}
public void Dispose()
{
Dispose(true);
GC.SuppressFinalize(this);
}
protected virtual void Dispose(bool disposing)
{
if (_disposed)
return;
lock (_lock)
{
if (_tfBuffer != IntPtr.Zero)
{
try
{
TF3NativeInterface.tf3_buffer_destroy(_tfBuffer);
_logger?.LogInformation("[TF3] TF3 buffer disposed successfully");
}
catch (Exception ex)
{
_logger?.LogError(ex, "[TF3] Error disposing TF3 buffer: {Message}", ex.Message);
}
finally
{
_tfBuffer = IntPtr.Zero;
_initialized = false;
}
}
}
_disposed = true;
}
~TF3BufferManager()
{
Dispose(false);
}
}

138
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/TF3/TF3NativeInterface.cs Normal file
View File

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