From d45dbe867172f4b575695620b7b63a6a4f8e9e10 Mon Sep 17 00:00:00 2001 From: Darrell Date: Mon, 18 Nov 2024 22:45:36 -0500 Subject: [PATCH] Locator configuration added (#794) * Stop hardcoding locators/scenarios --- src/Events/NodeSettingReceivedEventArgs.cs | 6 +- src/Models/Config.cs | 54 ++++- src/Models/State.cs | 57 ++++-- src/Optimizers/AbsorptionErrOptimizer.cs | 1 - src/Services/MqttCoordinator.cs | 221 ++++++++++++++++----- src/config.example.yaml | 22 +- tests/ConfigTests.cs | 48 +++++ 7 files changed, 328 insertions(+), 81 deletions(-) create mode 100644 tests/ConfigTests.cs diff --git a/src/Events/NodeSettingReceivedEventArgs.cs b/src/Events/NodeSettingReceivedEventArgs.cs index ca55d5fb..fd03186e 100644 --- a/src/Events/NodeSettingReceivedEventArgs.cs +++ b/src/Events/NodeSettingReceivedEventArgs.cs @@ -2,7 +2,7 @@ public class NodeSettingReceivedEventArgs { - public string NodeId { get; set; } - public string Setting { get; set; } - public string Payload { get; set; } + public string? NodeId { get; set; } + public string? Setting { get; set; } + public string? Payload { get; set; } } \ No newline at end of file diff --git a/src/Models/Config.cs b/src/Models/Config.cs index 7f73f623..71b68999 100644 --- a/src/Models/Config.cs +++ b/src/Models/Config.cs @@ -25,7 +25,8 @@ public class Config [YamlMember(Alias = "map")] public ConfigMap Map { get; set; } = new(); - [YamlMember(Alias = "floors")] public ConfigFloor[] Floors { get; set; } = Array.Empty(); + [YamlMember(Alias = "floors")] + public ConfigFloor[] Floors { get; set; } = Array.Empty(); [YamlMember(Alias = "nodes")] public ConfigNode[] Nodes { get; set; } = Array.Empty(); @@ -39,10 +40,59 @@ public class Config [YamlMember(Alias = "history")] public ConfigHistory History { get; set; } = new(); + [YamlMember(Alias = "locators")] + public ConfigLocators Locators { get; set; } = new(); + + [YamlMember(Alias = "optimization")] + public ConfigOptimization Optimization { get; set; } = new(); + } + + public class ConfigLocators + { + [YamlMember(Alias = "nadaraya_watson")] + public NadarayaWatsonConfig NadarayaWatson { get; set; } = new(); + + [YamlMember(Alias = "nealder_mead")] + public NealderMeadConfig NealderMead { get; set; } = new(); + + [YamlMember(Alias = "nearest_node")] + public NearestNodeConfig NearestNode { get; set; } = new(); + } + + public class NadarayaWatsonConfig + { + [YamlMember(Alias = "enabled")] + public bool Enabled { get; set; } + + [YamlMember(Alias = "floors")] + public string[]? Floors { get; set; } + + [YamlMember(Alias = "bandwidth")] + public double Bandwidth { get; set; } = 0.5; + + [YamlMember(Alias = "kernel")] + public string Kernel { get; set; } = "gaussian"; + } + + public class NealderMeadConfig + { + [YamlMember(Alias = "enabled")] + public bool Enabled { get; set; } + + [YamlMember(Alias = "floors")] + public string[]? Floors { get; set; } + [YamlMember(Alias = "weighting")] public ConfigWeighting Weighting { get; set; } = new(); + } + + public class NearestNodeConfig + { + [YamlMember(Alias = "enabled")] + public bool Enabled { get; set; } - [YamlMember(Alias = "optimization")] public ConfigOptimization Optimization { get; set; } = new(); + [YamlMember(Alias = "max_distance")] + public double? MaxDistance { get; set; } } public class ConfigMap diff --git a/src/Models/State.cs b/src/Models/State.cs index 63974f7c..849bed80 100644 --- a/src/Models/State.cs +++ b/src/Models/State.cs @@ -13,14 +13,6 @@ public class State { public State(ConfigLoader cl) { - IEnumerable GetFloorsByIds(string[]? floorIds) - { - if (floorIds == null) yield break; - foreach (var floorId in floorIds) - if (Floors.TryGetValue(floorId, out var floor)) - yield return floor; - } - void LoadConfig(Config c) { Config = c; @@ -56,12 +48,13 @@ void LoadConfig(Config c) NamesToTrack = namesToTrack; ConfigDeviceByName = configDeviceByName; - Weighting = c.Weighting?.Algorithm switch + var w = c?.Locators?.NealderMead?.Weighting; + Weighting = w?.Algorithm switch { "equal" => new EqualWeighting(), - "gaussian" => new GaussianWeighting(c.Weighting?.Props), - "exponential" => new ExponentialWeighting(c.Weighting?.Props), - _ => new GaussianWeighting(c.Weighting?.Props), + "gaussian" => new GaussianWeighting(w?.Props), + "exponential" => new ExponentialWeighting(w?.Props), + _ => new GaussianWeighting(w?.Props), }; foreach (var device in Devices.Values) device.Check = true; } @@ -100,7 +93,6 @@ public OptimizationSnapshot TakeOptimizationSnapshot() Tx = tx, Rx = rx, }); - } if (OptimizationSnaphots.Count > Config?.Optimization.MaxSnapshots) OptimizationSnaphots.RemoveAt(0); @@ -109,11 +101,44 @@ public OptimizationSnapshot TakeOptimizationSnapshot() return os; } + IEnumerable GetFloorsByIds(string[]? floorIds) + { + if (floorIds == null) + { + foreach (var floor in Floors.Values) + yield return floor; + } + else + foreach (var floorId in floorIds) + if (Floors.TryGetValue(floorId, out var floor)) + yield return floor; + } + public IEnumerable GetScenarios(Device device) { - foreach (var floor in Floors.Values) yield return new Scenario(Config, new NelderMeadMultilateralizer(device, floor, this), floor.Name); - //yield return new Scenario(_state.Config, new MultiFloorMultilateralizer(device, _state), "Multifloor"); - yield return new Scenario(Config, new NearestNode(device), "NearestNode"); + var nealderMead = Config?.Locators?.NealderMead; + var nadarayaWatson = Config?.Locators?.NadarayaWatson; + var nearestNode = Config?.Locators?.NearestNode; + + if ((nealderMead?.Enabled ?? false) || (nadarayaWatson?.Enabled ?? false) || (nearestNode?.Enabled ?? false)) + { + if (nealderMead?.Enabled ?? false) + foreach (var floor in GetFloorsByIds(nealderMead?.Floors)) + yield return new Scenario(Config, new NelderMeadMultilateralizer(device, floor, this), floor.Name); + + if (nadarayaWatson?.Enabled ?? false) + foreach (var floor in GetFloorsByIds(nadarayaWatson?.Floors)) + yield return new Scenario(Config, new NadarayaWatsonMultilateralizer(device, floor, this), floor.Name); + + if (nearestNode?.Enabled ?? false) + yield return new Scenario(Config, new NearestNode(device), "NearestNode"); + } + else + { + Log.Warning("No locators enabled, using default NelderMead"); + foreach (var floor in Floors.Values) + yield return new Scenario(Config, new NelderMeadMultilateralizer(device, floor, this), floor.Name); + } } public bool ShouldTrack(Device device) diff --git a/src/Optimizers/AbsorptionErrOptimizer.cs b/src/Optimizers/AbsorptionErrOptimizer.cs index 1c1faa8e..56595a72 100644 --- a/src/Optimizers/AbsorptionErrOptimizer.cs +++ b/src/Optimizers/AbsorptionErrOptimizer.cs @@ -40,7 +40,6 @@ public OptimizationResults Optimize(OptimizationSnapshot os) var error = rxNodes .Select((dn, i) => new { err = pos[i] - Distance(x, dn), weight = 1 }) .Average(a => a.weight * Math.Pow(a.err, 2)); - //Console.WriteLine("{0,-20}> Absorption: {1:#0.000, 10} Err: {2:##0.0000000000000000}", node.Id, x[0], error); return error; }); diff --git a/src/Services/MqttCoordinator.cs b/src/Services/MqttCoordinator.cs index 2e9bcce0..999cdfd5 100644 --- a/src/Services/MqttCoordinator.cs +++ b/src/Services/MqttCoordinator.cs @@ -11,6 +11,21 @@ namespace ESPresense.Services; +public class MqttMessageProcessingException : Exception +{ + public string Topic { get; } + public string? Payload { get; } + public string MessageType { get; } + + public MqttMessageProcessingException(string message, string topic, string? payload, string messageType, Exception? innerException = null) + : base(message, innerException) + { + Topic = topic; + Payload = payload; + MessageType = messageType; + } +} + public class MqttCoordinator { private readonly ConfigLoader _cfg; @@ -125,76 +140,52 @@ private async Task GetClient() private async Task OnMqttMessageReceived(MqttApplicationMessageReceivedEventArgs arg) { var parts = arg.ApplicationMessage.Topic.Split('/'); + var payload = arg.ApplicationMessage.ConvertPayloadToString(); + try { switch (parts) { case ["espresense", "rooms", _, "telemetry"]: - if (NodeTelemetryReceivedAsync != null) - { - var ds = JsonConvert.DeserializeObject(arg.ApplicationMessage.ConvertPayloadToString()); - if (ds != null) await NodeTelemetryReceivedAsync(new NodeTelemetryReceivedEventArgs { NodeId = parts[2], Payload = ds }); - } - + await ProcessTelemetryMessage(parts[2], payload); break; case ["espresense", "rooms", _, "status"]: - var online = arg.ApplicationMessage.ConvertPayloadToString() == "online"; - NodeStatusReceivedAsync?.Invoke(new NodeStatusReceivedEventArgs { NodeId = parts[2], Online = online }); + await ProcessStatusMessage(parts[2], payload); break; case ["espresense", "rooms", _, _]: - { - if (NodeSettingReceivedAsync != null) - await NodeSettingReceivedAsync(new NodeSettingReceivedEventArgs - { - NodeId = parts[2], - Setting = parts[3], - Payload = arg.ApplicationMessage.ConvertPayloadToString() - } - ); - break; - } + await ProcessNodeSettingMessage(parts[2], parts[3], payload); + break; case ["espresense", "devices", _, _]: - if (DeviceMessageReceivedAsync != null) - { - var deviceId = parts[2]; - var nodeId = parts[3]; - var deserializeObject = JsonConvert.DeserializeObject(arg.ApplicationMessage.ConvertPayloadToString()); - if (deserializeObject != null) - await DeviceMessageReceivedAsync(new DeviceMessageEventArgs - { - DeviceId = deviceId, - NodeId = nodeId, - Payload = deserializeObject - }); - } - + await ProcessDeviceMessage(parts[2], parts[3], payload); break; case ["espresense", "settings", _, "config"]: - if (DeviceConfigReceivedAsync != null) - { - var ds = JsonConvert.DeserializeObject(arg.ApplicationMessage.ConvertPayloadToString() ?? ""); - if (ds != null) - { - ds.OriginalId = parts[2]; - await DeviceConfigReceivedAsync(new DeviceSettingsEventArgs - { - DeviceId = parts[2], - Payload = ds - }); - } - } + await ProcessDeviceConfigMessage(parts[2], payload); break; case ["homeassistant", "device_tracker", _, "config"]: - HandleDiscoveryMessage(arg.ApplicationMessage.Topic, arg.ApplicationMessage.ConvertPayloadToString()); + await ProcessDiscoveryMessage(arg.ApplicationMessage.Topic, payload); break; default: - if (MqttMessageReceivedAsync != null) await MqttMessageReceivedAsync(arg); + if (MqttMessageReceivedAsync != null) + await MqttMessageReceivedAsync(arg); break; } } + catch (JsonSerializationException ex) + { + _logger.LogError(ex, "JSON deserialization error for topic {Topic}. Payload: {Payload}", + arg.ApplicationMessage.Topic, payload); + MqttMessageMalformed?.Invoke(this, EventArgs.Empty); + } + catch (MqttMessageProcessingException ex) + { + _logger.LogError(ex, "Error processing {MessageType} message for topic {Topic}. Payload: {Payload}", + ex.MessageType, ex.Topic, ex.Payload); + MqttMessageMalformed?.Invoke(this, EventArgs.Empty); + } catch (Exception ex) { - _logger.LogWarning("Error parsing mqtt message from {topic}: {error}", arg.ApplicationMessage.Topic, ex.Message); + _logger.LogError(ex, "Unexpected error processing message for topic {Topic}. Payload: {Payload}", + arg.ApplicationMessage.Topic, payload); MqttMessageMalformed?.Invoke(this, EventArgs.Empty); } } @@ -211,11 +202,131 @@ public async Task EnqueueAsync(string topic, string? payload, bool retain = fals } } - private void HandleDiscoveryMessage(string topic, string payload) + private async Task ProcessTelemetryMessage(string nodeId, string? payload) { - _logger.LogTrace($"Received discovery message on topic: {topic}"); - if (AutoDiscovery.TryDeserialize(topic, payload, out var msg)) - PreviousDeviceDiscovered?.Invoke(this, new PreviousDeviceDiscoveredEventArgs { AutoDiscover = msg }); - } -} + if (NodeTelemetryReceivedAsync == null) return; + + try + { + var telemetry = JsonConvert.DeserializeObject(payload ?? ""); + if (telemetry == null) + throw new MqttMessageProcessingException( + "Telemetry data was null after deserialization", + $"espresense/rooms/{nodeId}/telemetry", + payload, + "Telemetry"); + + await NodeTelemetryReceivedAsync(new NodeTelemetryReceivedEventArgs + { + NodeId = nodeId, + Payload = telemetry + }); + } + catch (JsonException ex) + { + throw new MqttMessageProcessingException( + "Failed to parse telemetry data", + $"espresense/rooms/{nodeId}/telemetry", + payload, + "Telemetry", + ex); + } + } + + private async Task ProcessStatusMessage(string nodeId, string? payload) + { + if (NodeStatusReceivedAsync == null) return; + + if (payload == null) + throw new MqttMessageProcessingException("Status payload was null", $"espresense/rooms/{nodeId}/status", null, "Status"); + + var online = payload == "online"; + await NodeStatusReceivedAsync(new NodeStatusReceivedEventArgs + { + NodeId = nodeId, + Online = online + }); + } + + private async Task ProcessDeviceMessage(string deviceId, string nodeId, string? payload) + { + if (DeviceMessageReceivedAsync == null) return; + try + { + var deviceMessage = JsonConvert.DeserializeObject(payload ?? ""); + if (deviceMessage == null) + throw new MqttMessageProcessingException("Device message was null after deserialization", $"espresense/devices/{deviceId}/{nodeId}", payload, "DeviceMessage"); + + await DeviceMessageReceivedAsync(new DeviceMessageEventArgs + { + DeviceId = deviceId, + NodeId = nodeId, + Payload = deviceMessage + }); + } + catch (JsonException ex) + { + throw new MqttMessageProcessingException("Failed to parse device message", $"espresense/devices/{deviceId}/{nodeId}", payload, "DeviceMessage", ex); + } + } + + private async Task ProcessDeviceConfigMessage(string deviceId, string? payload) + { + if (DeviceConfigReceivedAsync == null) return; + + try + { + var deviceSettings = JsonConvert.DeserializeObject(payload ?? ""); + if (deviceSettings == null) + throw new MqttMessageProcessingException("Device settings were null after deserialization", $"espresense/settings/{deviceId}/config", payload, "DeviceConfig"); + + deviceSettings.OriginalId = deviceId; + await DeviceConfigReceivedAsync(new DeviceSettingsEventArgs + { + DeviceId = deviceId, + Payload = deviceSettings + }); + } + catch (JsonException ex) + { + throw new MqttMessageProcessingException( + "Failed to parse device settings", + $"espresense/settings/{deviceId}/config", + payload, + "DeviceConfig", + ex); + } + } + + private async Task ProcessNodeSettingMessage(string nodeId, string setting, string? payload) + { + if (NodeSettingReceivedAsync == null) return; + await NodeSettingReceivedAsync(new NodeSettingReceivedEventArgs + { + NodeId = nodeId, + Setting = setting, + Payload = payload + }); + } + + private async Task ProcessDiscoveryMessage(string topic, string? payload) + { + try + { + _logger.LogTrace($"Received discovery message on topic: {topic}"); + + if (payload == null) + throw new MqttMessageProcessingException("Discovery message payload was null", topic, null, "Discovery"); + + if (!AutoDiscovery.TryDeserialize(topic, payload, out var msg)) + throw new MqttMessageProcessingException("Failed to deserialize discovery message", topic, payload, "Discovery"); + + PreviousDeviceDiscovered?.Invoke(this, new PreviousDeviceDiscoveredEventArgs { AutoDiscover = msg }); + } + catch (Exception ex) when (ex is not MqttMessageProcessingException) + { + throw new MqttMessageProcessingException("Error processing discovery message", topic, payload, "Discovery", ex); + } + } +} \ No newline at end of file diff --git a/src/config.example.yaml b/src/config.example.yaml index 08e961f0..db1b6633 100644 --- a/src/config.example.yaml +++ b/src/config.example.yaml @@ -39,10 +39,24 @@ history: enabled: false # Enable to log history to db (Beta) expire_after: 24h # Expire after 24 hours -weighting: - algorithm: gaussian - props: - sigma: 0.10 +locators: + nadaraya_watson: + enabled: true + floors: ["first", "second", "outside"] + bandwidth: 0.5 + kernel: gaussian + + nealder_mead: + enabled: false + floors: ["first", "second", "outside"] + weighting: + algorithm: gaussian + props: + sigma: 0.10 + + nearest_node: + enabled: true + max_distance: 10 # Floors w/ the points to draw it in meters floors: diff --git a/tests/ConfigTests.cs b/tests/ConfigTests.cs new file mode 100644 index 00000000..f56a5249 --- /dev/null +++ b/tests/ConfigTests.cs @@ -0,0 +1,48 @@ +using System; +using System.IO; +using YamlDotNet.Serialization; +using ESPresense.Models; + +namespace ESPresense.Companion.Tests; + +public class ConfigTests +{ + [Test] + public void TestLocatorsDeserialization() + { + string yaml = @" + locators: + nadaraya_watson: + enabled: true + floors: [""floor1"", ""floor2""] + bandwidth: 0.5 + kernel: ""gaussian"" + nealder_mead: + enabled: false + floors: [""floor3""] + nearest_node: + enabled: true + max_distance: 10.0 + "; + + var deserializer = new DeserializerBuilder().Build(); + var config = deserializer.Deserialize(yaml); + + Assert.NotNull(config); + Assert.NotNull(config.Locators); + + var nadarayaWatson = config.Locators.NadarayaWatson; + Assert.True(nadarayaWatson.Enabled); + Assert.That(nadarayaWatson.Floors, Is.EqualTo(new[] { "floor1", "floor2" })); + Assert.That(nadarayaWatson.Bandwidth, Is.EqualTo(0.5)); + Assert.That(nadarayaWatson.Kernel, Is.EqualTo("gaussian")); + + var nealderMead = config.Locators.NealderMead; + Assert.False(nealderMead.Enabled); + Assert.That(nealderMead.Floors, Is.EqualTo(new[] { "floor3" })); + + var nearestNode = config.Locators.NearestNode; + Assert.True(nearestNode.Enabled); + Assert.That(nearestNode.MaxDistance, Is.EqualTo(10.0)); + } +} \ No newline at end of file