1844 lines
77 KiB
C#
1844 lines
77 KiB
C#
using Microsoft.Extensions.Configuration;
|
|
using Microsoft.Extensions.Hosting;
|
|
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Xloc;
|
|
using System;
|
|
using System.Runtime.InteropServices;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using RobotNet10.Shared.Sensor;
|
|
using RobotNet10.Shared.Geometry;
|
|
|
|
|
|
namespace RobotNet10.RobotApp.Navigation;
|
|
|
|
/// <summary>
|
|
/// Integration service to run the navigation program using the Navigation C API
|
|
/// Integrates with XlocIntegrationService for pose updates and provides navigation control
|
|
/// </summary>
|
|
public class NavigationIntegrationService : IHostedService, IDisposable
|
|
{
|
|
private readonly NavigationIntegrationConfiguration _config;
|
|
private readonly IDeviceProvider _deviceProvider;
|
|
private readonly ILogger<NavigationIntegrationService> _logger;
|
|
private readonly XlocIntegrationService? _xlocService;
|
|
private readonly OdometryService? _odometryService;
|
|
private readonly PS5ControllerService? _ps5ControllerService;
|
|
private readonly IInverseKinematics? _inverseKinematics;
|
|
private readonly object _lock = new();
|
|
private readonly SemaphoreSlim _twistDispatchSemaphore = new SemaphoreSlim(1, 1); // Ensure only one thread sends twist at a time
|
|
|
|
private NavigationClient? _navigationClient;
|
|
private Timer? _updateTimer; // Timer for periodic updates (pose, feedback, etc.)
|
|
|
|
// Cached device references
|
|
private ILidar? _lidarDevice1;
|
|
private ILidar? _lidarDevice2; // Second lidar (right side)
|
|
private ILidar? _lidarDevice3; // Third lidar (left side)
|
|
|
|
private bool _disposed = false;
|
|
private bool _isInitialized = false;
|
|
private string? _lastActiveMapName = null; // Track last active map to detect changes
|
|
private volatile bool _hasActiveGoal = false;
|
|
/// <summary>Raised when navigation reaches a terminal state (Succeeded, Preempted, Recalled, Aborted, Rejected, Lost).</summary>
|
|
public event Action<NavigationState>? OnNavigationResult;
|
|
|
|
public NavigationIntegrationService(
|
|
IConfiguration configuration,
|
|
IDeviceProvider deviceProvider,
|
|
ILogger<NavigationIntegrationService> logger,
|
|
XlocIntegrationService? xlocService = null,
|
|
OdometryService? odometryService = null,
|
|
NavigationClient? navigationClient = null,
|
|
PS5ControllerService? ps5ControllerService = null,
|
|
IInverseKinematics? inverseKinematics = null)
|
|
{
|
|
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
_xlocService = xlocService;
|
|
_odometryService = odometryService;
|
|
_navigationClient = navigationClient;
|
|
_ps5ControllerService = ps5ControllerService;
|
|
_inverseKinematics = inverseKinematics;
|
|
|
|
// Load configuration
|
|
var configSection = configuration.GetSection("Navigation:Integration");
|
|
if (!configSection.Exists())
|
|
{
|
|
_logger.LogWarning("Configuration section 'Navigation:Integration' not found. Using defaults.");
|
|
_config = new NavigationIntegrationConfiguration();
|
|
}
|
|
else
|
|
{
|
|
_config = new NavigationIntegrationConfiguration();
|
|
configSection.Bind(_config);
|
|
|
|
// Log footprint configuration for debugging
|
|
if (_config.RobotFootprint != null && _config.RobotFootprint.Length > 0)
|
|
{
|
|
_logger.LogInformation("Loaded RobotFootprint from config: {Count} points", _config.RobotFootprint.Length);
|
|
for (int i = 0; i < _config.RobotFootprint.Length; i++)
|
|
{
|
|
var point = _config.RobotFootprint[i];
|
|
_logger.LogInformation(" Point[{Index}]: X={X}, Y={Y}, Z={Z}", i, point.X, point.Y, point.Z);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("RobotFootprint not loaded from config or is empty");
|
|
}
|
|
}
|
|
|
|
ValidateConfiguration();
|
|
}
|
|
|
|
private void ValidateConfiguration()
|
|
{
|
|
if (_config.Enabled)
|
|
{
|
|
if (_config.UpdateIntervalMs <= 0 || _config.UpdateIntervalMs > 1000)
|
|
{
|
|
_logger.LogWarning("UpdateIntervalMs {Interval} is out of range [1-1000]. Using default 100 ms.", _config.UpdateIntervalMs);
|
|
_config.UpdateIntervalMs = 100;
|
|
}
|
|
|
|
if (_config.EnableNavigationIntegration && _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Navigation integration is enabled but NavigationIntegrationService is not available. Will be disabled.");
|
|
_config.EnableNavigationIntegration = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
public async Task StartAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (!_config.Enabled)
|
|
{
|
|
_logger.LogInformation("NavigationIntegrationService is disabled in configuration");
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("Starting NavigationIntegrationService (async, non-blocking)...");
|
|
|
|
try
|
|
{
|
|
// IMPORTANT: Run initialization in a BACKGROUND TASK to avoid blocking the main application startup
|
|
// This allows the main app to continue while navigation initializes asynchronously
|
|
// Navigation will retry automatically if devices aren't ready yet
|
|
_ = Task.Run(async () => await InitializeInBackgroundAsync(cancellationToken), cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error starting NavigationIntegrationService background task: {Message}", ex.Message);
|
|
// Don't throw - allow service to retry later
|
|
}
|
|
|
|
// Return immediately without waiting for initialization
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize NavigationIntegrationService in background without blocking
|
|
/// - Waits for devices to connect
|
|
/// - Retries automatically if devices not ready
|
|
/// - Navigation will NOT block the main app startup
|
|
/// </summary>
|
|
private async Task InitializeInBackgroundAsync(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
// Wait for devices to be connected (with timeout for faster feedback)
|
|
_logger.LogInformation("[NAV] Waiting for devices to be connected (background task)...");
|
|
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(30), cancellationToken);
|
|
|
|
if (!connected)
|
|
{
|
|
_logger.LogWarning("[NAV] Timeout waiting for devices. NavigationIntegrationService will retry initialization in background.");
|
|
_ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("[NAV] Devices connected. Initializing NavigationIntegrationService...");
|
|
await InitializeAsync(cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "[NAV] Error in background initialization: {Message}", ex.Message);
|
|
// Retry automatically
|
|
_ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
|
|
}
|
|
}
|
|
|
|
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("[NAV] Devices are now connected. Initializing NavigationIntegrationService...");
|
|
await InitializeAsync(cancellationToken);
|
|
return;
|
|
}
|
|
|
|
retryCount++;
|
|
if (retryCount % 12 == 0) // Log every minute
|
|
{
|
|
_logger.LogInformation("[NAV] Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
|
|
retryCount, maxRetries);
|
|
}
|
|
}
|
|
|
|
if (retryCount >= maxRetries)
|
|
{
|
|
_logger.LogWarning("[NAV] Timeout waiting for devices. NavigationIntegrationService will not be initialized.");
|
|
}
|
|
}
|
|
|
|
private async Task InitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
bool shouldInitialize = false;
|
|
NavigationClient? navigationClient = null;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (_isInitialized)
|
|
return;
|
|
|
|
shouldInitialize = true;
|
|
// Verify xloc service and client are available before creating navigation client
|
|
if (_xlocService?.XlocClient == null)
|
|
{
|
|
throw new InvalidOperationException("XlocService or XlocClient is not available. XlocClient must be initialized before NavigationClient.");
|
|
}
|
|
|
|
// Get TF buffer from XlocClient (which received it from TF3BufferManager)
|
|
IntPtr tfBuffer = _xlocService.XlocClient.TfBuffer;
|
|
if (tfBuffer == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("TF buffer is not initialized. Ensure TF3BufferManager is initialized first.");
|
|
}
|
|
|
|
// Create navigation client with TF buffer
|
|
navigationClient = new NavigationClient(tfBuffer, _logger);
|
|
_navigationClient = navigationClient;
|
|
}
|
|
|
|
if (!shouldInitialize)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Initialize navigation client (outside lock to avoid deadlock)
|
|
navigationClient.Initialize();
|
|
_logger.LogInformation("NavigationClient initialized successfully");
|
|
|
|
// Set robot footprint if configured
|
|
if (_config.RobotFootprint != null && _config.RobotFootprint.Length > 0)
|
|
{
|
|
var footprint = new Point[_config.RobotFootprint.Length];
|
|
_logger.LogInformation("Setting robot footprint with {Count} points. Config points:", footprint.Length);
|
|
for (int i = 0; i < _config.RobotFootprint.Length; i++)
|
|
{
|
|
var point = _config.RobotFootprint[i];
|
|
_logger.LogInformation(" Config Point[{Index}]: X={X}, Y={Y}, Z={Z}", i, point.X, point.Y, point.Z);
|
|
footprint[i] = new Point
|
|
{
|
|
x = point.X,
|
|
y = point.Y,
|
|
z = point.Z
|
|
};
|
|
_logger.LogInformation(" Footprint Point[{Index}]: x={X}, y={Y}, z={Z}", i, footprint[i].x, footprint[i].y, footprint[i].z);
|
|
}
|
|
|
|
if (navigationClient.SetRobotFootprint(footprint))
|
|
{
|
|
_logger.LogInformation("Robot footprint set successfully ({Count} points)", footprint.Length);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to set robot footprint");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Robot footprint not configured or empty. Navigation will use default footprint");
|
|
}
|
|
|
|
// Wait for navigation to be ready before adding data (outside lock to allow await)
|
|
_logger.LogInformation("Waiting for navigation to be ready...");
|
|
int readyCheckCount = 0;
|
|
const int maxReadyChecks = 50; // Wait up to 5 seconds (50 * 100ms)
|
|
bool navigationReady = false;
|
|
|
|
while (readyCheckCount < maxReadyChecks && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
var feedback = navigationClient.GetFeedback();
|
|
if (feedback != null && feedback.IsReady)
|
|
{
|
|
navigationReady = true;
|
|
_logger.LogInformation("Navigation is ready (is_ready = true)");
|
|
break;
|
|
}
|
|
await Task.Delay(100, cancellationToken);
|
|
readyCheckCount++;
|
|
}
|
|
|
|
if (!navigationReady)
|
|
{
|
|
_logger.LogWarning("Navigation not ready after {Count} checks. Will continue but data dispatch may fail.", maxReadyChecks);
|
|
}
|
|
|
|
// Add static map from Xloc if available (only if navigation is ready)
|
|
// if (_config.EnableStaticMap && _xlocService != null)
|
|
// {
|
|
// try
|
|
// {
|
|
// var mapData = _xlocService.GetStaticGridMap();
|
|
// if (mapData != null)
|
|
// {
|
|
// if (navigationClient.DispatchStaticMap(mapData, "map"))
|
|
// {
|
|
// _logger.LogInformation("Static map added successfully from Xloc");
|
|
// }
|
|
// else
|
|
// {
|
|
// _logger.LogWarning("Failed to add static map to navigation");
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// _logger.LogWarning("Static map requested but not available from Xloc");
|
|
// }
|
|
// }
|
|
// catch (Exception ex)
|
|
// {
|
|
// _logger.LogError(ex, "Error adding static map from Xloc");
|
|
// }
|
|
// }
|
|
// else if (_config.EnableStaticMap && !navigationReady)
|
|
// {
|
|
// _logger.LogWarning("Static map requested but navigation is not ready yet");
|
|
// }
|
|
|
|
// Get device references for dispatch loops
|
|
ILidar? lidarDevice1 = null;
|
|
if (_config.EnableLaserScan1 && !string.IsNullOrEmpty(_config.Lidar1DeviceId))
|
|
{
|
|
var device = _deviceProvider.GetDevice(_config.Lidar1DeviceId);
|
|
if (device is ILidar lidar && device.IsConnected)
|
|
{
|
|
lidarDevice1 = lidar;
|
|
_logger.LogInformation("LIDAR device '{DeviceId}' found and connected", _config.Lidar1DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. Laser scan dispatch will be disabled.",
|
|
_config.Lidar1DeviceId);
|
|
}
|
|
}
|
|
|
|
// Second lidar (right side)
|
|
ILidar? lidarDevice2 = null;
|
|
if (_config.EnableLaserScan2 && !string.IsNullOrEmpty(_config.Lidar2DeviceId))
|
|
{
|
|
var device2 = _deviceProvider.GetDevice(_config.Lidar2DeviceId);
|
|
if (device2 is ILidar lidar2 && device2.IsConnected)
|
|
{
|
|
lidarDevice2 = lidar2;
|
|
_logger.LogInformation("LIDAR 2 device '{DeviceId}' found and connected", _config.Lidar2DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR 2 device '{DeviceId}' not found or not connected. Laser scan 2 dispatch will be disabled.",
|
|
_config.Lidar2DeviceId);
|
|
}
|
|
}
|
|
|
|
// Third lidar (left side)
|
|
ILidar? lidarDevice3 = null;
|
|
if (_config.EnableLaserScan3 && !string.IsNullOrEmpty(_config.Lidar3DeviceId))
|
|
{
|
|
var device3 = _deviceProvider.GetDevice(_config.Lidar3DeviceId);
|
|
if (device3 is ILidar lidar3 && device3.IsConnected)
|
|
{
|
|
lidarDevice3 = lidar3;
|
|
_logger.LogInformation("LIDAR 3 device '{DeviceId}' found and connected", _config.Lidar3DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR 3 device '{DeviceId}' not found or not connected. Laser scan 3 dispatch will be disabled.",
|
|
_config.Lidar3DeviceId);
|
|
}
|
|
}
|
|
|
|
// Start dispatch loops as background tasks (similar to Xloc)
|
|
if (_config.EnableOdometryDispatch && _odometryService != null)
|
|
{
|
|
_ = Task.Run(async () => await OdometryDispatchLoopAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("Odometry dispatch loop started at {Rate} Hz", 1000.0 / _config.OdomDispatchIntervalMs);
|
|
}
|
|
|
|
if (_config.EnableLaserScan1 && lidarDevice1 != null)
|
|
{
|
|
_ = Task.Run(async () => await LaserScan1DispatchLoopAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("Laser scan 1 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
|
|
}
|
|
|
|
if (_config.EnableLaserScan2 && lidarDevice2 != null)
|
|
{
|
|
_ = Task.Run(async () => await LaserScan2DispatchLoopAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("Laser scan 2 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
|
|
}
|
|
|
|
if (_config.EnableLaserScan3 && lidarDevice3 != null)
|
|
{
|
|
_ = Task.Run(async () => await LaserScan3DispatchLoopAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("Laser scan 3 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
|
|
}
|
|
|
|
// Dispatch grid map once during initialization if enabled
|
|
if (_config.EnableGridMapDispatch && _xlocService != null)
|
|
{
|
|
_ = Task.Run(async () => await DispatchGridMapOnceAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("GridMap dispatch scheduled for initialization");
|
|
}
|
|
|
|
// if (_config.EnableTwistLinear && _odometryService != null)
|
|
// {
|
|
// _ = Task.Run(async () => await TwistLinearDispatchLoopAsync(cancellationToken), cancellationToken);
|
|
// _logger.LogInformation("Twist linear dispatch loop started at {Rate} Hz", 1000.0 / _config.TwistDispatchIntervalMs);
|
|
// }
|
|
|
|
if (_config.EnableNavigationTwistDispatch && _inverseKinematics != null)
|
|
{
|
|
_ = Task.Run(async () => await NavigationTwistDispatchLoopAsync(cancellationToken), cancellationToken);
|
|
_logger.LogInformation("Navigation twist dispatch loop started at {Rate} Hz", 1000.0 / _config.NavigationTwistDispatchIntervalMs);
|
|
}
|
|
|
|
// Set initialized flag and assign device references FIRST (inside lock for thread safety)
|
|
// This ensures _isInitialized is true before timer starts
|
|
lock (_lock)
|
|
{
|
|
_lidarDevice1 = lidarDevice1;
|
|
_lidarDevice2 = lidarDevice2;
|
|
_lidarDevice3 = lidarDevice3;
|
|
_isInitialized = true;
|
|
}
|
|
|
|
// Start update timer for periodic pose/feedback updates
|
|
// Create timer AFTER _isInitialized is set to prevent race condition
|
|
Timer? updateTimer = new Timer(UpdateNavigationState, null,
|
|
TimeSpan.FromMilliseconds(_config.UpdateIntervalMs),
|
|
TimeSpan.FromMilliseconds(_config.UpdateIntervalMs));
|
|
_logger.LogInformation("Navigation update timer started (interval: {Interval} ms)", _config.UpdateIntervalMs);
|
|
|
|
// Assign timer reference (inside lock for thread safety)
|
|
lock (_lock)
|
|
{
|
|
_updateTimer = updateTimer;
|
|
}
|
|
|
|
_logger.LogInformation("✅ NavigationIntegrationService initialized successfully");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Failed to initialize NavigationIntegrationService");
|
|
lock (_lock)
|
|
{
|
|
_isInitialized = false;
|
|
_navigationClient = null;
|
|
}
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Periodic update callback - updates navigation state, pose, and feedback
|
|
/// </summary>
|
|
private void UpdateNavigationState(object? state)
|
|
{
|
|
NavigationClient? client = null;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return;
|
|
|
|
client = _navigationClient;
|
|
}
|
|
|
|
if (client == null)
|
|
return;
|
|
|
|
// Check if NavigationClient is fully initialized before using it
|
|
// This prevents calling methods when _handle is still IntPtr.Zero
|
|
if (!client.IsInitialized)
|
|
{
|
|
_logger.LogTrace("NavigationClient not fully initialized yet, skipping update");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
// Publish map→odom from Xloc (map→base) and odometry (odom→base), not from navigation pose (circular).
|
|
if (_config.EnableNavigationIntegration && client != null
|
|
&& _xlocService != null && _odometryService != null)
|
|
{
|
|
var mapPose = _xlocService.GetCurrentPose2D();
|
|
var odom = _odometryService.CurrentOdometry;
|
|
if (mapPose.HasValue && !string.IsNullOrEmpty(odom.Header.FrameId))
|
|
{
|
|
var oq = odom.Pose.Pose.Orientation;
|
|
double odomYaw = Math.Atan2(
|
|
2.0 * (oq.W * oq.Z + oq.X * oq.Y),
|
|
1.0 - 2.0 * (oq.Y * oq.Y + oq.Z * oq.Z));
|
|
double odomX = odom.Pose.Pose.Position.X;
|
|
double odomY = odom.Pose.Pose.Position.Y;
|
|
|
|
double yawMapOdom = mapPose.Value.yaw - odomYaw;
|
|
double cosOb = Math.Cos(odomYaw);
|
|
double sinOb = Math.Sin(odomYaw);
|
|
double invX = -(cosOb * odomX + sinOb * odomY);
|
|
double invY = -(-sinOb * odomX + cosOb * odomY);
|
|
double cosMb = Math.Cos(mapPose.Value.yaw);
|
|
double sinMb = Math.Sin(mapPose.Value.yaw);
|
|
double mapOdomX = mapPose.Value.x + (cosMb * invX - sinMb * invY);
|
|
double mapOdomY = mapPose.Value.y + (sinMb * invX + cosMb * invY);
|
|
|
|
double halfYaw = yawMapOdom * 0.5;
|
|
client.UpdateMapToOdomTransform(
|
|
mapOdomX, mapOdomY, 0.0,
|
|
0.0, 0.0, Math.Sin(halfYaw), Math.Cos(halfYaw));
|
|
|
|
_logger.LogTrace(
|
|
"Updated map→odom from Xloc+odom: ({X:F2}, {Y:F2}, yaw={Yaw:F2}°)",
|
|
mapOdomX, mapOdomY, yawMapOdom * 180.0 / Math.PI);
|
|
}
|
|
}
|
|
|
|
// Get and log navigation feedback periodically
|
|
var feedback = client.GetFeedback();
|
|
if (feedback != null)
|
|
{
|
|
_logger.LogTrace("Navigation state: {State}, Ready: {Ready}, GoalChecked: {GoalChecked}",
|
|
feedback.StateString, feedback.IsReady, feedback.GoalChecked);
|
|
// Notify when goal completes so order controller can clear state and accept new orders
|
|
if (_hasActiveGoal && IsTerminalState(feedback.NavigationState))
|
|
{
|
|
_hasActiveGoal = false;
|
|
try { OnNavigationResult?.Invoke(feedback.NavigationState); }
|
|
catch (Exception ex) { _logger.LogError(ex, "OnNavigationResult subscriber error"); }
|
|
}
|
|
}
|
|
|
|
// Check for map changes and dispatch if needed
|
|
if (_config.EnableGridMapDispatch && _xlocService != null)
|
|
{
|
|
CheckAndDispatchGridMapIfChanged();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error updating navigation state");
|
|
}
|
|
}
|
|
|
|
private static bool IsTerminalState(NavigationState state)
|
|
{
|
|
return state is NavigationState.Succeeded or NavigationState.Preempted or NavigationState.Recalled
|
|
or NavigationState.Aborted or NavigationState.Rejected or NavigationState.Lost;
|
|
}
|
|
|
|
public async Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("Stopping NavigationIntegrationService...");
|
|
|
|
try
|
|
{
|
|
// IMPORTANT: Set _isInitialized to false FIRST to stop update timer
|
|
lock (_lock)
|
|
{
|
|
_isInitialized = false;
|
|
|
|
// Dispose update timer
|
|
_updateTimer?.Dispose();
|
|
_updateTimer = null;
|
|
}
|
|
|
|
// Wait a bit for update loop to notice _isInitialized = false and stop
|
|
_logger.LogInformation("Waiting for update loop to stop...");
|
|
await Task.Delay(500, cancellationToken);
|
|
|
|
// Cancel any active navigation goals
|
|
if (_navigationClient != null)
|
|
{
|
|
_navigationClient.Cancel();
|
|
_navigationClient.Dispose();
|
|
_navigationClient = null;
|
|
_logger.LogInformation("NavigationClient disposed successfully");
|
|
}
|
|
|
|
_logger.LogInformation("NavigationIntegrationService stopped successfully");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping NavigationIntegrationService");
|
|
}
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
#region Public API Methods
|
|
|
|
/// <summary>
|
|
/// Get the navigation client (for external use)
|
|
/// </summary>
|
|
public NavigationClient? GetNavigationClient()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if navigation is initialized
|
|
/// </summary>
|
|
public bool IsInitialized
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _isInitialized && _navigationClient != null && _navigationClient.IsInitialized;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to
|
|
/// </summary>
|
|
public bool MoveTo(double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
|
|
{
|
|
_logger.LogWarning("Cannot move to goal: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.MoveTo(x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order
|
|
/// </summary>
|
|
public bool MoveToOrder(IntPtr orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
|
|
{
|
|
_logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder called with order_handle: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}",
|
|
orderHandle.ToInt64(), x, y, z, frameId, xyTolerance, yawTolerance);
|
|
bool result = _navigationClient.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
|
|
if (result) _hasActiveGoal = true;
|
|
return result;
|
|
}
|
|
catch (ArgumentException ex)
|
|
{
|
|
// Log at debug level since this is expected validation error, API endpoint will return error response
|
|
_logger.LogDebug(ex, "Invalid order handle: {OrderHandle}", orderHandle.ToInt64());
|
|
throw; // Re-throw to be caught by API endpoint
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Unexpected error in MoveToOrder: {Message}", ex.Message);
|
|
throw; // Re-throw to be caught by API endpoint
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order (using OrderHandle wrapper)
|
|
/// </summary>
|
|
public bool MoveToOrder(OrderHandle orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
|
|
{
|
|
_logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder called with OrderHandle wrapper: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}",
|
|
orderHandle?.Handle.ToInt64() ?? 0, x, y, z, frameId, xyTolerance, yawTolerance);
|
|
bool result = _navigationClient.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
|
|
if (result) _hasActiveGoal = true;
|
|
return result;
|
|
}
|
|
catch (ArgumentException ex)
|
|
{
|
|
// Log at debug level since this is expected validation error, API endpoint will return error response
|
|
_logger.LogDebug(ex, "Invalid order handle: {OrderHandle}", orderHandle?.Handle.ToInt64() ?? 0);
|
|
throw; // Re-throw to be caught by API endpoint
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Unexpected error in MoveToOrder: {Message}", ex.Message);
|
|
throw; // Re-throw to be caught by API endpoint
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order (using OrderData from JSON)
|
|
/// </summary>
|
|
public bool MoveToOrder(OrderData orderData, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
|
|
{
|
|
_logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder called with OrderData: {OrderId}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}",
|
|
orderData?.OrderId ?? "null", x, y, z, frameId, xyTolerance, yawTolerance);
|
|
bool result = _navigationClient.MoveToOrder(orderData, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
_logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
|
|
if (result) _hasActiveGoal = true;
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in MoveToOrder with OrderData: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a docking goal to a predefined marker
|
|
/// </summary>
|
|
public bool DockTo(string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.05,
|
|
double yawTolerance = 0.05)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot dock to marker: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.DockTo(marker, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move straight toward the target position
|
|
/// </summary>
|
|
public bool MoveStraightTo(double distance)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot move straight: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.MoveStraightTo(distance);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotate in place to align with target orientation
|
|
/// </summary>
|
|
public bool RotateTo(double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double yawTolerance = 0.1)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot rotate: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.RotateTo(x, y, z, qx, qy, qz, qw, frameId, yawTolerance);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pause the robot's movement
|
|
/// </summary>
|
|
public void Pause()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot pause: NavigationIntegrationService not initialized");
|
|
return;
|
|
}
|
|
|
|
_navigationClient.Pause();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Resume motion after a pause
|
|
/// </summary>
|
|
public void Resume()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot resume: NavigationIntegrationService not initialized");
|
|
return;
|
|
}
|
|
|
|
_navigationClient.Resume();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cancel the current goal and stop the robot
|
|
/// </summary>
|
|
public void Cancel()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot cancel: NavigationIntegrationService not initialized");
|
|
return;
|
|
}
|
|
|
|
_navigationClient.Cancel();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send limited linear velocity command
|
|
/// </summary>
|
|
public bool SetTwistLinear(double linearX, double linearY, double linearZ)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot set linear twist: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.SetTwistLinear(linearX, linearY, linearZ);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send limited angular velocity command
|
|
/// </summary>
|
|
public bool SetTwistAngular(double angularX, double angularY, double angularZ)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot set angular twist: NavigationIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _navigationClient.SetTwistAngular(angularX, angularY, angularZ);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's current pose
|
|
/// </summary>
|
|
public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? GetRobotPose()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetRobotPoseStamped();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's current pose as 2D (x, y, theta)
|
|
/// </summary>
|
|
public (double x, double y, double theta)? GetRobotPose2D()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetRobotPose2D();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's current twist
|
|
/// </summary>
|
|
public (double x, double y, double theta, string frameId)? GetTwist()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetTwist();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get navigation feedback
|
|
/// </summary>
|
|
public NavigationFeedbackData? GetFeedback()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetFeedback();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get global planner path data from navigation_get_global_data.
|
|
/// </summary>
|
|
public GlobalPathData? GetGlobalPathData()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetGlobalPathData();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get local planner path data from navigation_get_local_data.
|
|
/// </summary>
|
|
public GlobalPathData? GetLocalPathData()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetLocalPathData();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get local planner cost map data from navigation_get_local_data.
|
|
/// </summary>
|
|
public CostMapData? GetCostMapData()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null)
|
|
return null;
|
|
|
|
return _navigationClient.GetCostMapData();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the configured robot footprint polygon (relative to base_link frame).
|
|
/// </summary>
|
|
public FootprintPoint[]? GetRobotFootprint()
|
|
{
|
|
return _config.RobotFootprint;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Dispatch Loops
|
|
/// <summary>
|
|
/// Odometry dispatch loop - runs in background task
|
|
/// </summary>
|
|
private async Task OdometryDispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("[ODOM-DISPATCH-LOOP] Starting odometry dispatch loop");
|
|
|
|
while (_config.EnableOdometryDispatch && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _odometryService == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
try
|
|
{
|
|
var odom = _odometryService.CurrentOdometry;
|
|
if (!string.IsNullOrEmpty(odom.Header.FrameId)) // Check if odometry is valid
|
|
{ if (_navigationClient != null)
|
|
{
|
|
Odometry navOdom = default;
|
|
try
|
|
{
|
|
// Convert Shared.Sensor.Odometry to NavigationInterop.Odometry
|
|
navOdom = odom.ToNavigationOdometry();
|
|
|
|
// Dispatch the struct
|
|
_navigationClient.DispatchOdometry(navOdom, "/odom");
|
|
}
|
|
finally
|
|
{
|
|
|
|
// Free allocated memory after dispatch (even if exception occurs)
|
|
NavigationConversionExtensions.FreeNavigationOdometry(ref navOdom);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "[ODOM-DISPATCH-LOOP] Error in odometry dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.OdomDispatchIntervalMs, cancellationToken);
|
|
}
|
|
|
|
_logger.LogInformation("[ODOM-DISPATCH-LOOP] Odometry dispatch loop ended");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Laser scan dispatch loop - runs in background task
|
|
/// </summary>
|
|
private async Task LaserScan1DispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (_config.EnableLaserScan1 && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _lidarDevice1 == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
try
|
|
{
|
|
var scan = _lidarDevice1.CurrentMeasurementData;
|
|
if (scan.HasValue)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
LaserScan updateLaserScan = default;
|
|
try
|
|
{
|
|
var originalScan = scan.Value;
|
|
|
|
// Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
|
|
var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
|
|
Header scanHeader = new Header
|
|
{
|
|
seq = originalScan.Header.Seq,
|
|
sec = (uint)timeSpan.TotalSeconds,
|
|
nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
|
|
frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "fscan")
|
|
};
|
|
|
|
// Allocate and copy ranges array
|
|
IntPtr rangesPtr = IntPtr.Zero;
|
|
nuint rangesCount = 0;
|
|
if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
|
|
{
|
|
float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
|
|
int rangesSize = rangesFloat.Length * sizeof(float);
|
|
rangesPtr = Marshal.AllocHGlobal(rangesSize);
|
|
Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
|
|
rangesCount = (nuint)rangesFloat.Length;
|
|
}
|
|
|
|
// Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
|
|
IntPtr intensitiesPtr = IntPtr.Zero;
|
|
nuint intensitiesCount = 0;
|
|
if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
|
|
originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
|
|
{
|
|
float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
|
|
int intensitiesSize = intensitiesFloat.Length * sizeof(float);
|
|
intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
|
|
Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
|
|
intensitiesCount = (nuint)intensitiesFloat.Length;
|
|
}
|
|
|
|
updateLaserScan = new LaserScan
|
|
{
|
|
header = scanHeader,
|
|
angle_min = (float)-2.356194496154785,
|
|
angle_max = (float)2.3557233810424805,
|
|
angle_increment = (float)0.00581718236207962,
|
|
time_increment = (float)6.172839493956417e-05,
|
|
scan_time = (float)0.06666667014360428,
|
|
range_min = (float)0.0,
|
|
range_max = (float)100.0,
|
|
ranges = rangesPtr,
|
|
ranges_count = rangesCount,
|
|
intensities = intensitiesPtr,
|
|
intensities_count = intensitiesCount
|
|
};
|
|
// _logger.LogInformation("Start dispatching laser scan 1");
|
|
_navigationClient.DispatchLaserScan(updateLaserScan,"/fscan");
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory after dispatch (even if exception occurs)
|
|
NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
|
{
|
|
_logger.LogDebug("Laser scan dispatch loop stopped: {Message}", ex.Message);
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in laser scan dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Laser scan 2 dispatch loop - runs in background task
|
|
/// </summary>
|
|
private async Task LaserScan2DispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (_config.EnableLaserScan2 && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _lidarDevice2 == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
|
|
try
|
|
{
|
|
var scan = _lidarDevice2.CurrentMeasurementData;
|
|
if (scan.HasValue)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_navigationClient != null)
|
|
{
|
|
LaserScan updateLaserScan = default;
|
|
try
|
|
{
|
|
var originalScan = scan.Value;
|
|
|
|
// Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
|
|
var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
|
|
Header scanHeader = new Header
|
|
{
|
|
seq = originalScan.Header.Seq,
|
|
sec = (uint)timeSpan.TotalSeconds,
|
|
nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
|
|
frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "b_scan")
|
|
};
|
|
|
|
// Allocate and copy ranges array
|
|
IntPtr rangesPtr = IntPtr.Zero;
|
|
nuint rangesCount = 0;
|
|
if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
|
|
{
|
|
float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
|
|
int rangesSize = rangesFloat.Length * sizeof(float);
|
|
rangesPtr = Marshal.AllocHGlobal(rangesSize);
|
|
Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
|
|
rangesCount = (nuint)rangesFloat.Length;
|
|
}
|
|
|
|
// Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
|
|
IntPtr intensitiesPtr = IntPtr.Zero;
|
|
nuint intensitiesCount = 0;
|
|
if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
|
|
originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
|
|
{
|
|
float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
|
|
int intensitiesSize = intensitiesFloat.Length * sizeof(float);
|
|
intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
|
|
Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
|
|
intensitiesCount = (nuint)intensitiesFloat.Length;
|
|
}
|
|
|
|
updateLaserScan = new LaserScan
|
|
{
|
|
header = scanHeader,
|
|
angle_min = (float)-1.0707963705062866,
|
|
angle_max = (float)1.0707963705062866,
|
|
angle_increment = (float)0.00581718236207962,
|
|
time_increment = (float)6.172839493956417e-05,
|
|
scan_time = (float)0.06666667014360428,
|
|
range_min = (float)0.0,
|
|
range_max = (float)100.0,
|
|
ranges = rangesPtr,
|
|
ranges_count = rangesCount,
|
|
intensities = intensitiesPtr,
|
|
intensities_count = intensitiesCount
|
|
};
|
|
_navigationClient.DispatchLaserScan(updateLaserScan, "/b_scan");
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory after dispatch (even if exception occurs)
|
|
NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
|
{
|
|
_logger.LogDebug("Laser scan 2 dispatch loop stopped: {Message}", ex.Message);
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in laser scan 2 dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Laser scan 3 dispatch loop - runs in background task
|
|
/// </summary>
|
|
private async Task LaserScan3DispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (_config.EnableLaserScan3 && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _lidarDevice3 == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
try
|
|
{
|
|
var scan = _lidarDevice3.CurrentMeasurementData;
|
|
if (scan.HasValue)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_navigationClient != null)
|
|
{
|
|
LaserScan updateLaserScan = default;
|
|
try
|
|
{
|
|
var originalScan = scan.Value;
|
|
|
|
// Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
|
|
var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
|
|
Header scanHeader = new Header
|
|
{
|
|
seq = originalScan.Header.Seq,
|
|
sec = (uint)timeSpan.TotalSeconds,
|
|
nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
|
|
frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "f_scan")
|
|
};
|
|
|
|
// Allocate and copy ranges array
|
|
IntPtr rangesPtr = IntPtr.Zero;
|
|
nuint rangesCount = 0;
|
|
if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
|
|
{
|
|
float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
|
|
int rangesSize = rangesFloat.Length * sizeof(float);
|
|
rangesPtr = Marshal.AllocHGlobal(rangesSize);
|
|
Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
|
|
rangesCount = (nuint)rangesFloat.Length;
|
|
}
|
|
|
|
// Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
|
|
IntPtr intensitiesPtr = IntPtr.Zero;
|
|
nuint intensitiesCount = 0;
|
|
if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
|
|
originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
|
|
{
|
|
float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
|
|
int intensitiesSize = intensitiesFloat.Length * sizeof(float);
|
|
intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
|
|
Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
|
|
intensitiesCount = (nuint)intensitiesFloat.Length;
|
|
}
|
|
|
|
updateLaserScan = new LaserScan
|
|
{
|
|
header = scanHeader,
|
|
angle_min = (float)-1.0707963705062866,
|
|
angle_max = (float)1.0707963705062866,
|
|
angle_increment = (float)0.00581718236207962,
|
|
time_increment = (float)6.172839493956417e-05,
|
|
scan_time = (float)0.06666667014360428,
|
|
range_min = (float)0.0,
|
|
range_max = (float)100.0,
|
|
ranges = rangesPtr,
|
|
ranges_count = rangesCount,
|
|
intensities = intensitiesPtr,
|
|
intensities_count = intensitiesCount
|
|
};
|
|
_navigationClient.DispatchLaserScan(updateLaserScan, "/f_scan");
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory after dispatch (even if exception occurs)
|
|
NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
|
{
|
|
_logger.LogDebug("Laser scan 3 dispatch loop stopped: {Message}", ex.Message);
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in laser scan 3 dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch grid map once during initialization
|
|
/// </summary>
|
|
private async Task DispatchGridMapOnceAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Wait for navigation to be ready
|
|
int readyCheckCount = 0;
|
|
const int maxReadyChecks = 50; // Wait up to 5 seconds (50 * 100ms)
|
|
|
|
while (readyCheckCount < maxReadyChecks && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
readyCheckCount++;
|
|
continue;
|
|
}
|
|
|
|
// Try to dispatch the map
|
|
if (TryDispatchGridMap())
|
|
{
|
|
_logger.LogInformation("[GRIDMAP-DISPATCH] Grid map dispatched successfully during initialization");
|
|
return;
|
|
}
|
|
|
|
await Task.Delay(100, cancellationToken);
|
|
readyCheckCount++;
|
|
}
|
|
|
|
if (readyCheckCount >= maxReadyChecks)
|
|
{
|
|
_logger.LogWarning("[GRIDMAP-DISPATCH] Failed to dispatch grid map during initialization after {Count} attempts", maxReadyChecks);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if map has changed and dispatch if needed
|
|
/// Only handles static map from xloc
|
|
/// </summary>
|
|
private void CheckAndDispatchGridMapIfChanged()
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var diagnostics = _xlocService.GetDiagnostics();
|
|
if (diagnostics == null)
|
|
return;
|
|
|
|
// Check if active static map changed
|
|
string currentMap = diagnostics.CurrentActiveMap ?? string.Empty;
|
|
if (currentMap != _lastActiveMapName)
|
|
{
|
|
_logger.LogInformation("[GRIDMAP-DISPATCH] Active map changed from '{OldMap}' to '{NewMap}', dispatching new map",
|
|
_lastActiveMapName ?? "(none)", currentMap);
|
|
_lastActiveMapName = currentMap;
|
|
|
|
if (!string.IsNullOrEmpty(currentMap))
|
|
{
|
|
TryDispatchGridMap();
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "[GRIDMAP-DISPATCH] Error checking for map changes");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Try to dispatch static grid map - returns true if successful
|
|
/// Only handles static map from xloc
|
|
/// </summary>
|
|
private bool TryDispatchGridMap()
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
|
|
return false;
|
|
|
|
try
|
|
{
|
|
// Check xloc diagnostics to ensure it's ready
|
|
var diagnostics = _xlocService.GetDiagnostics();
|
|
if (diagnostics == null)
|
|
return false;
|
|
|
|
// Check if there's an active static map
|
|
if (string.IsNullOrEmpty(diagnostics.CurrentActiveMap))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Get static gridmap data from xloc
|
|
OccupancyGridData? gridData = _xlocService.GetStaticGridMap(reloadFromFile: true);
|
|
|
|
if (gridData == null)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Validate grid data before processing
|
|
if (gridData.Width == 0 || gridData.Height == 0)
|
|
{
|
|
_logger.LogWarning("[GRIDMAP-DISPATCH] Invalid grid dimensions: width={Width}, height={Height}. Skipping dispatch.",
|
|
gridData.Width, gridData.Height);
|
|
return false;
|
|
}
|
|
|
|
// Additional validation: check if resolution is reasonable
|
|
if (gridData.Resolution <= 0 || gridData.Resolution > 1.0)
|
|
{
|
|
_logger.LogWarning("[GRIDMAP-DISPATCH] Invalid resolution: {Resolution}. Skipping dispatch.", gridData.Resolution);
|
|
return false;
|
|
}
|
|
|
|
// Convert OccupancyGridData to xloc_occupancy_grid_t
|
|
xloc_occupancy_grid_t xlocGrid = new xloc_occupancy_grid_t
|
|
{
|
|
header = new xloc_header_t
|
|
{
|
|
seq = 0,
|
|
stamp = new xloc_unix_time_t { sec = 0, nsec = 0 },
|
|
frame_id = Marshal.StringToHGlobalAnsi(gridData.FrameId ?? "map")
|
|
},
|
|
resolution = gridData.Resolution,
|
|
width = gridData.Width,
|
|
height = gridData.Height,
|
|
origin = new xloc_pose_t
|
|
{
|
|
position = new double[] { gridData.Origin.X, gridData.Origin.Y, gridData.Origin.Z },
|
|
orientation = new double[] { gridData.Origin.Qx, gridData.Origin.Qy, gridData.Origin.Qz, gridData.Origin.Qw }
|
|
},
|
|
data = IntPtr.Zero,
|
|
data_length = (nuint)(gridData.Data?.Length ?? 0)
|
|
};
|
|
|
|
// Allocate and copy data
|
|
IntPtr dataPtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
if (gridData.Data != null && gridData.Data.Length > 0)
|
|
{
|
|
dataPtr = Marshal.AllocHGlobal(gridData.Data.Length);
|
|
Marshal.Copy(gridData.Data, 0, dataPtr, gridData.Data.Length);
|
|
xlocGrid.data = dataPtr;
|
|
}
|
|
|
|
// Convert to Navigation OccupancyGrid
|
|
OccupancyGrid navGrid = default;
|
|
try
|
|
{
|
|
navGrid = xlocGrid.ToNavigationOccupancyGrid();
|
|
|
|
_logger.LogInformation("[GRIDMAP-DISPATCH] Dispatching grid map: width={Width}, height={Height}, resolution={Resolution}, data_count={DataCount}",
|
|
navGrid.info.width, navGrid.info.height, navGrid.info.resolution,
|
|
navGrid.data_count.ToUInt64());
|
|
|
|
// Dispatch the struct
|
|
lock (_lock)
|
|
{
|
|
if (_navigationClient != null)
|
|
{
|
|
_navigationClient.DispatchGridMap(navGrid, "/map");
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory after dispatch
|
|
NavigationConversionExtensions.FreeNavigationOccupancyGrid(ref navGrid);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
// Free xloc grid data pointer
|
|
if (dataPtr != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(dataPtr);
|
|
}
|
|
// Free frame_id string
|
|
if (xlocGrid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(xlocGrid.header.frame_id);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "[GRIDMAP-DISPATCH] Error dispatching grid map");
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Twist linear dispatch loop - runs in background task
|
|
/// </summary>
|
|
private async Task TwistLinearDispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (_config.EnableTwistLinear && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _odometryService == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
try
|
|
{
|
|
var odom = _odometryService.CurrentOdometry;
|
|
if (odom.Header.FrameId != null)
|
|
{
|
|
// Extract linear velocities from odometry twist
|
|
// TwistWithCovariance -> Twist -> Linear -> Vector3 (X, Y, Z)
|
|
double linearX = odom.Twist.Twist.Linear.X;
|
|
double linearY = odom.Twist.Twist.Linear.Y;
|
|
double linearZ = odom.Twist.Twist.Linear.Z;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (_navigationClient != null)
|
|
{
|
|
// Console.WriteLine($"Dispatching twist linear: x={linearX:F2}, y={linearY:F2}, z={linearZ:F2}");
|
|
_navigationClient.SetTwistLinear(linearX, linearY, linearZ);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
|
{
|
|
_logger.LogDebug("Twist linear dispatch loop stopped: {Message}", ex.Message);
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in twist linear dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.TwistDispatchIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Navigation twist dispatch loop - reads twist from navigation and sends to DifferentialDrive
|
|
/// Only sends when PS5 controller is disabled to ensure only one source controls the robot
|
|
/// </summary>
|
|
private async Task NavigationTwistDispatchLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (_config.EnableNavigationTwistDispatch && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (!_isInitialized || _navigationClient == null || _inverseKinematics == null || !_navigationClient.IsInitialized)
|
|
{
|
|
await Task.Delay(100, cancellationToken);
|
|
continue;
|
|
}
|
|
// Check if PS5 controller is disabled - only send navigation twist when PS5 is disabled
|
|
if (_ps5ControllerService != null && _ps5ControllerService.State == PS5ControllerState.Active)
|
|
{
|
|
// PS5 is active, skip navigation twist to avoid conflict
|
|
await Task.Delay(_config.NavigationTwistDispatchIntervalMs, cancellationToken);
|
|
continue;
|
|
}
|
|
|
|
try
|
|
{
|
|
// Read twist from navigation
|
|
// Console.WriteLine("Checking for navigation twist...");
|
|
var twistResult = _navigationClient.GetTwist();
|
|
if (twistResult.HasValue)
|
|
{
|
|
var (x, y, theta, frameId) = twistResult.Value;
|
|
// Console.WriteLine($"Got navigation twist: x={x:F2}, y={y:F2}, theta={theta:F2}, frameId={frameId}");
|
|
// Convert Twist2D to Twist (Shared.Geometry.Twist)
|
|
var twist = new RobotNet10.Shared.Geometry.Twist
|
|
{
|
|
Linear = new RobotNet10.Shared.Geometry.Vector3(x, 0.0, 0.0),
|
|
Angular = new RobotNet10.Shared.Geometry.Vector3(0.0, 0.0, theta)
|
|
};
|
|
|
|
// Send to DifferentialDrive - use semaphore to ensure only one thread sends at a time
|
|
// Check PS5 state before acquiring semaphore to avoid unnecessary blocking
|
|
if (_ps5ControllerService != null && _ps5ControllerService.State == PS5ControllerState.Active)
|
|
{
|
|
// PS5 became active, skip this iteration
|
|
continue;
|
|
}
|
|
|
|
// Acquire semaphore to ensure only one thread sends twist at a time
|
|
if (await _twistDispatchSemaphore.WaitAsync(0, cancellationToken))
|
|
{
|
|
try
|
|
{
|
|
// Double-check PS5 is still disabled and inverse kinematics is available
|
|
if (_inverseKinematics != null)
|
|
{
|
|
// Console.WriteLine("Sending navigation twist to DifferentialDrive...");
|
|
_inverseKinematics.Enable();
|
|
await _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error sending navigation twist to DifferentialDrive");
|
|
}
|
|
finally
|
|
{
|
|
_twistDispatchSemaphore.Release();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
|
|
{
|
|
_logger.LogDebug("Navigation twist dispatch loop stopped: {Message}", ex.Message);
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in navigation twist dispatch loop");
|
|
}
|
|
|
|
await Task.Delay(_config.NavigationTwistDispatchIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
public void Dispose()
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
_disposed = true;
|
|
|
|
try
|
|
{
|
|
// Stop update loop first
|
|
lock (_lock)
|
|
{
|
|
_isInitialized = false;
|
|
}
|
|
|
|
// Give loop time to stop
|
|
Thread.Sleep(500);
|
|
|
|
_updateTimer?.Dispose();
|
|
_navigationClient?.Dispose();
|
|
_twistDispatchSemaphore?.Dispose();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error disposing NavigationIntegrationService");
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configuration for NavigationIntegrationService
|
|
/// </summary>
|
|
public class NavigationIntegrationConfiguration
|
|
{
|
|
/// <summary>
|
|
/// Enable/disable the integration service
|
|
/// </summary>
|
|
public bool Enabled { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// Update interval in milliseconds for periodic state updates (default: 100ms = 10Hz)
|
|
/// </summary>
|
|
public int UpdateIntervalMs { get; set; } = 100;
|
|
|
|
/// <summary>
|
|
/// Enable integration with NavigationClient for pose updates
|
|
/// </summary>
|
|
public bool EnableNavigationIntegration { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Robot footprint points (polygon outline)
|
|
/// If null or empty, default footprint will be used
|
|
/// </summary>
|
|
public FootprintPoint[]? RobotFootprint { get; set; } = null;
|
|
|
|
/// <summary>
|
|
/// Enable adding static map from Xloc during initialization
|
|
/// </summary>
|
|
public bool EnableStaticMap { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Enable odometry dispatch loop
|
|
/// </summary>
|
|
public bool EnableOdometryDispatch { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Odometry dispatch interval in milliseconds (default: 20ms = 50Hz)
|
|
/// </summary>
|
|
public int OdomDispatchIntervalMs { get; set; } = 20;
|
|
|
|
/// <summary>
|
|
/// Enable laser scan dispatch loop
|
|
/// </summary>
|
|
public bool EnableLaserScan1 { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Enable static grid map dispatch from xloc.
|
|
/// Map is dispatched once during initialization and when the active map changes.
|
|
/// </summary>
|
|
public bool EnableGridMapDispatch { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// [DEPRECATED] GridMap dispatch interval - no longer used.
|
|
/// Map is now dispatched only when needed (initialization and map changes).
|
|
/// Kept for backward compatibility.
|
|
/// </summary>
|
|
public int GridMapDispatchIntervalMs { get; set; } = 1000;
|
|
|
|
/// <summary>
|
|
/// [DEPRECATED] Navigation only uses static grid map from xloc.
|
|
/// This property is kept for backward compatibility but is not used.
|
|
/// </summary>
|
|
public bool UseOnlineGridMap { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// LIDAR device ID for laser scan dispatch (Lidar 1)
|
|
/// </summary>
|
|
public string Lidar1DeviceId { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Enable laser scan 2 dispatch loop
|
|
/// </summary>
|
|
public bool EnableLaserScan2 { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// LIDAR 2 device ID for laser scan dispatch (right side)
|
|
/// </summary>
|
|
public string Lidar2DeviceId { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Enable laser scan 3 dispatch loop
|
|
/// </summary>
|
|
public bool EnableLaserScan3 { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// LIDAR 3 device ID for laser scan dispatch (left side)
|
|
/// </summary>
|
|
public string Lidar3DeviceId { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Laser scan dispatch interval in milliseconds (default: 50ms = 20Hz)
|
|
/// </summary>
|
|
public int LaserScanDispatchIntervalMs { get; set; } = 50;
|
|
|
|
/// <summary>
|
|
/// Enable twist linear dispatch loop (from odometry)
|
|
/// </summary>
|
|
public bool EnableTwistLinear { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Twist dispatch interval in milliseconds (default: 20ms = 50Hz)
|
|
/// </summary>
|
|
public int TwistDispatchIntervalMs { get; set; } = 20;
|
|
|
|
/// <summary>
|
|
/// Enable navigation twist dispatch loop (reads twist from navigation and sends to DifferentialDrive)
|
|
/// Only sends when PS5 controller is disabled
|
|
/// </summary>
|
|
public bool EnableNavigationTwistDispatch { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Navigation twist dispatch interval in milliseconds (default: 20ms = 50Hz)
|
|
/// </summary>
|
|
public int NavigationTwistDispatchIntervalMs { get; set; } = 20;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Robot footprint point configuration
|
|
/// </summary>
|
|
public class FootprintPoint
|
|
{
|
|
public double X { get; set; }
|
|
public double Y { get; set; }
|
|
public double Z { get; set; }
|
|
}
|
|
|