1109 lines
37 KiB
C#
1109 lines
37 KiB
C#
using RobotNet10.Shared.Sensor;
|
|
using System.Runtime.InteropServices;
|
|
|
|
namespace RobotNet10.RobotApp.Xloc;
|
|
|
|
/// <summary>
|
|
/// High-level wrapper for xloc localization library
|
|
/// Provides thread-safe access to xloc functionality with automatic resource management
|
|
/// </summary>
|
|
public class XlocClient : IDisposable
|
|
{
|
|
private volatile IntPtr _handle = IntPtr.Zero;
|
|
// public IntPtr _tfBuffer = IntPtr.Zero; // TF3 buffer for xloc
|
|
private readonly object _lock = new object();
|
|
private readonly object _dispatchLock = new object();
|
|
private bool _disposed = false;
|
|
private readonly ILogger? _logger;
|
|
private IntPtr _tfBuffer = IntPtr.Zero;
|
|
|
|
/// <summary>
|
|
/// Public property to access the TF3 buffer for use in navigation and other components
|
|
/// </summary>
|
|
public IntPtr TfBuffer => _tfBuffer;
|
|
|
|
public XlocClient(IntPtr tfBuffer, ILogger? logger = null)
|
|
{
|
|
_logger = logger;
|
|
_tfBuffer = tfBuffer; // TF3 buffer for xloc
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize xloc instance
|
|
/// Note: TF3BufferManager must be initialized before calling this method
|
|
/// </summary>
|
|
public void Initialize()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_handle != IntPtr.Zero)
|
|
{
|
|
_logger?.LogWarning("XlocClient already initialized");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
// Verify TF buffer is available from TF3BufferManager
|
|
if (_tfBuffer == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("TF buffer not initialized. Ensure TF3BufferManager is initialized first.");
|
|
}
|
|
|
|
// _logger?.LogInformation("[XLOC] ═══════════════════════════════════════════");
|
|
// _logger?.LogInformation("[XLOC] Initializing XlocClient...");
|
|
// _logger?.LogInformation("[XLOC] TF buffer available: {Buffer}", _tfBuffer);
|
|
|
|
// Wait a moment to ensure TF3 transforms are fully propagated
|
|
// This is critical - the native xloc library needs time to index the transforms
|
|
_logger?.LogInformation("[XLOC] Waiting for TF3 transforms to be ready (500ms)...");
|
|
System.Threading.Thread.Sleep(500);
|
|
|
|
// _logger?.LogInformation("[XLOC] Creating xloc instance with TF buffer...");
|
|
|
|
// Create xloc instance with TF buffer (initialized and populated with static transforms)
|
|
_handle = XlocNativeInterface.xloc_create(_tfBuffer);
|
|
|
|
_logger?.LogInformation("[XLOC] xloc_create() returned handle: {Handle}", _handle);
|
|
|
|
if (_handle == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("Failed to create xloc instance. xloc_create returned null.");
|
|
}
|
|
|
|
// _logger?.LogInformation("[XLOC] ═══════════════════════════════════════════");
|
|
// _logger?.LogInformation("[XLOC] ✅ XlocClient initialized successfully");
|
|
// _logger?.LogInformation("[XLOC] Ready to serve: localization, mapping, odometry dispatch");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[XLOC] ❌ Exception in Initialize: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if xloc is initialized
|
|
/// </summary>
|
|
public bool IsInitialized
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _handle != IntPtr.Zero;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch Odometry data to xloc
|
|
/// </summary>
|
|
public void DispatchOdometry(Odometry odom, string sensorId)
|
|
{
|
|
ThrowIfNotInitializedFast();
|
|
|
|
xloc_odometry_t xlocOdom = default;
|
|
try
|
|
{
|
|
var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
|
|
_logger?.LogTrace("[{Time}] [DISPATCH-START] Odometry from {SensorId}", timestamp, sensorId);
|
|
|
|
xlocOdom = odom.ToXlocOdometry();
|
|
lock (_dispatchLock)
|
|
{
|
|
_logger?.LogTrace("[{Time}] [NATIVE-CALL] xloc_dispatch_odometry...", timestamp);
|
|
XlocNativeInterface.xloc_dispatch_odometry(_handle, sensorId, ref xlocOdom);
|
|
_logger?.LogTrace("[{Time}] [NATIVE-DONE] xloc_dispatch_odometry OK", timestamp);
|
|
}
|
|
|
|
_logger?.LogTrace("[{Time}] [DISPATCH-DONE] Odometry from {SensorId}", timestamp, sensorId);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] Odometry dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory
|
|
XlocConversionExtensions.FreeXlocOdometry(ref xlocOdom);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch IMU data to xloc
|
|
/// </summary>
|
|
public void DispatchImu(Imu imu, string sensorId)
|
|
{
|
|
ThrowIfNotInitializedFast();
|
|
|
|
xloc_imu_t xlocImu = default;
|
|
try
|
|
{
|
|
var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
|
|
_logger?.LogTrace("[{Time}] [DISPATCH-START] IMU from {SensorId}", timestamp, sensorId);
|
|
|
|
xlocImu = imu.ToXlocImu();
|
|
|
|
lock (_dispatchLock)
|
|
{
|
|
_logger?.LogTrace("[{Time}] [NATIVE-CALL] xloc_dispatch_imu...", timestamp);
|
|
XlocNativeInterface.xloc_dispatch_imu(_handle, sensorId, ref xlocImu);
|
|
_logger?.LogTrace("[{Time}] [NATIVE-DONE] xloc_dispatch_imu OK", timestamp);
|
|
}
|
|
|
|
_logger?.LogTrace("[{Time}] [DISPATCH-DONE] IMU from {SensorId}", timestamp, sensorId);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] IMU dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory
|
|
XlocConversionExtensions.FreeXlocImu(ref xlocImu);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch LaserScan data to xloc
|
|
/// </summary>
|
|
public void DispatchLaserScan(LaserScan scan, string sensorId)
|
|
{
|
|
ThrowIfNotInitializedFast();
|
|
|
|
if (string.IsNullOrWhiteSpace(sensorId))
|
|
{
|
|
throw new ArgumentException("Sensor ID must not be null or empty.", nameof(sensorId));
|
|
}
|
|
|
|
if (scan.Ranges == null || scan.Ranges.Length == 0)
|
|
{
|
|
_logger?.LogWarning("[DISPATCH-SKIP] LaserScan from {SensorId} has no ranges", sensorId);
|
|
return;
|
|
}
|
|
|
|
xloc_laserscan_t xlocScan = default;
|
|
try
|
|
{
|
|
var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
|
|
|
|
xlocScan = scan.ToXlocLaserScan();
|
|
|
|
var lockWaitStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
lock (_dispatchLock)
|
|
{
|
|
var lockWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - lockWaitStart;
|
|
var nativeStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
|
|
XlocNativeInterface.xloc_dispatch_laserscan(_handle, sensorId, ref xlocScan);
|
|
|
|
var nativeMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - nativeStart;
|
|
|
|
if (lockWaitMs > 5 || nativeMs > 10)
|
|
{
|
|
// _logger?.LogWarning(
|
|
// "[XLOC-DIAG] DispatchLaserScan {SensorId} lock_wait={LockWaitMs}ms native={NativeMs}ms",
|
|
// sensorId, lockWaitMs, nativeMs);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
// Free allocated memory
|
|
XlocConversionExtensions.FreeXlocLaserScan(ref xlocScan);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Activate a map file
|
|
/// </summary>
|
|
public bool ActivateMap(string mapFileName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_activate_map(_handle, mapFileName);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Map activated successfully: {MapFile}", mapFileName);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to activate map {MapFile}: Code={Code}, Message={Message}",
|
|
mapFileName, response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error activating map {MapFile}", mapFileName);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start localization
|
|
/// Automatically resets SLAM error state before starting to clear any previous trajectory state
|
|
/// </summary>
|
|
public bool StartLocalization()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
// Reset SLAM error state first to clear any previous trajectory state
|
|
// This is required when starting a new localization session after stopping a previous one
|
|
_logger?.LogInformation("Resetting SLAM error state before starting localization...");
|
|
ResetSlamError();
|
|
|
|
// Small delay to ensure reset is processed
|
|
System.Threading.Thread.Sleep(100);
|
|
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_start_localization(_handle);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Localization started successfully");
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to start localization: Code={Code}, Message={Message}",
|
|
response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error starting localization");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop localization
|
|
/// </summary>
|
|
public bool StopLocalization()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_stop_localization(_handle);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Localization stopped successfully");
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to stop localization: Code={Code}, Message={Message}",
|
|
response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error stopping localization");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start mapping
|
|
/// Automatically resets SLAM error state before starting to clear any previous trajectory state
|
|
/// </summary>
|
|
public bool StartMapping()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
// Reset SLAM error state first to clear any previous trajectory state
|
|
// This is required when starting a new mapping session after stopping a previous one
|
|
_logger?.LogInformation("Resetting SLAM error state before starting mapping...");
|
|
ResetSlamError();
|
|
|
|
// Small delay to ensure reset is processed
|
|
System.Threading.Thread.Sleep(100);
|
|
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_start_mapping(_handle);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Mapping started successfully");
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to start mapping: Code={Code}, Message={Message}",
|
|
response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error starting mapping");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop mapping and save to file
|
|
/// </summary>
|
|
public bool StopMapping(string mapFileName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_stop_mapping(_handle, mapFileName);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Mapping stopped and saved to {MapFile}", mapFileName);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to stop mapping: Code={Code}, Message={Message}",
|
|
response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error stopping mapping");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Switch to a different map with optional initial pose
|
|
/// </summary>
|
|
public bool SwitchMap(string mapFileName, (double x, double y, double z, double qx, double qy, double qz, double qw)? initialPose = null)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
if (initialPose.HasValue)
|
|
{
|
|
var pose = new xloc_pose_t
|
|
{
|
|
position = new double[] { initialPose.Value.x, initialPose.Value.y, initialPose.Value.z },
|
|
orientation = new double[] { initialPose.Value.qx, initialPose.Value.qy, initialPose.Value.qz, initialPose.Value.qw }
|
|
};
|
|
response = XlocNativeInterface.xloc_switch_map(_handle, mapFileName, 1, ref pose);
|
|
}
|
|
else
|
|
{
|
|
var dummyPose = new xloc_pose_t();
|
|
response = XlocNativeInterface.xloc_switch_map(_handle, mapFileName, 0, ref dummyPose);
|
|
}
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Switched to map: {MapFile}", mapFileName);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to switch map: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error switching map");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Change map origin (re-center the map coordinate system)
|
|
/// </summary>
|
|
public bool ChangeMapOrigin(double x, double y, double z, double qx, double qy, double qz, double qw)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
var newOrigin = new xloc_pose_t
|
|
{
|
|
position = new double[] { x, y, z },
|
|
orientation = new double[] { qx, qy, qz, qw }
|
|
};
|
|
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_change_map_origin(_handle, ref newOrigin);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Map origin changed to: ({X}, {Y}, {Z})", x, y, z);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to change map origin: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error changing map origin");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start updating existing map (allow SLAM to modify the loaded map)
|
|
/// </summary>
|
|
public bool StartUpdateMap()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_start_update_map(_handle);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Started updating map");
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to start map update: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error starting map update");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop updating map and optionally save the updated map
|
|
/// </summary>
|
|
public bool StopUpdateMap(bool saveUpdatedMap = true)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_stop_update_map(_handle, saveUpdatedMap ? 1 : 0);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Stopped updating map (save={Save})", saveUpdatedMap);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to stop map update: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error stopping map update");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reset SLAM error state (attempt recovery from error)
|
|
/// </summary>
|
|
public bool ResetSlamError()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_reset_slam_error(_handle);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("SLAM error reset successfully");
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to reset SLAM error: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error resetting SLAM error");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set initial pose for localization
|
|
/// </summary>
|
|
public bool SetInitialPose(double x, double y, double z, double qx, double qy, double qz, double qw)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
// Get map origin BEFORE setting initial pose
|
|
var mapBefore = GetStaticGridMap(reloadFromFile: false);
|
|
_logger?.LogWarning("=== XlocClient.SetInitialPose - BEFORE ===");
|
|
if (mapBefore != null)
|
|
{
|
|
_logger?.LogWarning("Map origin BEFORE: ({X}, {Y}, {Z})",
|
|
mapBefore.Origin.X, mapBefore.Origin.Y, mapBefore.Origin.Z);
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Map origin BEFORE: NULL (no map available)");
|
|
}
|
|
|
|
var pose = new xloc_pose_t
|
|
{
|
|
position = new double[] { x, y, z },
|
|
orientation = new double[] { qx, qy, qz, qw }
|
|
};
|
|
|
|
_logger?.LogWarning("Calling xloc_set_initial_pose: ({X}, {Y}, {Z}) Q:({Qx}, {Qy}, {Qz}, {Qw})",
|
|
x, y, z, qx, qy, qz, qw);
|
|
|
|
xloc_status_response_t response;
|
|
lock (_lock)
|
|
{
|
|
response = XlocNativeInterface.xloc_set_initial_pose(_handle, ref pose);
|
|
}
|
|
|
|
string message = XlocConversionExtensions.GetMessageAndFree(ref response);
|
|
|
|
if (response.code == 0)
|
|
{
|
|
_logger?.LogInformation("Initial pose set to: ({X}, {Y}, {Z})", x, y, z);
|
|
|
|
// Get map origin AFTER setting initial pose
|
|
System.Threading.Thread.Sleep(50); // Small delay to let XLOC process
|
|
var mapAfter = GetStaticGridMap(reloadFromFile: false);
|
|
_logger?.LogWarning("=== XlocClient.SetInitialPose - AFTER ===");
|
|
if (mapAfter != null)
|
|
{
|
|
_logger?.LogWarning("Map origin AFTER: ({X}, {Y}, {Z})",
|
|
mapAfter.Origin.X, mapAfter.Origin.Y, mapAfter.Origin.Z);
|
|
|
|
// Compare with before
|
|
if (mapBefore != null)
|
|
{
|
|
var dx = Math.Abs(mapBefore.Origin.X - mapAfter.Origin.X);
|
|
var dy = Math.Abs(mapBefore.Origin.Y - mapAfter.Origin.Y);
|
|
var dz = Math.Abs(mapBefore.Origin.Z - mapAfter.Origin.Z);
|
|
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001)
|
|
{
|
|
_logger?.LogError("⚠️ CRITICAL ERROR: Map origin changed after xloc_set_initial_pose!");
|
|
_logger?.LogError(" Before: ({X}, {Y}, {Z})", mapBefore.Origin.X, mapBefore.Origin.Y, mapBefore.Origin.Z);
|
|
_logger?.LogError(" After: ({X}, {Y}, {Z})", mapAfter.Origin.X, mapAfter.Origin.Y, mapAfter.Origin.Z);
|
|
_logger?.LogError(" Difference: ΔX={DX:F3}m, ΔY={DY:F3}m, ΔZ={DZ:F3}m", dx, dy, dz);
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("✓ Map origin unchanged after xloc_set_initial_pose ✓");
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Map origin AFTER: NULL (no map available)");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Failed to set initial pose: Code={Code}, Message={Message}", response.code, message);
|
|
return false;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error setting initial pose");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get current pose estimate from xloc
|
|
/// </summary>
|
|
public (double x, double y, double z, double qx, double qy, double qz, double qw)? GetCurrentPose()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
IntPtr posePtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
_logger?.LogTrace("[DEBUG] About to call xloc_get_current_pose()...");
|
|
lock (_lock)
|
|
{
|
|
posePtr = XlocNativeInterface.xloc_get_current_pose(_handle);
|
|
}
|
|
_logger?.LogTrace("[DEBUG] xloc_get_current_pose() returned: {PosePtr}", posePtr);
|
|
|
|
if (posePtr == IntPtr.Zero)
|
|
{
|
|
_logger?.LogWarning("xloc_get_current_pose returned null");
|
|
return null;
|
|
}
|
|
|
|
_logger?.LogTrace("[DEBUG] Marshaling pose structure...");
|
|
// Marshal the pose from unmanaged memory
|
|
var pose = Marshal.PtrToStructure<xloc_pose_t>(posePtr);
|
|
_logger?.LogTrace("[DEBUG] Pose marshaled successfully");
|
|
|
|
return (
|
|
pose.position[0], pose.position[1], pose.position[2],
|
|
pose.orientation[0], pose.orientation[1], pose.orientation[2], pose.orientation[3]
|
|
);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DEBUG] Exception in GetCurrentPose: {Message}", ex.Message);
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (posePtr != IntPtr.Zero)
|
|
XlocNativeInterface.xloc_free_pose(posePtr);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get static grid map (from loaded map file)
|
|
/// </summary>
|
|
public OccupancyGridData? GetStaticGridMap(bool reloadFromFile = false)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
IntPtr gridPtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
// _logger?.LogTrace("[DEBUG] About to call xloc_get_static_grid_map()...");
|
|
lock (_lock)
|
|
{
|
|
gridPtr = XlocNativeInterface.xloc_get_static_grid_map(_handle, reloadFromFile ? 1 : 0);
|
|
}
|
|
// _logger?.LogTrace("[DEBUG] xloc_get_static_grid_map() returned: {GridPtr}", gridPtr);
|
|
|
|
if (gridPtr == IntPtr.Zero)
|
|
{
|
|
// _logger?.LogWarning("xloc_get_static_grid_map returned null");
|
|
return null;
|
|
}
|
|
|
|
// _logger?.LogTrace("[DEBUG] Marshaling occupancy grid structure...");
|
|
var grid = Marshal.PtrToStructure<xloc_occupancy_grid_t>(gridPtr);
|
|
// _logger?.LogInformation("[XLOC] GetStaticGridMap marshaled: width={Width}, height={Height}, resolution={Resolution}, data_length={DataLength}, data_ptr={DataPtr}",
|
|
// grid.width, grid.height, grid.resolution, grid.data_length, grid.data);
|
|
|
|
// Marshal frame_id string
|
|
string frameId = string.Empty;
|
|
if (grid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(grid.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
// Copy occupancy data
|
|
byte[] data = new byte[grid.data_length];
|
|
if (grid.data != IntPtr.Zero && grid.data_length > 0)
|
|
{
|
|
Marshal.Copy(grid.data, data, 0, (int)grid.data_length);
|
|
}
|
|
|
|
var result = new OccupancyGridData
|
|
{
|
|
Resolution = grid.resolution,
|
|
Width = grid.width,
|
|
Height = grid.height,
|
|
Origin = new PoseData
|
|
{
|
|
X = grid.origin.position[0],
|
|
Y = grid.origin.position[1],
|
|
Z = grid.origin.position[2],
|
|
Qx = grid.origin.orientation[0],
|
|
Qy = grid.origin.orientation[1],
|
|
Qz = grid.origin.orientation[2],
|
|
Qw = grid.origin.orientation[3]
|
|
},
|
|
Data = data,
|
|
FrameId = frameId
|
|
};
|
|
|
|
// _logger?.LogInformation("[XLOC] GetStaticGridMap returning: width={Width}, height={Height}, resolution={Resolution}, data_length={DataLength}",
|
|
// result.Width, result.Height, result.Resolution, result.Data?.Length ?? 0);
|
|
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DEBUG] Exception in GetStaticGridMap: {Message}", ex.Message);
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (gridPtr != IntPtr.Zero)
|
|
XlocNativeInterface.xloc_free_occupancy_grid(gridPtr);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get static grid map pointer (from loaded map file)
|
|
/// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
|
|
/// </summary>
|
|
public IntPtr GetStaticGridMapPtr(bool reloadFromFile = false)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
lock (_lock)
|
|
{
|
|
return XlocNativeInterface.xloc_get_static_grid_map(_handle, reloadFromFile ? 1 : 0);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get online grid map pointer (from SLAM)
|
|
/// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
|
|
/// </summary>
|
|
public IntPtr GetOnlineGridMapPtr()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
lock (_lock)
|
|
{
|
|
return XlocNativeInterface.xloc_get_online_grid_map(_handle);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get online grid map (from SLAM)
|
|
/// </summary>
|
|
public OccupancyGridData? GetOnlineGridMap()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
IntPtr gridPtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
_logger?.LogTrace("[DEBUG] About to call xloc_get_online_grid_map()...");
|
|
lock (_lock)
|
|
{
|
|
gridPtr = XlocNativeInterface.xloc_get_online_grid_map(_handle);
|
|
}
|
|
_logger?.LogTrace("[DEBUG] xloc_get_online_grid_map() returned: {GridPtr}", gridPtr);
|
|
|
|
if (gridPtr == IntPtr.Zero)
|
|
{
|
|
_logger?.LogWarning("xloc_get_online_grid_map returned null");
|
|
return null;
|
|
}
|
|
|
|
_logger?.LogTrace("[DEBUG] Marshaling occupancy grid structure...");
|
|
var grid = Marshal.PtrToStructure<xloc_occupancy_grid_t>(gridPtr);
|
|
_logger?.LogTrace("[DEBUG] Occupancy grid marshaled successfully");
|
|
|
|
// Marshal frame_id string
|
|
string frameId = string.Empty;
|
|
if (grid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(grid.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
// Copy occupancy data
|
|
byte[] data = new byte[grid.data_length];
|
|
if (grid.data != IntPtr.Zero && grid.data_length > 0)
|
|
{
|
|
Marshal.Copy(grid.data, data, 0, (int)grid.data_length);
|
|
}
|
|
|
|
return new OccupancyGridData
|
|
{
|
|
Resolution = grid.resolution,
|
|
Width = grid.width,
|
|
Height = grid.height,
|
|
Origin = new PoseData
|
|
{
|
|
X = grid.origin.position[0],
|
|
Y = grid.origin.position[1],
|
|
Z = grid.origin.position[2],
|
|
Qx = grid.origin.orientation[0],
|
|
Qy = grid.origin.orientation[1],
|
|
Qz = grid.origin.orientation[2],
|
|
Qw = grid.origin.orientation[3]
|
|
},
|
|
Data = data,
|
|
FrameId = frameId
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DEBUG] Exception in GetOnlineGridMap: {Message}", ex.Message);
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (gridPtr != IntPtr.Zero)
|
|
XlocNativeInterface.xloc_free_occupancy_grid(gridPtr);
|
|
}
|
|
}
|
|
|
|
public XlocDiagnosticsData? GetDiagnostics()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
IntPtr diagPtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
diagPtr = XlocNativeInterface.xloc_get_diagnostics(_handle);
|
|
if (diagPtr == IntPtr.Zero)
|
|
{
|
|
_logger?.LogWarning("xloc_get_diagnostics returned null");
|
|
return null;
|
|
}
|
|
|
|
var diag = Marshal.PtrToStructure<xloc_diagnostics_t>(diagPtr);
|
|
|
|
// Marshal map name string
|
|
string mapName = string.Empty;
|
|
if (diag.current_active_map != IntPtr.Zero)
|
|
{
|
|
mapName = Marshal.PtrToStringAnsi(diag.current_active_map) ?? string.Empty;
|
|
}
|
|
|
|
// Marshal frame_id string from header
|
|
string frameId = string.Empty;
|
|
if (diag.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(diag.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
return new XlocDiagnosticsData
|
|
{
|
|
HeaderSeq = diag.header.seq,
|
|
HeaderStampSec = diag.header.stamp.sec,
|
|
HeaderStampNsec = diag.header.stamp.nsec,
|
|
HeaderFrameId = frameId,
|
|
XlocState = diag.xloc_state,
|
|
CurrentActiveMap = mapName,
|
|
Reliability = diag.reliability,
|
|
MatchingScore = diag.matching_score
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting diagnostics");
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (diagPtr != IntPtr.Zero)
|
|
{
|
|
XlocNativeInterface.xloc_free_diagnostics(diagPtr);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private void ThrowIfNotInitialized()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_handle == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("XlocClient is not initialized. Call Initialize() first.");
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lock-free initialization check for high-frequency dispatch paths.
|
|
/// Uses volatile read of _handle to avoid lock contention with admin operations.
|
|
/// </summary>
|
|
private void ThrowIfNotInitializedFast()
|
|
{
|
|
if (_handle == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("XlocClient is not initialized. Call Initialize() first.");
|
|
}
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
Dispose(true);
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
protected virtual void Dispose(bool disposing)
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (_handle != IntPtr.Zero)
|
|
{
|
|
try
|
|
{
|
|
XlocNativeInterface.xloc_destroy(_handle);
|
|
_logger?.LogInformation("XlocClient disposed successfully");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error disposing XlocClient");
|
|
}
|
|
finally
|
|
{
|
|
_handle = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
// Note: TF buffer is managed by TF3BufferManager, do not destroy it here
|
|
// Only reset the reference
|
|
_tfBuffer = IntPtr.Zero;
|
|
}
|
|
|
|
_disposed = true;
|
|
}
|
|
|
|
~XlocClient()
|
|
{
|
|
Dispose(false);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Occupancy grid data for display
|
|
/// </summary>
|
|
public class OccupancyGridData
|
|
{
|
|
public float Resolution { get; set; }
|
|
public uint Width { get; set; }
|
|
public uint Height { get; set; }
|
|
public PoseData Origin { get; set; } = new();
|
|
public byte[] Data { get; set; } = Array.Empty<byte>();
|
|
public string FrameId { get; set; } = string.Empty;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pose data
|
|
/// </summary>
|
|
public class PoseData
|
|
{
|
|
public double X { get; set; }
|
|
public double Y { get; set; }
|
|
public double Z { get; set; }
|
|
public double Qx { get; set; }
|
|
public double Qy { get; set; }
|
|
public double Qz { get; set; }
|
|
public double Qw { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// XLOC diagnostics data
|
|
/// </summary>
|
|
public class XlocDiagnosticsData
|
|
{
|
|
public uint HeaderSeq { get; set; }
|
|
public uint HeaderStampSec { get; set; }
|
|
public uint HeaderStampNsec { get; set; }
|
|
public string HeaderFrameId { get; set; } = string.Empty;
|
|
public byte XlocState { get; set; } // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR
|
|
public string CurrentActiveMap { get; set; } = string.Empty;
|
|
public double Reliability { get; set; } // 0.0 to 1.0
|
|
public double MatchingScore { get; set; } // SLAM matching quality
|
|
|
|
public string StateString => XlocState switch
|
|
{
|
|
0 => "MAPPING",
|
|
1 => "LOCALIZATION",
|
|
2 => "PROCESSING",
|
|
3 => "READY",
|
|
4 => "ERROR",
|
|
_ => $"Unknown ({XlocState})"
|
|
};
|
|
}
|