381 lines
14 KiB
C#
381 lines
14 KiB
C#
using RobotNet10.RobotApp.TF3;
|
|
using RobotNet10.Shared.Geometry;
|
|
using Microsoft.Extensions.Configuration;
|
|
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;
|
|
private readonly List<Tf3StaticTransformConfig> _staticTransforms;
|
|
|
|
// Fallback used when "Tf3:StaticTransforms" is missing from configuration (e.g. Examples/ usage without IConfiguration)
|
|
private static List<Tf3StaticTransformConfig> DefaultStaticTransforms => new()
|
|
{
|
|
new Tf3StaticTransformConfig
|
|
{
|
|
FrameId = "base_link",
|
|
ChildFrameId = "imu",
|
|
Pose = new Pose(new Point(0.0, 0.0, 0.0), new Quaternion(1.0, 0.0, 0.0, 0.0))
|
|
},
|
|
new Tf3StaticTransformConfig
|
|
{
|
|
FrameId = "base_link",
|
|
ChildFrameId = "scan_1",
|
|
Pose = new Pose(new Point(0.095, 0.01, 0.0), new Quaternion(0.0, 0.0, 0.0261769, 0.9996573))
|
|
},
|
|
new Tf3StaticTransformConfig
|
|
{
|
|
FrameId = "base_link",
|
|
ChildFrameId = "scan_2",
|
|
Pose = new Pose(new Point(-0.355, -0.01, 0.0), new Quaternion(0.0, 0.0, 1.0, 0.0))
|
|
},
|
|
new Tf3StaticTransformConfig
|
|
{
|
|
FrameId = "base_link",
|
|
ChildFrameId = "scan_3",
|
|
Pose = new Pose(new Point(0.707, 0.2825, 0.0), new Quaternion(0.0, 0.0, 0.3826834, 0.9238795))
|
|
}
|
|
};
|
|
|
|
/// <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 configuration and logger.
|
|
/// Static transforms are bound from "Tf3:StaticTransforms"; falls back to defaults if absent.
|
|
/// </summary>
|
|
public TF3BufferManager(IConfiguration? configuration = null, ILogger? logger = null)
|
|
{
|
|
_logger = logger;
|
|
|
|
var configuredTransforms = configuration?.GetSection("Tf3:StaticTransforms")
|
|
.Get<List<Tf3StaticTransformConfig>>();
|
|
_staticTransforms = (configuredTransforms != null && configuredTransforms.Count > 0)
|
|
? configuredTransforms
|
|
: DefaultStaticTransforms;
|
|
}
|
|
|
|
/// <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;
|
|
|
|
// Static transforms configured via appsettings.json ("Tf3:StaticTransforms")
|
|
foreach (var config in _staticTransforms)
|
|
{
|
|
var transform = new TF3_Transform
|
|
{
|
|
timestamp_sec = sec,
|
|
timestamp_nsec = nsec,
|
|
frame_id = config.FrameId,
|
|
child_frame_id = config.ChildFrameId,
|
|
translation_x = config.Pose.Position.X,
|
|
translation_y = config.Pose.Position.Y,
|
|
translation_z = config.Pose.Position.Z,
|
|
rotation_x = config.Pose.Orientation.X,
|
|
rotation_y = config.Pose.Orientation.Y,
|
|
rotation_z = config.Pose.Orientation.Z,
|
|
rotation_w = config.Pose.Orientation.W
|
|
};
|
|
|
|
_logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
|
|
transform.frame_id, transform.child_frame_id);
|
|
TF3NativeInterface.tf3_set_transform(_tfBuffer, ref transform, "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);
|
|
}
|
|
}
|