Refactor adding pose entries into separate service

backend/api/Database/Models/Pose.cs

@@ -1,17 +1,13 @@
-using System.ComponentModel.DataAnnotations;
+#nullable disable
+using System.ComponentModel.DataAnnotations;
 using Api.Services.Models;
 using Microsoft.EntityFrameworkCore;
-
-#nullable disable
+using IsarPose = Api.Mqtt.MessageModels.IsarPose;
 namespace Api.Database.Models
 {
     [Owned]
     public class Orientation
     {
-        public float X { get; set; }
-        public float Y { get; set; }
-        public float Z { get; set; }
-        public float W { get; set; }
 
         public Orientation()
         {
@@ -28,11 +24,14 @@ public Orientation(float x = 0, float y = 0, float z = 0, float w = 1)
             Z = z;
             W = w;
         }
+        public float X { get; set; }
+        public float Y { get; set; }
+        public float Z { get; set; }
+        public float W { get; set; }
 
         public override bool Equals(object obj)
         {
-            if (obj is not Orientation)
-                return false;
+            if (obj is not Orientation) { return false; }
             const float Tolerance = 1e-6F;
             var orientation = (Orientation)obj;
             if (MathF.Abs(orientation.X - X) > Tolerance)
@@ -63,9 +62,6 @@ public override int GetHashCode()
     [Owned]
     public class Position
     {
-        public float X { get; set; }
-        public float Y { get; set; }
-        public float Z { get; set; }
 
         public Position()
         {
@@ -80,47 +76,27 @@ public Position(float x = 0, float y = 0, float z = 0)
             Y = y;
             Z = z;
         }
+        public float X { get; set; }
+        public float Y { get; set; }
+        public float Z { get; set; }
     }
 
     [Owned]
     public class Pose
     {
-        [Required]
-        public Position Position { get; set; }
-
-        [Required]
-        public Orientation Orientation { get; set; }
-
-        // Since this is a ground robot the only quaternion vector
-        // that makes sense is up (0, 0, 1)
-        // Echo representes North at 0deg and increases this value clockwise
-        // Our representation has East at 0deg with rotations anti-clockwise
-        public Orientation AxisAngleToQuaternion(float echoAngle)
-        {
-            float angle;
-            echoAngle %= 2F * MathF.PI;
-
-            if (echoAngle < 0) echoAngle += 2F * MathF.PI;
-
-            angle = (450 * MathF.PI / 180) - echoAngle;
-
-            var quaternion = new Orientation()
-            {
-                X = 0,
-                Y = 0,
-                Z = MathF.Sin(angle / 2),
-                W = MathF.Cos(angle / 2)
-            };
-
-            return quaternion;
-        }
 
         public Pose()
         {
             Position = new Position();
             Orientation = new Orientation();
         }
 
+        public Pose(IsarPose isarPose)
+        {
+            Position = new Position(isarPose.Position.X, isarPose.Position.Y, isarPose.Position.Z);
+            Orientation = new Orientation(isarPose.Orientation.X, isarPose.Orientation.Y, isarPose.Orientation.Z, isarPose.Orientation.W);
+        }
+
         public Pose(
             float x_pos,
             float y_pos,
@@ -146,5 +122,34 @@ public Pose(Position position, Orientation orientation)
             Position = position;
             Orientation = orientation;
         }
+        [Required]
+        public Position Position { get; set; }
+
+        [Required]
+        public Orientation Orientation { get; set; }
+
+        // Since this is a ground robot the only quaternion vector
+        // that makes sense is up (0, 0, 1)
+        // Echo representes North at 0deg and increases this value clockwise
+        // Our representation has East at 0deg with rotations anti-clockwise
+        public Orientation AxisAngleToQuaternion(float echoAngle)
+        {
+            float angle;
+            echoAngle %= 2F * MathF.PI;
+
+            if (echoAngle < 0) { echoAngle += 2F * MathF.PI; }
+
+            angle = 450 * MathF.PI / (180 - echoAngle);
+
+            var quaternion = new Orientation
+            {
+                X = 0,
+                Y = 0,
+                Z = MathF.Sin(angle / 2),
+                W = MathF.Cos(angle / 2)
+            };
+
+            return quaternion;
+        }
     }
 }

backend/api/EventHandlers/MqttEventHandler.cs

@@ -227,7 +227,7 @@ private async void OnMissionUpdate(object? sender, MqttReceivedArgs mqttArgs)
             if (flotillaMissionRun.IsCompleted) { robot.CurrentMissionId = null; }
 
             await robotService.Update(robot);
-            _logger.LogInformation("Robot '{Id}' ('{Name}') - completed mission {MissionId}", robot.IsarId, robot.Name, flotillaMissionRun.MissionId);
+            _logger.LogInformation("Robot '{Id}' ('{Name}') - completed mission run {MissionRunId}", robot.IsarId, robot.Name, flotillaMissionRun.Id);
 
             if (!flotillaMissionRun.IsCompleted) { return; }
 
@@ -308,6 +308,7 @@ private async void OnBatteryUpdate(object? sender, MqttReceivedArgs mqttArgs)
             else
             {
                 robot.BatteryLevel = batteryStatus.BatteryLevel;
+                _logger.LogInformation("Calling from OnBatteryUpdate with intended status {Status}", robot.Status);
                 await robotService.Update(robot);
                 await timeseriesService.Create(
                     new RobotBatteryTimeseries
@@ -356,37 +357,12 @@ await timeseriesService.Create(
         private async void OnPoseUpdate(object? sender, MqttReceivedArgs mqttArgs)
         {
             var provider = GetServiceProvider();
-            var robotService = provider.GetRequiredService<IRobotService>();
-            var timeseriesService = provider.GetRequiredService<ITimeseriesService>();
+            var poseTimeseriesService = provider.GetRequiredService<IPoseTimeseriesService>();
 
             var poseStatus = (IsarPoseMessage)mqttArgs.Message;
+            var pose = new Pose(poseStatus.Pose);
 
-            var robot = await robotService.ReadByIsarId(poseStatus.IsarId);
-            if (robot == null)
-            {
-                _logger.LogWarning(
-                    "Could not find corresponding robot for pose update on robot '{RobotName}' with ISAR id '{IsarId}'", poseStatus.RobotName, poseStatus.IsarId);
-            }
-            else
-            {
-                try { poseStatus.Pose.CopyIsarPoseToRobotPose(robot.Pose); }
-                catch (NullReferenceException e)
-                {
-                    _logger.LogWarning(
-                        "NullReferenceException while updating pose on robot '{RobotName}' with ISAR id '{IsarId}': {Message}", robot.Name, robot.IsarId, e.Message);
-                }
-
-                await robotService.Update(robot);
-                await timeseriesService.Create(
-                    new RobotPoseTimeseries(robot.Pose)
-                    {
-                        MissionId = robot.CurrentMissionId,
-                        RobotId = robot.Id,
-                        Time = DateTimeOffset.UtcNow
-                    }
-                );
-                _logger.LogDebug("Updated pose on robot '{RobotName}' with ISAR id '{IsarId}'", robot.Name, robot.IsarId);
-            }
+            await poseTimeseriesService.AddPoseEntry(pose, poseStatus.IsarId);
         }
     }
 }

backend/api/Program.cs

@@ -65,6 +65,7 @@
 builder.Services.AddScoped<IRobotModelService, RobotModelService>();
 builder.Services.AddScoped<IMissionSchedulingService, MissionSchedulingService>();
 builder.Services.AddScoped<ITaskDurationService, TaskDurationService>();
+builder.Services.AddScoped<IPoseTimeseriesService, PoseTimeseriesService>();
 
 bool useInMemoryDatabase = builder.Configuration
     .GetSection("Database")

backend/api/Services/ActionServices/PoseTimeseriesService.cs

@@ -0,0 +1,47 @@
+using Api.Database.Models;
+namespace Api.Services.ActionServices
+{
+    public interface IPoseTimeseriesService
+    {
+        public Task AddPoseEntry(Pose pose, string isarId);
+    }
+
+    public class PoseTimeseriesService : IPoseTimeseriesService
+    {
+        private readonly ILogger<PoseTimeseriesService> _logger;
+        private readonly IRobotService _robotService;
+        private readonly ITimeseriesService _timeseriesService;
+
+        public PoseTimeseriesService(ILogger<PoseTimeseriesService> logger, IRobotService robotService, ITimeseriesService timeseriesService)
+        {
+            _logger = logger;
+            _robotService = robotService;
+            _timeseriesService = timeseriesService;
+        }
+
+        public async Task AddPoseEntry(Pose pose, string isarId)
+        {
+            var robot = await _robotService.ReadByIsarId(isarId);
+            if (robot == null)
+            {
+                _logger.LogWarning(
+                    "Could not find corresponding robot for pose update on robot with ISAR id '{IsarId}'", isarId);
+                return;
+            }
+
+            robot.Pose = pose;
+
+            await _robotService.Update(robot);
+            await _timeseriesService.Create(
+                new RobotPoseTimeseries(robot.Pose)
+                {
+                    MissionId = robot.CurrentMissionId,
+                    RobotId = robot.Id,
+                    Time = DateTimeOffset.UtcNow
+                }
+            );
+            _logger.LogDebug("Updated pose on robot '{RobotName}' with ISAR id '{IsarId}'", robot.Name, robot.IsarId);
+
+        }
+    }
+}