1055 lines
39 KiB
C#
1055 lines
39 KiB
C#
using RobotNet10.CustomConfiguration.Events;
|
|
using RobotNet10.CustomConfiguration.Models;
|
|
using RobotNet10.CustomConfiguration.Services;
|
|
using RobotNet10.RobotApp.Services.Navigation;
|
|
using RobotNet10.RobotApp.Services.Navigation.CSharp;
|
|
using System.Reflection;
|
|
|
|
namespace RobotNet10.RobotApp.Services.ConfigManager;
|
|
|
|
/// <summary>
|
|
/// Service implementation for managing navigation configurations
|
|
/// </summary>
|
|
public class NavigationConfig : INavigationConfig
|
|
{
|
|
private readonly IConfigManager _configManager;
|
|
private readonly Logger<NavigationConfig> _logger;
|
|
|
|
private readonly Lock _lockObject = new();
|
|
|
|
// Config objects
|
|
private PurePursuitConfig? _purePursuitConfig;
|
|
private VelocitySignalProcessingConfig? _velocitySignalProcessingConfig;
|
|
private VelocityEstimatorConfig? _velocityEstimatorConfig;
|
|
private MotorDynamicsConfig? _motorDynamicsConfig;
|
|
private PIDConfig? _movePidConfig;
|
|
private PIDConfig? _rotatePidConfig;
|
|
private Navigation.NavigationConfig? _navigationConfig;
|
|
private RobotNet10.NavigationTune.Shared.Models.StanleyConfig? _stanleyConfig;
|
|
private DockToConfig? _dockToConfig;
|
|
private DockToConfig? _moveStraightConfig;
|
|
private LocalPlannerConfig? _localPlannerConfig;
|
|
|
|
// Load flags
|
|
private bool _purePursuitConfigLoaded = false;
|
|
private bool _velocitySignalProcessingConfigLoaded = false;
|
|
private bool _velocityEstimatorConfigLoaded = false;
|
|
private bool _motorDynamicsConfigLoaded = false;
|
|
private bool _movePidConfigLoaded = false;
|
|
private bool _rotatePidConfigLoaded = false;
|
|
private bool _navigationConfigLoaded = false;
|
|
private bool _stanleyConfigLoaded = false;
|
|
private bool _dockToConfigLoaded = false;
|
|
private bool _moveStraightConfigLoaded = false;
|
|
private bool _localPlannerConfigLoaded = false;
|
|
|
|
// Config type constants
|
|
private const string PURE_PURSUIT_CONFIG_TYPE = "PurePursuitConfig";
|
|
private const string VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE = "VelocitySignalProcessingConfig";
|
|
private const string VELOCITY_ESTIMATOR_CONFIG_TYPE = "VelocityEstimatorConfig";
|
|
private const string MOTOR_DYNAMICS_CONFIG_TYPE = "MotorDynamicsConfig";
|
|
private const string MOVE_PID_CONFIG_TYPE = "MovePidConfig";
|
|
private const string ROTATE_PID_CONFIG_TYPE = "RotatePidConfig";
|
|
private const string NAVIGATION_CONFIG_TYPE = "NavigationConfig";
|
|
private const string STANLEY_CONFIG_TYPE = "StanleyConfig";
|
|
private const string DOCK_TO_CONFIG_TYPE = "DockToConfig";
|
|
private const string MOVE_STRAIGHT_CONFIG_TYPE = "MoveStraightConfig";
|
|
private const string LOCAL_PLANNER_CONFIG_TYPE = "LocalPlannerConfig";
|
|
|
|
public NavigationConfig(IConfigManager configManager, Logger<NavigationConfig> logger)
|
|
{
|
|
_configManager = configManager ?? throw new ArgumentNullException(nameof(configManager));
|
|
_logger = logger;
|
|
|
|
// Subscribe to config changes
|
|
_configManager.ConfigChanged += OnConfigChanged;
|
|
}
|
|
|
|
public PurePursuitConfig GetPurepursuitConfig()
|
|
{
|
|
if (!_purePursuitConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_purePursuitConfigLoaded)
|
|
{
|
|
LoadPurePursuitConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _purePursuitConfig ?? throw new InvalidOperationException("PurePursuit configuration not loaded");
|
|
}
|
|
|
|
public VelocitySignalProcessingConfig GetVelocitySignalProcessingConfig()
|
|
{
|
|
if (!_velocitySignalProcessingConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_velocitySignalProcessingConfigLoaded)
|
|
{
|
|
LoadVelocitySignalProcessingConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _velocitySignalProcessingConfig ?? throw new InvalidOperationException("VelocitySignalProcessing configuration not loaded");
|
|
}
|
|
|
|
public VelocityEstimatorConfig GetVelocityEstimatorConfig()
|
|
{
|
|
if (!_velocityEstimatorConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_velocityEstimatorConfigLoaded)
|
|
{
|
|
LoadVelocityEstimatorConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _velocityEstimatorConfig ?? throw new InvalidOperationException("VelocityEstimator configuration not loaded");
|
|
}
|
|
|
|
public MotorDynamicsConfig GetMotorDynamicsConfig()
|
|
{
|
|
if (!_motorDynamicsConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_motorDynamicsConfigLoaded)
|
|
{
|
|
LoadMotorDynamicsConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _motorDynamicsConfig ?? throw new InvalidOperationException("MotorDynamics configuration not loaded");
|
|
}
|
|
|
|
public PIDConfig GetMovePidConfig()
|
|
{
|
|
if (!_movePidConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_movePidConfigLoaded)
|
|
{
|
|
LoadMovePidConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _movePidConfig ?? throw new InvalidOperationException("MovePid configuration not loaded");
|
|
}
|
|
|
|
public PIDConfig GetRotatePidConfig()
|
|
{
|
|
if (!_rotatePidConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_rotatePidConfigLoaded)
|
|
{
|
|
LoadRotatePidConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _rotatePidConfig ?? throw new InvalidOperationException("RotatePid configuration not loaded");
|
|
}
|
|
|
|
public Navigation.NavigationConfig GetNavigationConfig()
|
|
{
|
|
if (!_navigationConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_navigationConfigLoaded)
|
|
{
|
|
LoadNavigationConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _navigationConfig ?? throw new InvalidOperationException("Navigation configuration not loaded");
|
|
}
|
|
|
|
public RobotNet10.NavigationTune.Shared.Models.StanleyConfig GetStanleyConig()
|
|
{
|
|
if (!_stanleyConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_stanleyConfigLoaded)
|
|
{
|
|
LoadStanleyConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _stanleyConfig ?? throw new InvalidOperationException("Stanley configuration not loaded");
|
|
}
|
|
|
|
private async Task LoadPurePursuitConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(PURE_PURSUIT_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("PurePursuit configuration not found, using defaults");
|
|
_purePursuitConfig = new PurePursuitConfig
|
|
{
|
|
LookaheadMin = 0.3,
|
|
Kdd = 1.0,
|
|
LookaheadMax = 2.0,
|
|
CurvatureGain = 1.0,
|
|
WaypointTolerance = 0.1,
|
|
MaxAngularVelocity = 1.5,
|
|
ResolutionSplit = 0.05f
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_purePursuitConfig = MapConfigVariablesToObject<PurePursuitConfig>(configFile.Variables);
|
|
_logger.Info("PurePursuit configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading PurePursuit configuration, using defaults: {ex.Message}");
|
|
_purePursuitConfig = new PurePursuitConfig
|
|
{
|
|
LookaheadMin = 0.3,
|
|
Kdd = 1.0,
|
|
LookaheadMax = 2.0,
|
|
CurvatureGain = 1.0,
|
|
WaypointTolerance = 0.1,
|
|
MaxAngularVelocity = 1.5,
|
|
ResolutionSplit = 0.05f
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_purePursuitConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadVelocitySignalProcessingConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("VelocitySignalProcessing configuration not found, using defaults");
|
|
_velocitySignalProcessingConfig = new VelocitySignalProcessingConfig
|
|
{
|
|
AlphaFilter = 0.3,
|
|
NoiseThreshold = 0.5
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_velocitySignalProcessingConfig = MapConfigVariablesToObject<VelocitySignalProcessingConfig>(configFile.Variables);
|
|
_logger.Info("VelocitySignalProcessing configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading VelocitySignalProcessing configuration, using defaults: {ex.Message}");
|
|
_velocitySignalProcessingConfig = new VelocitySignalProcessingConfig
|
|
{
|
|
AlphaFilter = 0.3,
|
|
NoiseThreshold = 0.5
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_velocitySignalProcessingConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadVelocityEstimatorConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(VELOCITY_ESTIMATOR_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("VelocityEstimator configuration not found, using defaults");
|
|
_velocityEstimatorConfig = new VelocityEstimatorConfig
|
|
{
|
|
MinBlendRatio = 0.15f,
|
|
MaxBlendRatio = 0.8f,
|
|
DefaultBlendRatio = 0.6,
|
|
GoodTrackingThreshold = 0.12f,
|
|
ModerateTrackingThreshold = 0.3,
|
|
GoodTrackingBlend = 0.7,
|
|
ModerateTrackingBlend = 0.5,
|
|
PoorTrackingBlend = 0.25f,
|
|
ConfidenceDecayRate = 0.95f,
|
|
MinConfidence = 0.3
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_velocityEstimatorConfig = MapConfigVariablesToObject<VelocityEstimatorConfig>(configFile.Variables);
|
|
_logger.Info("VelocityEstimator configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading VelocityEstimator configuration, using defaults: {ex.Message}");
|
|
_velocityEstimatorConfig = new VelocityEstimatorConfig
|
|
{
|
|
MinBlendRatio = 0.15f,
|
|
MaxBlendRatio = 0.8f,
|
|
DefaultBlendRatio = 0.6,
|
|
GoodTrackingThreshold = 0.12f,
|
|
ModerateTrackingThreshold = 0.3,
|
|
GoodTrackingBlend = 0.7,
|
|
ModerateTrackingBlend = 0.5,
|
|
PoorTrackingBlend = 0.25f,
|
|
ConfidenceDecayRate = 0.95f,
|
|
MinConfidence = 0.3
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_velocityEstimatorConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadMotorDynamicsConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(MOTOR_DYNAMICS_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("MotorDynamics configuration not found, using defaults");
|
|
_motorDynamicsConfig = new MotorDynamicsConfig
|
|
{
|
|
Tau = 0.3,
|
|
Delta = 0.05f
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_motorDynamicsConfig = MapConfigVariablesToObject<MotorDynamicsConfig>(configFile.Variables);
|
|
_logger.Info("MotorDynamics configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading MotorDynamics configuration, using defaults: {ex.Message}");
|
|
_motorDynamicsConfig = new MotorDynamicsConfig
|
|
{
|
|
Tau = 0.3,
|
|
Delta = 0.05f
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_motorDynamicsConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadMovePidConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(MOVE_PID_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("MovePid configuration not found, using defaults");
|
|
_movePidConfig = new PIDConfig
|
|
{
|
|
Kp = 1.0,
|
|
Ki = 0.0001,
|
|
Kd = 0.6
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_movePidConfig = MapConfigVariablesToObject<PIDConfig>(configFile.Variables);
|
|
_logger.Info("MovePid configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading MovePid configuration, using defaults: {ex.Message}");
|
|
_movePidConfig = new PIDConfig
|
|
{
|
|
Kp = 1.0,
|
|
Ki = 0.0001,
|
|
Kd = 0.6
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_movePidConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadRotatePidConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(ROTATE_PID_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("RotatePid configuration not found, using defaults");
|
|
_rotatePidConfig = new PIDConfig
|
|
{
|
|
Kp = 0.4,
|
|
Ki = 0.0,
|
|
Kd = 0.05,
|
|
IntegralZone = 0.087 // ~5° in radians
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_rotatePidConfig = MapConfigVariablesToObject<PIDConfig>(configFile.Variables);
|
|
_logger.Info("RotatePid configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading RotatePid configuration, using defaults: {ex.Message}");
|
|
_rotatePidConfig = new PIDConfig
|
|
{
|
|
Kp = 0.4,
|
|
Ki = 0.0,
|
|
Kd = 0.05
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_rotatePidConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadNavigationConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(NAVIGATION_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("Navigation configuration not found, using defaults");
|
|
_navigationConfig = new Navigation.NavigationConfig
|
|
{
|
|
MaxLinearVelocity = 0.5,
|
|
MaxAngularVelocity = 0.5,
|
|
MinLinearVelocity = 0.1,
|
|
RotateAngularVelocity = 0.5,
|
|
Acceleration = 0.5,
|
|
Deceleration = 0.5,
|
|
HeadingTolerance = 2,
|
|
ReachedRadius = 0.05,
|
|
InitialRotationThreshold = 5,
|
|
DockToRotateSpeed = 0.05,
|
|
LoadedMaxLinearVelocity = 0.3,
|
|
LoadedHeadingErrorThresholdDegrees = 10.0,
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_navigationConfig = MapConfigVariablesToObject<Navigation.NavigationConfig>(configFile.Variables);
|
|
_logger.Info("Navigation configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading Navigation configuration, using defaults: {ex.Message}");
|
|
_navigationConfig = new Navigation.NavigationConfig
|
|
{
|
|
MaxLinearVelocity = 0.5,
|
|
MaxAngularVelocity = 1.0,
|
|
MinLinearVelocity = 0.1,
|
|
RotateAngularVelocity = 0.5,
|
|
DockToRotateSpeed = 0.05,
|
|
LoadedMaxLinearVelocity = 0.3,
|
|
LoadedHeadingErrorThresholdDegrees = 10.0,
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_navigationConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadStanleyConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(STANLEY_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("Stanley configuration not found, using defaults");
|
|
_stanleyConfig = new RobotNet10.NavigationTune.Shared.Models.StanleyConfig
|
|
{
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
EnableCurvatureFeedforward = true,
|
|
KCurvatureFF = 1.0,
|
|
GoalTolerance = 0.05,
|
|
HeadingTolerance = 5.0,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
LowSpeedThreshold = 0.3,
|
|
LowSpeedAngularGain = 1.5,
|
|
ResolutionSplit = 0.05
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_stanleyConfig = MapConfigVariablesToObject<RobotNet10.NavigationTune.Shared.Models.StanleyConfig>(configFile.Variables);
|
|
_logger.Info("Stanley configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading Stanley configuration, using defaults: {ex.Message}");
|
|
_stanleyConfig = new RobotNet10.NavigationTune.Shared.Models.StanleyConfig
|
|
{
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
EnableCurvatureFeedforward = true,
|
|
KCurvatureFF = 1.0,
|
|
GoalTolerance = 0.05,
|
|
HeadingTolerance = 5.0,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
LowSpeedThreshold = 0.3,
|
|
LowSpeedAngularGain = 1.5,
|
|
ResolutionSplit = 0.05
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_stanleyConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadDockToConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(DOCK_TO_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("DockTo configuration not found, using defaults");
|
|
_dockToConfig = new DockToConfig
|
|
{
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
ReachedRadius = 0.03,
|
|
ResolutionSplit = 0.05,
|
|
DockToLength = 3,
|
|
FinePositioningTimeoutMs = 6000,
|
|
FinePositioningMaxRetries = 3,
|
|
MaxGoalPositionShift = 0.5,
|
|
MaxGoalAngleShiftDegrees = 15.0
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_dockToConfig = MapConfigVariablesToObject<DockToConfig>(configFile.Variables);
|
|
_logger.Info("DockTo configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading DockTo configuration, using defaults: {ex.Message}");
|
|
_dockToConfig = new DockToConfig
|
|
{
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
ReachedRadius = 0.03,
|
|
ResolutionSplit = 0.05,
|
|
DockToLength = 3,
|
|
FinePositioningTimeoutMs = 6000,
|
|
FinePositioningMaxRetries = 3,
|
|
MaxGoalPositionShift = 0.5,
|
|
MaxGoalAngleShiftDegrees = 15.0
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_dockToConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadMoveStraightConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(MOVE_STRAIGHT_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("MoveStraight configuration not found, using defaults");
|
|
_moveStraightConfig = new DockToConfig
|
|
{
|
|
DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD,
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
ReachedRadius = 0.05,
|
|
MaxAngularVelocity = 0.05,
|
|
ResolutionSplit = 0.05,
|
|
DecelerationDistance = 5.0,
|
|
DockingOvershootDetectionRadius = 0.5,
|
|
DockToLength = 3,
|
|
FinePositioningTimeoutMs = 6000,
|
|
FinePositioningMaxRetries = 3,
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_moveStraightConfig = MapConfigVariablesToObject<DockToConfig>(configFile.Variables);
|
|
_moveStraightConfig.DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD;
|
|
_logger.Info("MoveStraight configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading MoveStraight configuration, using defaults: {ex.Message}");
|
|
_moveStraightConfig = new DockToConfig
|
|
{
|
|
DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD,
|
|
K = 2.5,
|
|
Ks = 0.1,
|
|
WheelBase = 0.6,
|
|
MaxSteeringAngle = 0.5,
|
|
GoalApproachDistance = 1.0,
|
|
GoalGainMultiplier = 2.0,
|
|
ReachedRadius = 0.05,
|
|
MaxAngularVelocity = 0.05,
|
|
ResolutionSplit = 0.05,
|
|
DecelerationDistance = 5.0,
|
|
DockingOvershootDetectionRadius = 0.5,
|
|
DockToLength = 3,
|
|
FinePositioningTimeoutMs = 6000,
|
|
FinePositioningMaxRetries = 3,
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_moveStraightConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadLocalPlannerConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(LOCAL_PLANNER_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("LocalPlanner configuration not found, using defaults");
|
|
_localPlannerConfig = new LocalPlannerConfig();
|
|
}
|
|
else
|
|
{
|
|
_localPlannerConfig = MapConfigVariablesToObject<LocalPlannerConfig>(configFile.Variables);
|
|
_logger.Info("LocalPlanner configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading LocalPlanner configuration, using defaults: {ex.Message}");
|
|
_localPlannerConfig = new LocalPlannerConfig();
|
|
}
|
|
finally
|
|
{
|
|
_localPlannerConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private void OnConfigChanged(object? sender, ConfigChangedEventArgs e)
|
|
{
|
|
// Reload config if it's one of our config types
|
|
lock (_lockObject)
|
|
{
|
|
switch (e.ConfigType)
|
|
{
|
|
case PURE_PURSUIT_CONFIG_TYPE:
|
|
_purePursuitConfigLoaded = false;
|
|
_purePursuitConfig = null;
|
|
_logger.Info("PurePursuit configuration changed, will reload on next access");
|
|
break;
|
|
case VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE:
|
|
_velocitySignalProcessingConfigLoaded = false;
|
|
_velocitySignalProcessingConfig = null;
|
|
_logger.Info("VelocitySignalProcessing configuration changed, will reload on next access");
|
|
break;
|
|
case VELOCITY_ESTIMATOR_CONFIG_TYPE:
|
|
_velocityEstimatorConfigLoaded = false;
|
|
_velocityEstimatorConfig = null;
|
|
_logger.Info("VelocityEstimator configuration changed, will reload on next access");
|
|
break;
|
|
case MOTOR_DYNAMICS_CONFIG_TYPE:
|
|
_motorDynamicsConfigLoaded = false;
|
|
_motorDynamicsConfig = null;
|
|
_logger.Info("MotorDynamics configuration changed, will reload on next access");
|
|
break;
|
|
case MOVE_PID_CONFIG_TYPE:
|
|
_movePidConfigLoaded = false;
|
|
_movePidConfig = null;
|
|
_logger.Info("MovePid configuration changed, will reload on next access");
|
|
break;
|
|
case ROTATE_PID_CONFIG_TYPE:
|
|
_rotatePidConfigLoaded = false;
|
|
_rotatePidConfig = null;
|
|
_logger.Info("RotatePid configuration changed, will reload on next access");
|
|
break;
|
|
case NAVIGATION_CONFIG_TYPE:
|
|
_navigationConfigLoaded = false;
|
|
_navigationConfig = null;
|
|
_logger.Info("Navigation configuration changed, will reload on next access");
|
|
break;
|
|
case STANLEY_CONFIG_TYPE:
|
|
_stanleyConfigLoaded = false;
|
|
_stanleyConfig = null;
|
|
_logger.Info("Stanley configuration changed, will reload on next access");
|
|
break;
|
|
case DOCK_TO_CONFIG_TYPE:
|
|
_dockToConfigLoaded = false;
|
|
_dockToConfig = null;
|
|
_logger.Info("DockTo configuration changed, will reload on next access");
|
|
break;
|
|
case MOVE_STRAIGHT_CONFIG_TYPE:
|
|
_moveStraightConfigLoaded = false;
|
|
_moveStraightConfig = null;
|
|
_logger.Info("MoveStraight configuration changed, will reload on next access");
|
|
break;
|
|
case LOCAL_PLANNER_CONFIG_TYPE:
|
|
_localPlannerConfigLoaded = false;
|
|
_localPlannerConfig = null;
|
|
_logger.Info("LocalPlanner configuration changed, will reload on next access");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private T MapConfigVariablesToObject<T>(List<ConfigVariable> variables) where T : new()
|
|
{
|
|
var obj = new T();
|
|
var properties = typeof(T).GetProperties(BindingFlags.Public | BindingFlags.Instance | BindingFlags.SetProperty);
|
|
|
|
foreach (var property in properties)
|
|
{
|
|
// Find variable by exact name match (case-sensitive)
|
|
var variable = variables.FirstOrDefault(v => v.Name == property.Name);
|
|
|
|
if (variable != null && variable.Value != null)
|
|
{
|
|
try
|
|
{
|
|
var convertedValue = ConvertValue(variable.Value, property.PropertyType, variable.Type);
|
|
property.SetValue(obj, convertedValue);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
|
|
return obj;
|
|
}
|
|
|
|
private object? ConvertValue(object? value, Type targetType, ConfigVariableType variableType)
|
|
{
|
|
if (value == null)
|
|
return targetType.IsValueType ? Activator.CreateInstance(targetType) : null;
|
|
|
|
// If value is already of the correct type, return it
|
|
if (targetType.IsInstanceOfType(value))
|
|
return value;
|
|
|
|
try
|
|
{
|
|
// Handle nullable types
|
|
var underlyingType = Nullable.GetUnderlyingType(targetType) ?? targetType;
|
|
|
|
// Convert based on target type
|
|
if (underlyingType == typeof(string))
|
|
{
|
|
return value.ToString() ?? string.Empty;
|
|
}
|
|
else if (underlyingType == typeof(int))
|
|
{
|
|
if (value is int i) return i;
|
|
if (int.TryParse(value.ToString(), out var parsed)) return parsed;
|
|
if (value is double d) return (int)d;
|
|
throw new InvalidCastException($"Cannot convert {value.GetType()} to int");
|
|
}
|
|
else if (underlyingType == typeof(double))
|
|
{
|
|
if (value is double d) return d;
|
|
if (double.TryParse(value.ToString(), System.Globalization.NumberStyles.Any, System.Globalization.CultureInfo.InvariantCulture, out var parsed)) return parsed;
|
|
if (value is int i) return i;
|
|
throw new InvalidCastException($"Cannot convert {value.GetType()} to double");
|
|
}
|
|
else if (underlyingType == typeof(bool))
|
|
{
|
|
if (value is bool b) return b;
|
|
if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
|
|
// Handle numeric values: 0/1, "0"/"1", etc.
|
|
if (value.ToString()?.Trim() == "0" || value.ToString()?.Trim().ToLower() == "false") return false;
|
|
if (value.ToString()?.Trim() == "1" || value.ToString()?.Trim().ToLower() == "true") return true;
|
|
throw new InvalidCastException($"Cannot convert {value.GetType()} to bool");
|
|
}
|
|
else if (underlyingType.IsEnum)
|
|
{
|
|
var enumValue = Enum.Parse(underlyingType, value.ToString() ?? "", true);
|
|
return enumValue;
|
|
}
|
|
// Handle Dictionary types (e.g., Dictionary<SafetySpeed, double>)
|
|
else if (targetType.IsGenericType && targetType.GetGenericTypeDefinition() == typeof(Dictionary<,>))
|
|
{
|
|
return ConvertToDictionary(value, targetType);
|
|
}
|
|
else
|
|
{
|
|
// Try standard conversion
|
|
return Convert.ChangeType(value, underlyingType);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Error converting value {value} of type {value.GetType()} to {targetType}: {ex.Message}");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert value to Dictionary<TKey, TValue>
|
|
/// Handles conversion from Dictionary<string, object> (JSON) to typed dictionaries
|
|
/// Supports enum keys (e.g., Dictionary<SafetySpeed, double>)
|
|
/// </summary>
|
|
private object ConvertToDictionary(object value, Type targetDictionaryType)
|
|
{
|
|
// Get key and value types from Dictionary<TKey, TValue>
|
|
var genericArgs = targetDictionaryType.GetGenericArguments();
|
|
var keyType = genericArgs[0];
|
|
var valueType = genericArgs[1];
|
|
|
|
// Create dictionary instance
|
|
var dictionaryType = typeof(Dictionary<,>).MakeGenericType(keyType, valueType);
|
|
var dictionary = Activator.CreateInstance(dictionaryType);
|
|
var addMethod = dictionaryType.GetMethod("Add") ?? throw new InvalidOperationException("Cannot find Add method on Dictionary");
|
|
|
|
// Handle source Dictionary<string, object> from JSON
|
|
if (value is Dictionary<string, object> sourceDict)
|
|
{
|
|
foreach (var kvp in sourceDict)
|
|
{
|
|
// Convert key (usually from string to enum)
|
|
object convertedKey;
|
|
if (keyType.IsEnum)
|
|
{
|
|
// Parse enum from string
|
|
convertedKey = Enum.Parse(keyType, kvp.Key, ignoreCase: true);
|
|
}
|
|
else if (keyType == typeof(string))
|
|
{
|
|
convertedKey = kvp.Key;
|
|
}
|
|
else
|
|
{
|
|
convertedKey = Convert.ChangeType(kvp.Key, keyType);
|
|
}
|
|
|
|
// Convert value
|
|
object? convertedValue = ConvertDictionaryValue(kvp.Value, valueType);
|
|
|
|
// Add to dictionary
|
|
addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
|
|
}
|
|
|
|
return dictionary ?? new Dictionary<object, object>();
|
|
}
|
|
// Handle JsonElement (alternative JSON representation)
|
|
else if (value is System.Text.Json.JsonElement jsonElement && jsonElement.ValueKind == System.Text.Json.JsonValueKind.Object)
|
|
{
|
|
foreach (var property in jsonElement.EnumerateObject())
|
|
{
|
|
// Convert key
|
|
object convertedKey;
|
|
if (keyType.IsEnum)
|
|
{
|
|
convertedKey = Enum.Parse(keyType, property.Name, ignoreCase: true);
|
|
}
|
|
else if (keyType == typeof(string))
|
|
{
|
|
convertedKey = property.Name;
|
|
}
|
|
else
|
|
{
|
|
convertedKey = Convert.ChangeType(property.Name, keyType);
|
|
}
|
|
|
|
// Convert value from JsonElement
|
|
object? convertedValue = ConvertJsonElementValue(property.Value, valueType);
|
|
|
|
// Add to dictionary
|
|
addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
|
|
}
|
|
|
|
return dictionary ?? new Dictionary<object, object>();
|
|
}
|
|
else
|
|
{
|
|
throw new InvalidCastException($"Cannot convert {value.GetType()} to {targetDictionaryType}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert dictionary value to target type
|
|
/// </summary>
|
|
private static object? ConvertDictionaryValue(object? value, Type valueType)
|
|
{
|
|
if (value == null)
|
|
return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;
|
|
|
|
if (valueType == typeof(double))
|
|
{
|
|
if (value is double d) return d;
|
|
if (double.TryParse(value.ToString(), out var parsed)) return parsed;
|
|
if (value is int i) return (double)i;
|
|
return Convert.ChangeType(value, valueType);
|
|
}
|
|
else if (valueType == typeof(int))
|
|
{
|
|
if (value is int i) return i;
|
|
if (int.TryParse(value.ToString(), out var parsed)) return parsed;
|
|
if (value is double d) return (int)d;
|
|
return Convert.ChangeType(value, valueType);
|
|
}
|
|
else if (valueType == typeof(string))
|
|
{
|
|
return value.ToString() ?? string.Empty;
|
|
}
|
|
else if (valueType == typeof(bool))
|
|
{
|
|
if (value is bool b) return b;
|
|
if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
|
|
return Convert.ChangeType(value, valueType);
|
|
}
|
|
else if (valueType.IsEnum)
|
|
{
|
|
return Enum.Parse(valueType, value.ToString() ?? "", ignoreCase: true);
|
|
}
|
|
else
|
|
{
|
|
return Convert.ChangeType(value, valueType);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert JsonElement value to target type
|
|
/// </summary>
|
|
private static object? ConvertJsonElementValue(System.Text.Json.JsonElement element, Type valueType)
|
|
{
|
|
if (element.ValueKind == System.Text.Json.JsonValueKind.Null)
|
|
return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;
|
|
|
|
if (valueType == typeof(double))
|
|
{
|
|
return element.GetDouble();
|
|
}
|
|
else if (valueType == typeof(int))
|
|
{
|
|
return element.GetInt32();
|
|
}
|
|
else if (valueType == typeof(string))
|
|
{
|
|
return element.GetString();
|
|
}
|
|
else if (valueType == typeof(bool))
|
|
{
|
|
return element.GetBoolean();
|
|
}
|
|
else if (valueType.IsEnum)
|
|
{
|
|
return Enum.Parse(valueType, element.GetString() ?? "", ignoreCase: true);
|
|
}
|
|
else
|
|
{
|
|
var rawValue = element.GetRawText();
|
|
return System.Text.Json.JsonSerializer.Deserialize(rawValue, valueType);
|
|
}
|
|
}
|
|
|
|
public DockToConfig GetDockToConfig()
|
|
{
|
|
if (!_dockToConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_dockToConfigLoaded)
|
|
{
|
|
LoadDockToConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _dockToConfig ?? throw new InvalidOperationException("DockTo configuration not loaded");
|
|
}
|
|
|
|
public DockToConfig GetMoveStraightConfig()
|
|
{
|
|
if (!_moveStraightConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_moveStraightConfigLoaded)
|
|
{
|
|
LoadMoveStraightConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _moveStraightConfig ?? throw new InvalidOperationException("MoveStraight configuration not loaded");
|
|
}
|
|
|
|
public LocalPlannerConfig GetLocalPlannerConfig()
|
|
{
|
|
if (!_localPlannerConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_localPlannerConfigLoaded)
|
|
{
|
|
LoadLocalPlannerConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _localPlannerConfig ?? throw new InvalidOperationException("LocalPlanner configuration not loaded");
|
|
}
|
|
}
|