Initial commit
This commit is contained in:
@@ -0,0 +1,380 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user