ADIS_Projects/ADIS_Csharp/RobotLib/Battery/DevBattery.cs

using RobotLib.Communication;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text.Json;
using System.Threading.Tasks;

namespace RobotLib.Battery
{
    public class DevBattery : DevBase
    {
        private float voltage;
        private int lastResponseS = 1000;  // seconds since last response
        private const int offlineTresholdS = 11;   // treshold when to assess a robot as offline (no response for x sec)
        private Stopwatch stopwatchLastResponse = new();

        private const string TOPIC_ROBO_REQ_BATTERY = "/both/cmd/battery/get_volt";
        private const string TOPIC_ROBO_RESP_BATTERY = "/mobile/state/battery/voltage";
        private const string TOPIC_STAT_RESP_BATTERY = "/stationary/state/battery/voltage";

        public event EventHandler<BatteryEventArgs> BatteryChanged;
        public event EventHandler<LastResponseUpdateEventArgs> LastResponseUpdate;

        public float Voltage
        {
            get { return voltage; }
            private set
            {
                // value changed?
                if (voltage != value)
                {
                    // set new voltage and raise event
                    voltage = value;
                    BatteryChanged?.Invoke(this, new BatteryEventArgs(voltage));
                }
            }
        }

        public int SecondsSinceLastResponse
        {
            get { return lastResponseS; }
            private set
            {
                var lastVal = lastResponseS; // last stored value
                lastResponseS = value;   // update stored value
                // new value is smaller than online time and old value is larger or equal => newly connected
                if (value < offlineTresholdS && lastVal >= offlineTresholdS) 
                {
                    LastResponseUpdate?.Invoke(this, new LastResponseUpdateEventArgs(true, value));
                }
                // old value is larger than online time and new value is smaller => newly disconnected
                else if(value >= offlineTresholdS && lastVal < offlineTresholdS)
                {
                    Task.Run(() => this.Voltage = float.NaN);  // set voltage to NaN, now unknown
                    LastResponseUpdate?.Invoke(this, new LastResponseUpdateEventArgs(false, value));
                }
            }
        }

        public DevBattery(IPublisherSubscriber com, RobotMode mode) : base(com, GenerateListOfRelevantTopics(mode)) { }

        private static List<string> GenerateListOfRelevantTopics(RobotMode mode)
        {
            if (mode == RobotMode.Stationary)
            {
                return new List<string>() { TOPIC_STAT_RESP_BATTERY };
            }
            else
            {
                return new List<string>() { TOPIC_ROBO_RESP_BATTERY };
            }
        }

        public void UpdateLastResponse()
        {
            if (this.stopwatchLastResponse.IsRunning)
            {
                this.SecondsSinceLastResponse = this.stopwatchLastResponse.Elapsed.Seconds;
            }
        }

        public void RequestBatteryVoltage()
        {
            base.SendMessage(TOPIC_ROBO_REQ_BATTERY, true.ToString());
            this.UpdateLastResponse();
        }

        protected override void ParseMessage(string fromTopic, string message)
        {
            if (fromTopic == TOPIC_ROBO_RESP_BATTERY || fromTopic == TOPIC_STAT_RESP_BATTERY)
            {
                this.stopwatchLastResponse.Restart();
                this.SecondsSinceLastResponse = 0;

                var parsedString = GetValueFromMesage<string>("voltage", message);
                if (parsedString == null) parsedString = "?";

                // example message = "Battery: 1.25 V"
                var valueUnit = parsedString.Trim().Split(' ');
                string voltage = valueUnit[0].Trim();
                float fVoltage = float.NaN;

                if(float.TryParse(voltage, out fVoltage))
                {
                    this.Voltage = fVoltage;
                }
            }
        }

        private T GetValueFromMesage<T>(string parameter, string message)
        {
            var data = JsonSerializer.Deserialize<Dictionary<string, T>>(message);
            data.TryGetValue(parameter, out T value);
            return value;
        }
    }
}