Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotLocalization.cs

@@ -0,0 +1,116 @@
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Xloc;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot.Modules;
+
+
+public class RobotLocalization(IRobotConfiguration RobotConfiguration,
+                               XlocIntegrationService xlocService,
+                               SimulationVisualization SimVisualization,
+                               Logger<RobotLocalization> Logger) 
+                               : ILocalization
+{
+    public double X => IsSimulation ? SimVisualization.X : GetXlocX();
+    public double Y => IsSimulation ? SimVisualization.Y : GetXlocY();
+    public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : GetXlocTheta();
+    public bool IsReady => IsSimulation ? true : IsXlocReady();
+    public string CurrentActiveMap => IsSimulation ? "" : GetXlocCurrentActiveMap();
+    public double DeviationRange { get; private set; }
+    public double LocalizationScore => IsSimulation ? 1.0 : GetXlocLocalizationScore();
+    public bool PositionInitialized => IsSimulation ? true : GetXlocPositionInitialized();
+    private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;
+
+    private double GetXlocX()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.x ?? 0.0;
+    }
+
+    private double GetXlocY()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.y ?? 0.0;
+    }
+
+    private double GetXlocTheta()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.yaw ?? 0.0;
+    }
+
+    private bool IsXlocReady()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR. Accept 1,2,3 so orders are allowed once localizing.
+        if (diagnostics == null) return false;
+        return diagnostics.XlocState is 1 or 2 or 3;
+    }
+
+    private string GetXlocCurrentActiveMap()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        return diagnostics?.CurrentActiveMap ?? "";
+    }
+
+    private double GetXlocLocalizationScore()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        return diagnostics?.Reliability ?? 0.0; // Use Reliability (0.0 to 1.0) as LocalizationScore
+    }
+
+    private bool GetXlocPositionInitialized()
+    {
+        // Position is initialized if we have a valid pose from XLOC and it's not in ERROR state
+        var pose = xlocService.GetCurrentPose2D();
+        var diagnostics = xlocService.GetDiagnostics();
+        
+        return pose.HasValue && diagnostics?.XlocState != 4; // 4 = ERROR
+    }
+
+    public double DistanceTo(double x, double y)
+    {
+        return Math.Sqrt(Math.Pow(x - X, 2) + Math.Pow(y - Y, 2));
+    }
+
+    public MessageResult SetInitializePosition(double x, double y, double theta)
+    {
+        try
+        {
+            if (IsSimulation)
+            {
+                SimVisualization.LocalizationInitialize(x, y, theta * 180 / Math.PI);
+                return new(true);
+            }
+            else
+            {
+                // Use XlocIntegrationService to set initial pose
+                // theta is in radians, convert to radians for xloc (it expects radians)
+                bool result = xlocService.SetInitialPose(x, y, 0.0, 0.0, 0.0, theta);
+                if (result)
+                {
+                    return new(true, "Initial position set successfully");
+                }
+                else
+                {
+                    return new(false, "Failed to set initial position");
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Initialize robot position failed: {ex.Message}");
+            return new(false, $"Initialize robot position failed: {ex.Message}");
+        }
+    }
+
+    // private bool GetIsReady()
+    // {
+    //     if (IsSimulation) return true;
+    //     return xlocService.IsReady;
+    // }
+}