457 lines
17 KiB
C#
457 lines
17 KiB
C#
using RobotNet10.CustomConfiguration.Events;
|
|
using RobotNet10.CustomConfiguration.Models;
|
|
using RobotNet10.CustomConfiguration.Services;
|
|
using RobotNet10.RobotApp.Services.Robot;
|
|
using RobotNet10.RobotApp.Services.Simulation;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using System.Reflection;
|
|
|
|
namespace RobotNet10.RobotApp.Services.ConfigManager;
|
|
|
|
/// <summary>
|
|
/// Service implementation for managing robot configurations
|
|
/// </summary>
|
|
public class RobotConfiguration : IRobotConfiguration
|
|
{
|
|
private readonly IConfigManager _configManager;
|
|
private readonly Logger<RobotConfiguration> _logger;
|
|
|
|
private readonly Lock _lockObject = new();
|
|
private RobotPhysicalConfig? _robotPhysicalConfig;
|
|
private SimulationConfig? _simulationConfig;
|
|
private bool _robotPhysicalConfigLoaded = false;
|
|
private bool _simulationConfigLoaded = false;
|
|
|
|
private const string ROBOT_PHYSICAL_CONFIG_TYPE = "RobotPhysicalConfig";
|
|
private const string SIMULATION_CONFIG_TYPE = "SimulationConfig";
|
|
|
|
public RobotConfiguration(IConfigManager configManager, Logger<RobotConfiguration> logger)
|
|
{
|
|
_configManager = configManager ?? throw new ArgumentNullException(nameof(configManager));
|
|
_logger = logger;
|
|
|
|
// Subscribe to config changes
|
|
_configManager.ConfigChanged += OnConfigChanged;
|
|
}
|
|
|
|
public RobotPhysicalConfig GetRobotPhysicalConfig()
|
|
{
|
|
if (!_robotPhysicalConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_robotPhysicalConfigLoaded)
|
|
{
|
|
LoadRobotPhysicalConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _robotPhysicalConfig ?? throw new InvalidOperationException("Robot Physical configuration not loaded");
|
|
}
|
|
|
|
public SimulationConfig GetSimulationConfig()
|
|
{
|
|
if (!_simulationConfigLoaded)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
if (!_simulationConfigLoaded)
|
|
{
|
|
LoadSimulationConfigAsync().GetAwaiter().GetResult();
|
|
}
|
|
}
|
|
}
|
|
return _simulationConfig ?? throw new InvalidOperationException("Simulation configuration not loaded");
|
|
}
|
|
|
|
private async Task LoadRobotPhysicalConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(ROBOT_PHYSICAL_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("Robot Physical configuration not found, using defaults");
|
|
_robotPhysicalConfig = new RobotPhysicalConfig
|
|
{
|
|
WheelRadius = 0.1,
|
|
WheelBase = 0.6,
|
|
Width = 0.606,
|
|
Length = 1.106,
|
|
Height = 0.5,
|
|
NavigationType = NavigationType.Differential
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_robotPhysicalConfig = MapConfigVariablesToRobotPhysicalConfig(configFile.Variables);
|
|
_logger.Info("Robot Physical configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading Robot Physical configuration, using defaults: {ex.Message}");
|
|
_robotPhysicalConfig = new RobotPhysicalConfig
|
|
{
|
|
WheelRadius = 0.1,
|
|
WheelBase = 0.6,
|
|
Width = 0.606,
|
|
Length = 1.106,
|
|
Height = 0.5,
|
|
NavigationType = NavigationType.Differential
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_robotPhysicalConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private async Task LoadSimulationConfigAsync()
|
|
{
|
|
try
|
|
{
|
|
var configFile = await _configManager.GetConfigByTypeAsync(SIMULATION_CONFIG_TYPE);
|
|
|
|
if (configFile == null)
|
|
{
|
|
_logger.Warning("Simulation configuration not found, using defaults");
|
|
_simulationConfig = new SimulationConfig
|
|
{
|
|
IsEnable = false,
|
|
MaxVelocity = 1.5,
|
|
MaxAngularVelocity = 0.5,
|
|
Acceleration = 2.0,
|
|
Deceleration = 10.0
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_simulationConfig = MapConfigVariablesToObject<SimulationConfig>(configFile.Variables);
|
|
_logger.Info("Simulation configuration loaded successfully");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error loading Simulation configuration, using defaults: {ex.Message}");
|
|
_simulationConfig = new SimulationConfig
|
|
{
|
|
IsEnable = false,
|
|
MaxVelocity = 1.5,
|
|
MaxAngularVelocity = 0.5,
|
|
Acceleration = 2.0,
|
|
Deceleration = 10.0
|
|
};
|
|
}
|
|
finally
|
|
{
|
|
_simulationConfigLoaded = true;
|
|
}
|
|
}
|
|
|
|
private void OnConfigChanged(object? sender, ConfigChangedEventArgs e)
|
|
{
|
|
// Reload config if it's one of our config types
|
|
if (e.ConfigType == ROBOT_PHYSICAL_CONFIG_TYPE)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
_robotPhysicalConfigLoaded = false;
|
|
_robotPhysicalConfig = null;
|
|
}
|
|
_logger.Info("Robot Physical configuration changed, will reload on next access");
|
|
}
|
|
else if (e.ConfigType == SIMULATION_CONFIG_TYPE)
|
|
{
|
|
lock (_lockObject)
|
|
{
|
|
_simulationConfigLoaded = false;
|
|
_simulationConfig = null;
|
|
}
|
|
_logger.Info("Simulation configuration changed, will reload on next access");
|
|
}
|
|
}
|
|
|
|
private RobotPhysicalConfig MapConfigVariablesToRobotPhysicalConfig(List<ConfigVariable> variables)
|
|
{
|
|
var config = new RobotPhysicalConfig();
|
|
|
|
// Get properties with reflection to handle private setters
|
|
// Use BindingFlags to get properties with private setters
|
|
var properties = typeof(RobotPhysicalConfig).GetProperties(BindingFlags.Public | BindingFlags.Instance);
|
|
|
|
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(config, convertedValue);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
|
|
return config;
|
|
}
|
|
|
|
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(), 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 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 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);
|
|
}
|
|
}
|
|
}
|