339 lines
14 KiB
C#
339 lines
14 KiB
C#
using RobotNet10.RobotApp.TF3;
|
|
using RobotNet10.RobotApp.Xloc;
|
|
using RobotNet10.RobotApp.Navigation;
|
|
|
|
namespace RobotNet10.RobotApp.Examples;
|
|
|
|
/// <summary>
|
|
/// Example: How to properly initialize TF3BufferManager, XlocClient, and NavigationClient
|
|
///
|
|
/// This demonstrates the correct initialization sequence:
|
|
/// 1. Create and initialize TF3BufferManager
|
|
/// 2. Get the TF3 buffer
|
|
/// 3. Pass it to XlocClient and NavigationClient (both use same buffer)
|
|
/// 4. Proper cleanup on shutdown
|
|
/// </summary>
|
|
public class TF3XlocInitializationExample
|
|
{
|
|
private readonly ILogger<TF3XlocInitializationExample> _logger;
|
|
private TF3BufferManager? _tf3Manager;
|
|
private XlocClient? _xlocClient;
|
|
private NavigationClient? _navigationClient;
|
|
|
|
public TF3XlocInitializationExample(ILogger<TF3XlocInitializationExample> logger)
|
|
{
|
|
_logger = logger;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize all required components in the correct order
|
|
/// </summary>
|
|
public async Task StartupAsync()
|
|
{
|
|
try
|
|
{
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
_logger.LogInformation("Starting RobotApp with TF3 and XLOC");
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
|
|
// ========================================================================
|
|
// STEP 1: Initialize TF3BufferManager (MUST be first!)
|
|
// ========================================================================
|
|
_logger.LogInformation("[1/3] Initializing TF3BufferManager...");
|
|
_logger.LogInformation(" → Creating TF3 buffer with 10-second cache");
|
|
_logger.LogInformation(" → Publishing static robot transforms");
|
|
|
|
_tf3Manager = new TF3BufferManager(_logger);
|
|
_tf3Manager.Initialize();
|
|
|
|
_logger.LogInformation("✓ TF3BufferManager initialized successfully");
|
|
_logger.LogInformation(" Available frames: base_link, imu, scan_1, scan_2, scan_3");
|
|
|
|
// ========================================================================
|
|
// STEP 2: Initialize XlocClient with TF3 buffer
|
|
// ========================================================================
|
|
_logger.LogInformation("[2/4] Initializing XlocClient...");
|
|
_logger.LogInformation(" → Creating XLOC instance");
|
|
_logger.LogInformation(" → Using pre-initialized TF3 buffer");
|
|
|
|
_xlocClient = new XlocClient(_tf3Manager.TfBuffer, _logger);
|
|
_xlocClient.Initialize();
|
|
|
|
_logger.LogInformation("✓ XlocClient initialized successfully");
|
|
_logger.LogInformation(" Ready to load maps and perform localization/mapping");
|
|
|
|
// ========================================================================
|
|
// STEP 3: Initialize NavigationClient with same TF3 buffer
|
|
// ========================================================================
|
|
_logger.LogInformation("[3/4] Initializing NavigationClient...");
|
|
_logger.LogInformation(" → Creating Navigation instance");
|
|
_logger.LogInformation(" → Using same TF3 buffer as XlocClient");
|
|
|
|
_navigationClient = new NavigationClient(_tf3Manager.TfBuffer, _logger);
|
|
_navigationClient.Initialize();
|
|
|
|
_logger.LogInformation("✓ NavigationClient initialized successfully");
|
|
_logger.LogInformation(" Both XLOC and Navigation use the same TF3 buffer");
|
|
|
|
// ========================================================================
|
|
// STEP 4 (Optional): Activate map and start localization
|
|
// ========================================================================
|
|
_logger.LogInformation("[4/4] Loading map...");
|
|
|
|
string mapPath = "path/to/your/map.pgm";
|
|
if (_xlocClient.ActivateMap(mapPath))
|
|
{
|
|
_logger.LogInformation("✓ Map loaded: {MapPath}", mapPath);
|
|
|
|
// Start localization
|
|
if (_xlocClient.StartLocalization())
|
|
{
|
|
_logger.LogInformation("✓ Localization started");
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("⚠ Failed to start localization");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("⚠ Failed to load map: {MapPath}", mapPath);
|
|
}
|
|
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
_logger.LogInformation("✓ RobotApp startup complete");
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "✗ Error during startup");
|
|
await ShutdownAsync();
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Demonstrate XlocClient usage
|
|
/// </summary>
|
|
public void DemonstrateUsage()
|
|
{
|
|
if (_xlocClient == null)
|
|
{
|
|
_logger.LogError("XlocClient not initialized");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
_logger.LogInformation("XlocClient Usage Examples");
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
|
|
// Get current pose
|
|
var pose = _xlocClient.GetCurrentPose();
|
|
if (pose.HasValue)
|
|
{
|
|
_logger.LogInformation("Current Pose:");
|
|
_logger.LogInformation(" Position: ({X:F3}, {Y:F3}, {Z:F3})",
|
|
pose.Value.x, pose.Value.y, pose.Value.z);
|
|
_logger.LogInformation(" Orientation: ({Qx:F3}, {Qy:F3}, {Qz:F3}, {Qw:F3})",
|
|
pose.Value.qx, pose.Value.qy, pose.Value.qz, pose.Value.qw);
|
|
}
|
|
|
|
// Get diagnostics
|
|
var diag = _xlocClient.GetDiagnostics();
|
|
if (diag != null)
|
|
{
|
|
_logger.LogInformation("Diagnostics:");
|
|
_logger.LogInformation(" State: {State}", diag.StateString);
|
|
_logger.LogInformation(" Reliability: {Reliability:P2}", diag.Reliability);
|
|
_logger.LogInformation(" Matching Score: {Score:F3}", diag.MatchingScore);
|
|
}
|
|
|
|
// Get static map
|
|
var staticMap = _xlocClient.GetStaticGridMap();
|
|
if (staticMap != null)
|
|
{
|
|
_logger.LogInformation("Static Grid Map:");
|
|
_logger.LogInformation(" Resolution: {Resolution:F3} m/cell", staticMap.Resolution);
|
|
_logger.LogInformation(" Size: {Width}x{Height} cells", staticMap.Width, staticMap.Height);
|
|
}
|
|
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error during demonstration");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clean up all resources on shutdown
|
|
/// IMPORTANT: This is the reverse order of initialization
|
|
/// </summary>
|
|
public async Task ShutdownAsync()
|
|
{
|
|
try
|
|
{
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
_logger.LogInformation("Shutting down RobotApp");
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
|
|
// Stop any active operations
|
|
if (_xlocClient?.IsInitialized ?? false)
|
|
{
|
|
try
|
|
{
|
|
_logger.LogInformation("Stopping localization...");
|
|
_xlocClient.StopLocalization();
|
|
_logger.LogInformation("✓ Localization stopped");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "Error stopping localization");
|
|
}
|
|
}
|
|
|
|
// Shutdown in reverse order of startup
|
|
// First: NavigationClient
|
|
if (_navigationClient != null)
|
|
{
|
|
_logger.LogInformation("Disposing NavigationClient...");
|
|
_navigationClient.Dispose();
|
|
_navigationClient = null;
|
|
_logger.LogInformation("✓ NavigationClient disposed");
|
|
}
|
|
|
|
// Second: XlocClient
|
|
if (_xlocClient != null)
|
|
{
|
|
_logger.LogInformation("Disposing XlocClient...");
|
|
_xlocClient.Dispose();
|
|
_xlocClient = null;
|
|
_logger.LogInformation("✓ XlocClient disposed");
|
|
}
|
|
|
|
// Third: TF3BufferManager
|
|
if (_tf3Manager != null)
|
|
{
|
|
_logger.LogInformation("Disposing TF3BufferManager...");
|
|
_tf3Manager.Dispose();
|
|
_tf3Manager = null;
|
|
_logger.LogInformation("✓ TF3BufferManager disposed");
|
|
}
|
|
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
_logger.LogInformation("✓ Shutdown complete");
|
|
_logger.LogInformation("═══════════════════════════════════════════════════════════");
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "✗ Error during shutdown");
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Example: Using TF3BufferManager with multiple components
|
|
/// </summary>
|
|
public class MultiComponentExample
|
|
{
|
|
private readonly ILogger<MultiComponentExample> _logger;
|
|
private TF3BufferManager? _tf3Manager;
|
|
private XlocClient? _xlocClient;
|
|
private NavigationClient? _navigationClient;
|
|
|
|
public MultiComponentExample(ILogger<MultiComponentExample> logger)
|
|
{
|
|
_logger = logger;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize TF3 once, then pass it to multiple components
|
|
/// </summary>
|
|
public async Task InitializeAsync()
|
|
{
|
|
try
|
|
{
|
|
_logger.LogInformation("Initializing components with shared TF3 buffer...");
|
|
|
|
// 1. Initialize TF3 (shared by all components)
|
|
_tf3Manager = new TF3BufferManager(_logger);
|
|
_tf3Manager.Initialize();
|
|
_logger.LogInformation("✓ TF3 initialized");
|
|
|
|
IntPtr tfBuffer = _tf3Manager.TfBuffer;
|
|
|
|
// 2. Initialize components that use TF3
|
|
_xlocClient = new XlocClient(tfBuffer, _logger);
|
|
_xlocClient.Initialize();
|
|
_logger.LogInformation("✓ XlocClient initialized");
|
|
|
|
// Initialize NavigationClient with same buffer
|
|
_navigationClient = new NavigationClient(tfBuffer, _logger);
|
|
_navigationClient.Initialize();
|
|
_logger.LogInformation("✓ NavigationClient initialized");
|
|
|
|
// Both components are now using the same TF3 buffer
|
|
_logger.LogInformation("✓ All components share the same TF3 buffer");
|
|
// _markerDetectionManager.Initialize();
|
|
// _logger.LogInformation("✓ MarkerDetectionManager initialized");
|
|
|
|
_logger.LogInformation("✓ All components initialized with shared TF3");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Failed to initialize components");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// All components share the same TF3 buffer transforms
|
|
/// </summary>
|
|
public void PublishCustomTransform()
|
|
{
|
|
if (_tf3Manager?.IsInitialized != true)
|
|
{
|
|
_logger.LogWarning("TF3 not initialized");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
// All components benefit from any new transforms published to TF3
|
|
var customTransform = new TF3.TF3_Transform
|
|
{
|
|
timestamp_sec = (long)DateTimeOffset.UtcNow.ToUnixTimeSeconds(),
|
|
timestamp_nsec = 0,
|
|
frame_id = "base_link",
|
|
child_frame_id = "payload_sensor",
|
|
translation_x = 0.3,
|
|
translation_y = 0.0,
|
|
translation_z = 0.15,
|
|
rotation_x = 0.0,
|
|
rotation_y = 0.0,
|
|
rotation_z = 0.0,
|
|
rotation_w = 1.0
|
|
};
|
|
|
|
_tf3Manager.PublishTransform(customTransform, isStatic: true);
|
|
_logger.LogInformation("✓ Custom transform published - available to all components");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Failed to publish custom transform");
|
|
}
|
|
}
|
|
|
|
public async Task ShutdownAsync()
|
|
{
|
|
_navigationClient?.Dispose();
|
|
_xlocClient?.Dispose();
|
|
_tf3Manager?.Dispose();
|
|
await Task.CompletedTask;
|
|
}
|
|
}
|