399 lines
14 KiB
C#
399 lines
14 KiB
C#
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);
|
|
}
|
|
}
|