Initial commit
This commit is contained in:
@@ -0,0 +1,699 @@
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace RobotNet10.RobotApp.MarkerDetection;
|
||||
|
||||
/// <summary>
|
||||
/// Integration service to dispatch laser scan data to marker detection engine
|
||||
/// Provides rectangle or segment marker detection with pose estimation
|
||||
/// </summary>
|
||||
public class MarkerDetectionIntegrationService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly MarkerDetectionIntegrationConfiguration _config;
|
||||
private readonly IDeviceProvider _deviceProvider;
|
||||
private readonly ILogger<MarkerDetectionIntegrationService> _logger;
|
||||
private readonly object _lock = new();
|
||||
private readonly SemaphoreSlim _dispatchSemaphore = new SemaphoreSlim(1, 1); // Only 1 dispatch at a time
|
||||
|
||||
private MarkerDetectionClient? _markerDetectionClient;
|
||||
|
||||
// Timer for periodic marker pose updates and logging
|
||||
private Timer? _updateTimer;
|
||||
|
||||
// Cached device references
|
||||
private ILidar? _lidarDevice;
|
||||
|
||||
// Statistics (accessed from multiple threads)
|
||||
private long _laserScanCount = 0;
|
||||
private long _markerDetectionCount = 0;
|
||||
private DateTime _lastStatsLog = DateTime.UtcNow;
|
||||
|
||||
private bool _disposed = false;
|
||||
private bool _isInitialized = false;
|
||||
|
||||
public MarkerDetectionIntegrationService(
|
||||
IConfiguration configuration,
|
||||
IDeviceProvider deviceProvider,
|
||||
ILogger<MarkerDetectionIntegrationService> logger)
|
||||
{
|
||||
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Load configuration
|
||||
var configSection = configuration.GetSection("MarkerDetection:Integration");
|
||||
if (!configSection.Exists())
|
||||
{
|
||||
_logger.LogWarning("Configuration section 'MarkerDetection:Integration' not found. Using defaults.");
|
||||
_config = new MarkerDetectionIntegrationConfiguration();
|
||||
}
|
||||
else
|
||||
{
|
||||
_config = new MarkerDetectionIntegrationConfiguration();
|
||||
configSection.Bind(_config);
|
||||
}
|
||||
|
||||
ValidateConfiguration();
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
if (_config.Enabled)
|
||||
{
|
||||
if (_config.LaserScanDispatchIntervalMs <= 0 || _config.LaserScanDispatchIntervalMs > 1000)
|
||||
{
|
||||
_logger.LogWarning("LaserScanDispatchIntervalMs {Interval} is out of range [1-1000]. Using default 50 ms.", _config.LaserScanDispatchIntervalMs);
|
||||
_config.LaserScanDispatchIntervalMs = 50;
|
||||
}
|
||||
|
||||
if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions == null)
|
||||
{
|
||||
_logger.LogWarning("Rectangle marker type selected but options not provided. Using defaults.");
|
||||
_config.RectangleMarkerOptions = new MarkerDetectionRectangleOptions();
|
||||
}
|
||||
|
||||
if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions == null)
|
||||
{
|
||||
_logger.LogWarning("Segment marker type selected but options not provided. Using defaults.");
|
||||
_config.SegmentMarkerOptions = new MarkerDetectionSegmentOptions();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (!_config.Enabled)
|
||||
{
|
||||
_logger.LogInformation("MarkerDetectionIntegrationService is disabled in configuration");
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("Starting MarkerDetectionIntegrationService...");
|
||||
|
||||
try
|
||||
{
|
||||
// Wait for devices to be connected
|
||||
_logger.LogInformation("Waiting for devices to be connected...");
|
||||
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
||||
|
||||
if (!connected)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will retry initialization.");
|
||||
_ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
|
||||
return;
|
||||
}
|
||||
|
||||
await InitializeAsync(cancellationToken);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting MarkerDetectionIntegrationService: {Message}", ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task RetryInitializationAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
const int maxRetries = 60; // 5 minutes with 5 second intervals
|
||||
int retryCount = 0;
|
||||
|
||||
while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested && !_isInitialized)
|
||||
{
|
||||
await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);
|
||||
|
||||
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(1), cancellationToken);
|
||||
if (connected)
|
||||
{
|
||||
_logger.LogInformation("Devices are now connected. Initializing MarkerDetectionIntegrationService...");
|
||||
await InitializeAsync(cancellationToken);
|
||||
return;
|
||||
}
|
||||
|
||||
retryCount++;
|
||||
if (retryCount % 12 == 0) // Log every minute
|
||||
{
|
||||
_logger.LogInformation("Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
|
||||
retryCount, maxRetries);
|
||||
}
|
||||
}
|
||||
|
||||
if (retryCount >= maxRetries)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will not be initialized.");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task InitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_isInitialized)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
// Get LIDAR device reference
|
||||
if (!string.IsNullOrEmpty(_config.LidarDeviceId))
|
||||
{
|
||||
var device = _deviceProvider.GetDevice(_config.LidarDeviceId);
|
||||
if (device != null)
|
||||
{
|
||||
_logger.LogInformation(
|
||||
"[LIDAR-INIT] Device '{DeviceId}' found. Type: {DeviceType}, IsConnected: {IsConnected}",
|
||||
_config.LidarDeviceId, device.GetType().Name, device.IsConnected);
|
||||
}
|
||||
|
||||
if (device is ILidar lidar && device.IsConnected)
|
||||
{
|
||||
_lidarDevice = lidar;
|
||||
_logger.LogInformation(
|
||||
"[LIDAR-INIT] SUCCESS: LIDAR device '{DeviceId}' is initialized and connected. Ready to receive laser scan data.",
|
||||
_config.LidarDeviceId);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. Marker detection will not work.",
|
||||
_config.LidarDeviceId);
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.LogWarning("No LIDAR device ID configured. Marker detection will not work.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Create and initialize marker detection client
|
||||
_markerDetectionClient = new MarkerDetectionClient(_logger);
|
||||
|
||||
// Initialize with appropriate marker type
|
||||
if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions != null)
|
||||
{
|
||||
_markerDetectionClient.InitializeRectangle(_config.RectangleMarkerOptions);
|
||||
}
|
||||
else if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions != null)
|
||||
{
|
||||
_markerDetectionClient.InitializeSegment(_config.SegmentMarkerOptions);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException("Invalid marker type configuration or missing marker options.");
|
||||
}
|
||||
|
||||
_logger.LogInformation("MarkerDetectionClient initialized successfully");
|
||||
|
||||
// Start laser scan dispatch loop as background task (non-blocking)
|
||||
if (_lidarDevice != null)
|
||||
{
|
||||
_ = Task.Run(async () => await LaserScanDispatchLoopAsync(cancellationToken), cancellationToken);
|
||||
_logger.LogInformation("Laser scan dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
|
||||
}
|
||||
|
||||
// Start periodic update timer for pose logging and status updates
|
||||
_updateTimer = new Timer(UpdatePoseAndDiagnostics, null,
|
||||
TimeSpan.FromSeconds(2), TimeSpan.FromSeconds(2));
|
||||
_logger.LogInformation("MarkerDetection update timer started (every 2 seconds)");
|
||||
|
||||
_isInitialized = true;
|
||||
_logger.LogInformation("MarkerDetectionIntegrationService initialized successfully");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Failed to initialize MarkerDetectionIntegrationService: {Message}", ex.Message);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Stopping MarkerDetectionIntegrationService...");
|
||||
|
||||
try
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_isInitialized = false;
|
||||
|
||||
// Dispose timer
|
||||
_updateTimer?.Dispose();
|
||||
_updateTimer = null;
|
||||
}
|
||||
|
||||
// Wait a bit for dispatch loops to notice _isInitialized = false and stop
|
||||
_logger.LogInformation("Waiting for dispatch loops to stop...");
|
||||
await Task.Delay(500, cancellationToken);
|
||||
|
||||
// Clean up marker detection client
|
||||
if (_markerDetectionClient != null)
|
||||
{
|
||||
_markerDetectionClient.Dispose();
|
||||
_markerDetectionClient = null;
|
||||
_logger.LogInformation("MarkerDetectionClient disposed successfully");
|
||||
}
|
||||
|
||||
LogFinalStatistics();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping MarkerDetectionIntegrationService: {Message}", ex.Message);
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Laser scan dispatch loop - runs in background task
|
||||
/// </summary>
|
||||
private async Task LaserScanDispatchLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
int noDataCounter = 0;
|
||||
|
||||
while (_config.Enabled && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
if (!_isInitialized || _markerDetectionClient == null || _lidarDevice == null || !_markerDetectionClient.IsInitialized)
|
||||
{
|
||||
await Task.Delay(100, cancellationToken);
|
||||
continue;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var scan = _lidarDevice.CurrentMeasurementData;
|
||||
|
||||
// Log periodically to monitor intensities status (every ~5 seconds at 20Hz)
|
||||
if (scan.HasValue && _laserScanCount % 100 == 0)
|
||||
{
|
||||
var rawScan = scan.Value;
|
||||
if (rawScan.Intensities == null || rawScan.Intensities.Length == 0)
|
||||
{
|
||||
_logger.LogWarning(
|
||||
"[MARKER-DETECTION] LIDAR NOT providing intensities (Ranges: {RangeCount}). " +
|
||||
"Using default values. Check LIDAR RSSI configuration.",
|
||||
rawScan.Ranges?.Length ?? 0);
|
||||
}
|
||||
// else
|
||||
// {
|
||||
// _logger.LogInformation(
|
||||
// "[MARKER-DETECTION] ✓ LIDAR providing REAL intensities: {Count} values, " +
|
||||
// "Sample: [{I0:F3}, {I1:F3}, {I2:F3}]",
|
||||
// rawScan.Intensities.Length,
|
||||
// rawScan.Intensities[0],
|
||||
// rawScan.Intensities.Length > 1 ? rawScan.Intensities[1] : 0,
|
||||
// rawScan.Intensities.Length > 2 ? rawScan.Intensities[2] : 0);
|
||||
// }
|
||||
}
|
||||
if (scan == null)
|
||||
{
|
||||
noDataCounter++;
|
||||
if (noDataCounter % 100 == 0) // Log every 100 iterations (5 seconds at 20Hz)
|
||||
{
|
||||
// _logger.LogWarning(
|
||||
// "[LIDAR-DATA-DEBUG] CurrentMeasurementData is NULL. " +
|
||||
// "LIDAR device may not be connected or initialized. " +
|
||||
// "Configured device: '{LidarDeviceId}'. Missing data for {MissingIterations} iterations.",
|
||||
// _config.LidarDeviceId, noDataCounter);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var originalScan = scan.Value; // Assuming CurrentMeasurementData is nullable
|
||||
//in ra dữ liệu intensities và ranges để debug
|
||||
// _logger.LogInformation("LIDAR Data Debug - Ranges Count: {RangesCount}, Intensities Count: {IntensitiesCount}",
|
||||
// originalScan.Ranges?.Length ?? 0, originalScan.Intensities?.Length ?? 0);
|
||||
|
||||
// Validate scan data before processing
|
||||
// LIDAR must provide at least range data (Intensities may be empty)
|
||||
if (originalScan.Ranges == null || originalScan.Ranges.Length == 0)
|
||||
{
|
||||
noDataCounter++;
|
||||
// if (noDataCounter % 100 == 0)
|
||||
// {
|
||||
// _logger.LogWarning(
|
||||
// "[LIDAR-DATA-DEBUG] No RANGE data in laser scan. " +
|
||||
// "Ranges: {RangeLength}, Intensities: {IntensityLength}. " +
|
||||
// "LIDAR device '{LidarDeviceId}' is not providing valid range measurements. " +
|
||||
// "Missing data for {MissingIterations} iterations.",
|
||||
// originalScan.Ranges?.Length ?? 0,
|
||||
// originalScan.Intensities?.Length ?? 0,
|
||||
// _config.LidarDeviceId,
|
||||
// noDataCounter);
|
||||
// }
|
||||
continue;
|
||||
}
|
||||
|
||||
// Ensure Intensities array is not null
|
||||
// If LIDAR doesn't provide intensities, populate with default values
|
||||
// if (originalScan.Intensities == null || originalScan.Intensities.Length == 0)
|
||||
// {
|
||||
// originalScan.Intensities = new float[originalScan.Ranges.Length];
|
||||
// for (int i = 0; i < originalScan.Intensities.Length; i++)
|
||||
// {
|
||||
// originalScan.Intensities[i] = 1.0f; // Default fallback value
|
||||
// }
|
||||
// }
|
||||
|
||||
// Data is valid - reset counter
|
||||
noDataCounter = 0;
|
||||
|
||||
// Log once per second when valid data is being received
|
||||
// if (noDataCounter == 0 && _laserScanCount % 20 == 0) // ~1 second at 20Hz
|
||||
// {
|
||||
// _logger.LogDebug(
|
||||
// "[LIDAR-DATA-OK] Valid laser scan received: Ranges: {RangeLength}, Intensities: {IntensityLength}",
|
||||
// originalScan.Ranges.Length,
|
||||
// originalScan.Intensities.Length);
|
||||
// }
|
||||
|
||||
// Create updated scan with current timestamp
|
||||
var updatedScan = new LaserScan
|
||||
{
|
||||
Header = new Header
|
||||
{
|
||||
Seq = originalScan.Header.Seq,
|
||||
Stamp = DateTime.UtcNow,
|
||||
FrameId = "scan"
|
||||
},
|
||||
AngleMin = (float)-2.356194496154785,
|
||||
AngleMax = (float)2.356194496154785,
|
||||
AngleIncrement =(float) 0.00581718236207962,
|
||||
TimeIncrement = (float) 6.172839493956417e-05,
|
||||
ScanTime = (float) 0.06666667014360428,
|
||||
RangeMin = (float) 0.0,
|
||||
RangeMax = (float) 100.0,
|
||||
Ranges = originalScan.Ranges,
|
||||
Intensities = originalScan.Intensities
|
||||
};
|
||||
// In ra tất cả giũ liệu trong mảng Intensities
|
||||
// if (updatedScan.Intensities != null)
|
||||
// {
|
||||
// _logger.LogDebug("Intensities array length: {Length}", updatedScan.Intensities.Length);
|
||||
// for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
|
||||
// {
|
||||
// _logger.LogInformation("Intensity[{Index}]: {Value}", i, originalScan.Intensities[i]);
|
||||
// _logger.LogInformation("Range[{Index}]: {Value}", i, updatedScan.Ranges[i]);
|
||||
// }
|
||||
// }
|
||||
// In ra tất cả giũ liệu trong mảng Ranges và Intensities trong cùng 1 dòng và Seq, Stamp, FrameId để debug
|
||||
// if (updatedScan.Intensities != null)
|
||||
// {
|
||||
// for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
|
||||
// {
|
||||
// _logger.LogInformation("Detect marker Seq: {Seq}, Stamp: {Stamp}, FrameId: {FrameId}, Range[{Index}]: {Range}, Intensity[{Index}]: {Intensity}",
|
||||
// updatedScan.Header.Seq,
|
||||
// updatedScan.Header.Stamp.ToString("HH:mm:ss.ffff"),
|
||||
// updatedScan.Header.FrameId,
|
||||
// i,
|
||||
// updatedScan.Ranges[i],
|
||||
// i,
|
||||
// updatedScan.Intensities[i]);
|
||||
// }
|
||||
// }
|
||||
// Dispatch laser scan to marker detection
|
||||
// Re-check that client is still valid (could have been disposed by another thread)
|
||||
if (_markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
await _dispatchSemaphore.WaitAsync(cancellationToken);
|
||||
try
|
||||
{
|
||||
// Double-check again after acquiring semaphore
|
||||
if (_markerDetectionClient != null && _markerDetectionClient.IsInitialized)
|
||||
{
|
||||
int rangeCount = updatedScan.Ranges?.Length ?? 0;
|
||||
int intensityCount = updatedScan.Intensities?.Length ?? 0;
|
||||
|
||||
// _logger.LogInformation(
|
||||
// "[MARKER-DETECTION-DISPATCH] Sending to native code - " +
|
||||
// "Ranges: {RangeCount}, Intensities: {IntensityStatus} (Count: {IntensityCount})",
|
||||
// rangeCount,
|
||||
// updatedScan.Intensities == null ? "NULL" : "OK",
|
||||
// intensityCount);
|
||||
|
||||
// if (updatedScan.Intensities != null && intensityCount > 2)
|
||||
// {
|
||||
// _logger.LogInformation(
|
||||
// "[MARKER-DETECTION-DISPATCH] First 3 intensities being sent: [{I0:F2}, {I1:F2}, {I2:F2}]",
|
||||
// updatedScan.Intensities[0],
|
||||
// updatedScan.Intensities[1],
|
||||
// updatedScan.Intensities[2]);
|
||||
// }
|
||||
|
||||
_markerDetectionClient.DispatchLaserScan(updatedScan, _config.LaserScanSensorId ?? "scan");
|
||||
Interlocked.Increment(ref _laserScanCount);
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
_dispatchSemaphore.Release();
|
||||
}
|
||||
}
|
||||
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
||||
{
|
||||
// MarkerDetectionClient was disposed during shutdown - this is normal, just exit gracefully
|
||||
_logger.LogDebug("Laser scan dispatch loop stopped: {Message}", ex.Message);
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error dispatching laser scan data: {Message}", ex.Message);
|
||||
}
|
||||
|
||||
// Wait for next cycle
|
||||
await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Periodic timer callback to update marker pose and log diagnostics
|
||||
/// </summary>
|
||||
private void UpdatePoseAndDiagnostics(object? state)
|
||||
{
|
||||
if (!_isInitialized || _markerDetectionClient == null)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
// Get current marker pose if available
|
||||
if (_markerDetectionClient.IsInitialized)
|
||||
{
|
||||
var pose = _markerDetectionClient.GetMarkerPose();
|
||||
if (pose != null)
|
||||
{
|
||||
Interlocked.Increment(ref _markerDetectionCount);
|
||||
// _logger.LogDebug("Marker pose: x={X}, y={Y}, z={Z}, qx={Qx}, qy={Qy}, qz={Qz}, qw={Qw}, frame={Frame}",
|
||||
// pose.Pose.Position[0], pose.Pose.Position[1], pose.Pose.Position[2],
|
||||
// pose.Pose.Orientation[0], pose.Pose.Orientation[1], pose.Pose.Orientation[2], pose.Pose.Orientation[3],
|
||||
// pose.Header.FrameId);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error updating marker pose and diagnostics: {Message}", ex.Message);
|
||||
}
|
||||
|
||||
// Log statistics periodically
|
||||
var now = DateTime.UtcNow;
|
||||
if ((now - _lastStatsLog).TotalSeconds >= 10)
|
||||
{
|
||||
_lastStatsLog = now;
|
||||
LogStatistics();
|
||||
}
|
||||
}
|
||||
|
||||
private void LogStatistics()
|
||||
{
|
||||
var laserCount = Interlocked.Read(ref _laserScanCount);
|
||||
var markerCount = Interlocked.Read(ref _markerDetectionCount);
|
||||
|
||||
if (laserCount > 0)
|
||||
{
|
||||
var laserRate = laserCount / ((DateTime.UtcNow - _lastStatsLog).TotalSeconds + 0.001);
|
||||
// _logger.LogInformation(
|
||||
// "[MarkerDetection Stats] LaserScans: {LaserCount} (avg {LaserRate:F2} Hz), Marker Poses: {MarkerCount}",
|
||||
// laserCount, laserRate, markerCount);
|
||||
}
|
||||
else
|
||||
{
|
||||
// LIDAR data still empty - provide diagnostic info
|
||||
_logger.LogWarning(
|
||||
"[MarkerDetection Stats] No laser scan data received. " +
|
||||
"LIDAR Device ID configured: '{ConfigDeviceId}'. " +
|
||||
"Please verify LIDAR device is connected and providing data.",
|
||||
_config.LidarDeviceId);
|
||||
}
|
||||
}
|
||||
|
||||
private void LogFinalStatistics()
|
||||
{
|
||||
var laserCount = Interlocked.Read(ref _laserScanCount);
|
||||
var markerCount = Interlocked.Read(ref _markerDetectionCount);
|
||||
|
||||
_logger.LogInformation(
|
||||
"[MarkerDetection Final Stats] Total LaserScans: {LaserCount}, Total Marker Poses: {MarkerCount}",
|
||||
laserCount, markerCount);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Enable or disable marker detection
|
||||
/// </summary>
|
||||
public void SetEnableDetection(bool enable)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
|
||||
{
|
||||
_logger.LogWarning("Cannot set detection enable/disable: MarkerDetectionIntegrationService not initialized");
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_markerDetectionClient.SetEnableDetection(enable);
|
||||
_logger.LogInformation("Marker detection {Status}", enable ? "enabled" : "disabled");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error setting detection enable/disable: {Message}", ex.Message);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get current marker pose
|
||||
/// </summary>
|
||||
public MarkerDetectionPoseStamped? GetMarkerPose()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
|
||||
{
|
||||
_logger.LogWarning("Cannot get marker pose: MarkerDetectionIntegrationService not initialized");
|
||||
return null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
return _markerDetectionClient.GetMarkerPose();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error getting marker pose: {Message}", ex.Message);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_isInitialized = false;
|
||||
}
|
||||
|
||||
// Give loop time to stop
|
||||
Thread.Sleep(500);
|
||||
|
||||
_updateTimer?.Dispose();
|
||||
_markerDetectionClient?.Dispose();
|
||||
_dispatchSemaphore?.Dispose();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing MarkerDetectionIntegrationService: {Message}", ex.Message);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Marker detection type
|
||||
/// </summary>
|
||||
public enum MarkerDetectionType
|
||||
{
|
||||
Rectangle,
|
||||
Segment
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configuration for MarkerDetectionIntegrationService
|
||||
/// </summary>
|
||||
public class MarkerDetectionIntegrationConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Enable/disable the integration service
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Type of marker to detect (Rectangle or Segment)
|
||||
/// </summary>
|
||||
public MarkerDetectionType MarkerType { get; set; } = MarkerDetectionType.Rectangle;
|
||||
|
||||
/// <summary>
|
||||
/// Rectangle marker detection options
|
||||
/// Required if MarkerType is Rectangle
|
||||
/// </summary>
|
||||
public MarkerDetectionRectangleOptions? RectangleMarkerOptions { get; set; } = null;
|
||||
|
||||
/// <summary>
|
||||
/// Segment marker detection options
|
||||
/// Required if MarkerType is Segment
|
||||
/// </summary>
|
||||
public MarkerDetectionSegmentOptions? SegmentMarkerOptions { get; set; } = null;
|
||||
|
||||
/// <summary>
|
||||
/// LIDAR device ID for laser scan dispatch
|
||||
/// </summary>
|
||||
public string LidarDeviceId { get; set; } = "scan_1";
|
||||
|
||||
/// <summary>
|
||||
/// Sensor ID for laser scan
|
||||
/// </summary>
|
||||
public string? LaserScanSensorId { get; set; } = "scan_1";
|
||||
|
||||
/// <summary>
|
||||
/// Frame ID for laser scan
|
||||
/// </summary>
|
||||
public string? LaserScanFrameId { get; set; } = "scan_1";
|
||||
|
||||
/// <summary>
|
||||
/// Laser scan data dispatch interval in milliseconds (default: 50ms = 20Hz)
|
||||
/// </summary>
|
||||
public int LaserScanDispatchIntervalMs { get; set; } = 50;
|
||||
|
||||
/// <summary>
|
||||
/// Enable estimated marker pose for improved stability
|
||||
/// </summary>
|
||||
public bool EnableEstimateMarkerPose { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Initial estimated marker pose X (meters)
|
||||
/// </summary>
|
||||
public double EstimateMarkerPoseX { get; set; } = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Initial estimated marker pose Y (meters)
|
||||
/// </summary>
|
||||
public double EstimateMarkerPoseY { get; set; } = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Initial estimated marker pose theta (radians)
|
||||
/// </summary>
|
||||
public double EstimateMarkerPoseTheta { get; set; } = 0.0;
|
||||
}
|
||||
Reference in New Issue
Block a user