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