117 lines
4.1 KiB
C#
117 lines
4.1 KiB
C#
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;
|
|
// }
|
|
}
|