Implement localization as part of a mission

The localization procedure is now implemented and will run as part of
regular mission scheduling.

If the queue is empty a localization mission will be started for the
current deck. This assumes that the operator has confirmed that the
robot is on the deck of the mission that has been scheduled.

If there is an existing mission the system will check if a new mission
is in the same deck as that mission and if so schedule it. If not it
will be rejected.

If the last mission finishes a return to home mission will be scheduled
which puts the robot back at the default localization pose. If a
mission is scheduled in between the return to home mission another
localization will not be required.

backend/api.test/Mocks/CustomMissionServiceMock.cs

@@ -1,11 +1,12 @@
 using System;
 using System.Collections.Generic;
+using System.Linq;
 using System.Security.Cryptography;
 using System.Text;
 using System.Text.Json;
 using System.Threading.Tasks;
+using Api.Controllers.Models;
 using Api.Database.Models;
-
 namespace Api.Services
 {
     public class MockCustomMissionService : ICustomMissionService
@@ -36,16 +37,17 @@ public Task<string> UploadSource(List<MissionTask> tasks)
 
         public string CalculateHashFromTasks(IList<MissionTask> tasks)
         {
-            IList<MissionTask> genericTasks = new List<MissionTask>();
-            foreach (var task in tasks)
-            {
-                var taskCopy = new MissionTask(task);
-                genericTasks.Add(taskCopy);
-            }
+            IList<MissionTask> genericTasks = tasks.Select(task => new MissionTask(task)).ToList();
 
             string json = JsonSerializer.Serialize(genericTasks);
             byte[] hash = SHA256.HashData(Encoding.UTF8.GetBytes(json));
             return BitConverter.ToString(hash).Replace("-", "", StringComparison.CurrentCulture).ToUpperInvariant();
         }
+
+        public async Task<MissionRun> QueueCustomMissionRun(CustomMissionQuery customMissionQuery, MissionDefinition customMissionDefinition, Robot robot, IList<MissionTask> missionTasks)
+        {
+            await Task.CompletedTask;
+            return new MissionRun();
+        }
     }
 }

backend/api/Controllers/MissionSchedulingController.cs

@@ -16,12 +16,14 @@ public class MissionSchedulingController : ControllerBase
         private readonly IInstallationService _installationService;
         private readonly ICustomMissionService _customMissionService;
         private readonly IEchoService _echoService;
+        private readonly ILocalizationService _localizationService;
         private readonly ILogger<MissionSchedulingController> _logger;
         private readonly IMapService _mapService;
         private readonly IMissionDefinitionService _missionDefinitionService;
         private readonly IMissionRunService _missionRunService;
         private readonly ICustomMissionSchedulingService _customMissionSchedulingService;
         private readonly IRobotService _robotService;
+        private readonly Mutex _scheduleLocalizationMutex = new();
         private readonly ISourceService _sourceService;
         private readonly IStidService _stidService;
 
@@ -36,6 +38,7 @@ public MissionSchedulingController(
             ILogger<MissionSchedulingController> logger,
             IMapService mapService,
             IStidService stidService,
+            ILocalizationService localizationService,
             ISourceService sourceService,
             ICustomMissionSchedulingService customMissionSchedulingService
         )
@@ -49,6 +52,7 @@ ICustomMissionSchedulingService customMissionSchedulingService
             _customMissionService = customMissionService;
             _mapService = mapService;
             _stidService = stidService;
+            _localizationService = localizationService;
             _sourceService = sourceService;
             _missionDefinitionService = missionDefinitionService;
             _customMissionSchedulingService = customMissionSchedulingService;
@@ -74,19 +78,25 @@ [FromBody] ScheduleMissionQuery scheduledMissionQuery
         )
         {
             var robot = await _robotService.ReadById(scheduledMissionQuery.RobotId);
-            if (robot is null)
-            {
-                return NotFound($"Could not find robot with id {scheduledMissionQuery.RobotId}");
-            }
+            if (robot is null) { return NotFound($"Could not find robot with id {scheduledMissionQuery.RobotId}"); }
 
             var missionDefinition = await _missionDefinitionService.ReadById(scheduledMissionQuery.MissionDefinitionId);
-            if (missionDefinition == null)
-            {
-                return NotFound("Mission definition not found");
-            }
+            if (missionDefinition == null) { return NotFound("Mission definition not found"); }
+
+            try { await _localizationService.EnsureRobotIsOnSameInstallationAsMission(robot, missionDefinition); }
+            catch (InstallationNotFoundException e) { return NotFound(e.Message); }
+            catch (MissionException e) { return Conflict(e.Message); }
+
+            var missionTasks = await _missionDefinitionService.GetTasksFromSource(missionDefinition.Source, missionDefinition.InstallationCode);
 
-            List<MissionTask>? missionTasks;
-            missionTasks = await _missionDefinitionService.GetTasksFromSource(missionDefinition.Source, missionDefinition.InstallationCode);
+            _scheduleLocalizationMutex.WaitOne();
+
+            try { await _localizationService.EnsureRobotIsCorrectlyLocalized(robot, missionDefinition); }
+            catch (Exception e) when (e is AreaNotFoundException or DeckNotFoundException) { return NotFound(e.Message); }
+            catch (Exception e) when (e is RobotNotAvailableException or RobotLocalizationException) { return Conflict(e.Message); }
+            catch (IsarCommunicationException e) { return StatusCode(StatusCodes.Status502BadGateway, e.Message); }
+
+            finally { _scheduleLocalizationMutex.ReleaseMutex(); }
 
             if (missionTasks == null)
             {
@@ -104,7 +114,7 @@ [FromBody] ScheduleMissionQuery scheduledMissionQuery
                 Tasks = missionTasks,
                 InstallationCode = missionDefinition.InstallationCode,
                 Area = missionDefinition.Area,
-                Map = new MapMetadata()
+                Map = missionDefinition.Area?.MapMetadata ?? new MapMetadata()
             };
 
             await _mapService.AssignMapToMission(missionRun);
@@ -291,6 +301,19 @@ [FromBody] CustomMissionQuery customMissionQuery
             try { customMissionDefinition = await _customMissionSchedulingService.FindExistingOrCreateCustomMissionDefinition(customMissionQuery, missionTasks); }
             catch (SourceException e) { return StatusCode(StatusCodes.Status502BadGateway, e.Message); }
 
+            try { await _localizationService.EnsureRobotIsOnSameInstallationAsMission(robot, customMissionDefinition); }
+            catch (InstallationNotFoundException e) { return NotFound(e.Message); }
+            catch (MissionException e) { return Conflict(e.Message); }
+
+            _scheduleLocalizationMutex.WaitOne();
+
+            try { await _localizationService.EnsureRobotIsCorrectlyLocalized(robot, customMissionDefinition); }
+            catch (Exception e) when (e is AreaNotFoundException or DeckNotFoundException) { return NotFound(e.Message); }
+            catch (Exception e) when (e is RobotNotAvailableException or RobotLocalizationException) { return Conflict(e.Message); }
+            catch (IsarCommunicationException e) { return StatusCode(StatusCodes.Status502BadGateway, e.Message); }
+
+            finally { _scheduleLocalizationMutex.ReleaseMutex(); }
+
             MissionRun? newMissionRun;
             try { newMissionRun = await _customMissionSchedulingService.QueueCustomMissionRun(customMissionQuery, customMissionDefinition.Id, robot.Id, missionTasks); }
             catch (Exception e) when (e is RobotNotFoundException or MissionNotFoundException) { return NotFound(e.Message); }

backend/api/Database/Context/InitDb.cs

@@ -39,7 +39,8 @@ public static class InitDb
                 TagId = "Tagid here",
                 EchoTagLink = new Uri("https://www.I-am-echo-stid-tag-url.com"),
                 InspectionTarget = new Position(),
-                RobotPose = new Pose()
+                RobotPose = new Pose(),
+                Type = "inspection"
             };
 
         private static List<Installation> GetInstallations()
@@ -51,9 +52,16 @@ private static List<Installation> GetInstallations()
                 InstallationCode = "JSV"
             };
 
+            var installation2 = new Installation
+            {
+                Id = Guid.NewGuid().ToString(),
+                Name = "Kårstø",
+                InstallationCode = "KAA"
+            };
+
             return new List<Installation>(new[]
             {
-                installation1
+                installation1, installation2
             });
         }
 
@@ -67,9 +75,17 @@ private static List<Plant> GetPlants()
                 PlantCode = "P1"
             };
 
+            var plant2 = new Plant
+            {
+                Id = Guid.NewGuid().ToString(),
+                Installation = installations[0],
+                Name = "Kårstø",
+                PlantCode = "Kårstø"
+            };
+
             return new List<Plant>(new[]
             {
-                plant1
+                plant1, plant2
             });
         }
 
@@ -152,10 +168,7 @@ private static List<Area> GetAreas()
                 Name = "testArea",
                 MapMetadata = new MapMetadata(),
                 DefaultLocalizationPose = new DefaultLocalizationPose(),
-                SafePositions = new List<SafePosition>
-                {
-                    new()
-                }
+                SafePositions = new List<SafePosition>()
             };
 
             var area4 = new Area
@@ -437,13 +450,7 @@ private static List<MissionTask> GetMissionTasks()
                 Status = TaskStatus.Cancelled
             };
 
-            return new List<MissionTask>
-            {
-                task1,
-                task2,
-                task3,
-                task4
-            };
+            return new List<MissionTask> { task1, task2, task3, task4 };
         }
 
         private static List<MissionRun> GetMissionRuns()
@@ -606,10 +613,7 @@ public static void PopulateDb(FlotillaDbContext context)
                 var task = ExampleTask;
                 task.Inspections.Add(Inspection);
                 task.Inspections.Add(Inspection2);
-                var tasks = new List<MissionTask>
-                {
-                    task
-                };
+                var tasks = new List<MissionTask> { task };
                 missionRun.Tasks = tasks;
             }
             context.AddRange(robots);
@@ -619,6 +623,7 @@ public static void PopulateDb(FlotillaDbContext context)
             context.AddRange(plants);
             context.AddRange(decks);
             context.AddRange(areas);
+
             context.SaveChanges();
         }
     }

backend/api/Database/Models/MissionRun.cs

@@ -139,7 +139,7 @@ public void CalculateEstimatedDuration()
                     task => task.Inspections.Sum(inspection => inspection.VideoDuration ?? 0)
                 );
                 EstimatedDuration = (uint)(
-                    (Robot.Model.AverageDurationPerTag * Tasks.Count) + totalInspectionDuration
+                    Robot.Model.AverageDurationPerTag * Tasks.Count + totalInspectionDuration
                 );
             }
             else
@@ -188,6 +188,24 @@ var inspection in task.Inspections.Where(inspection => !inspection.IsCompleted)
                 }
             }
         }
+
+        public bool IsLocalizationMission()
+        {
+            if (Tasks.Count != 1)
+            {
+                return false;
+            }
+            return Tasks[0].Type == "localization";
+        }
+
+        public bool IsDriveToMission()
+        {
+            if (Tasks.Count != 1)
+            {
+                return false;
+            }
+            return Tasks[0].Type == "drive_to";
+        }
     }
 
     public enum MissionStatus

backend/api/Database/Models/MissionTask.cs

@@ -26,6 +26,7 @@ public MissionTask(EchoTag echoTag, Position tagPosition)
             EchoPoseId = echoTag.PoseId;
             TaskOrder = echoTag.PlanOrder;
             Status = TaskStatus.NotStarted;
+            Type = "inspection";
         }
 
         // ReSharper disable once NotNullOrRequiredMemberIsNotInitialized
@@ -40,6 +41,32 @@ public MissionTask(CustomTaskQuery taskQuery)
             RobotPose = taskQuery.RobotPose;
             TaskOrder = taskQuery.TaskOrder;
             Status = TaskStatus.NotStarted;
+            Type = "inspection";
+        }
+
+        public MissionTask(Pose robotPose, string type)
+        {
+            switch (type)
+            {
+                case "localization":
+                    Type = type;
+                    Description = "Localization";
+                    RobotPose = robotPose;
+                    TaskOrder = 0;
+                    Status = TaskStatus.NotStarted;
+                    InspectionTarget = new Position();
+                    Inspections = new List<Inspection>();
+                    break;
+                case "drive_to":
+                    Type = type;
+                    Description = "Return to home";
+                    RobotPose = robotPose;
+                    TaskOrder = 0;
+                    Status = TaskStatus.NotStarted;
+                    InspectionTarget = new Position();
+                    Inspections = new List<Inspection>();
+                    break;
+            }
         }
 
         // Creates a blank deepcopy of the provided task
@@ -68,6 +95,8 @@ public MissionTask(MissionTask copy)
         [Required]
         public int TaskOrder { get; set; }
 
+        public string Type { get; set; }
+
         [MaxLength(200)]
         public string? TagId { get; set; }
 

backend/api/EventHandlers/MissionEventHandler.cs

@@ -23,12 +23,15 @@ IServiceScopeFactory scopeFactory
 
             Subscribe();
         }
-
         private IMissionRunService MissionService =>
             _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IMissionRunService>();
 
         private IRobotService RobotService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IRobotService>();
 
+        private IReturnToHomeService ReturnToHomeService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IReturnToHomeService>();
+
+        private ILocalizationService LocalizationService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<ILocalizationService>();
+
         private IAreaService AreaService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IAreaService>();
 
         private IMissionSchedulingService MissionScheduling => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IMissionSchedulingService>();
@@ -66,6 +69,12 @@ private async void OnMissionRunCreated(object? sender, MissionRunCreatedEventArg
                 return;
             }
 
+            if (!await LocalizationService.RobotIsLocalized(missionRun.Robot.Id) && !missionRun.IsLocalizationMission())
+            {
+                _logger.LogWarning("A mission run was created while the robot was not localized and it will be put on the queue awaiting localization");
+                return;
+            }
+
             if (MissionScheduling.MissionRunQueueIsEmpty(await MissionService.ReadMissionRunQueue(missionRun.Robot.Id)))
             {
                 _logger.LogInformation("Mission run {MissionRunId} was not started as there are no mission runs on the queue", e.MissionRunId);
@@ -87,9 +96,34 @@ private async void OnRobotAvailable(object? sender, RobotAvailableEventArgs e)
                 return;
             }
 
+            if (!await LocalizationService.RobotIsLocalized(robot.Id))
+            {
+                try { await LocalizationService.EnsureRobotWasCorrectlyLocalizedInPreviousMissionRun(robot.Id); }
+                catch (Exception ex) when (ex is LocalizationFailedException or RobotNotFoundException or MissionNotFoundException or MissionException or TimeoutException)
+                {
+                    _logger.LogError("Could not confirm that the robot was correctly localized and the scheduled missions for the deck will be cancelled");
+                    // Cancel missions
+                    // Raise localization mission failed event?
+                }
+            }
+
             if (MissionScheduling.MissionRunQueueIsEmpty(await MissionService.ReadMissionRunQueue(robot.Id)))
             {
                 _logger.LogInformation("The robot was changed to available but there are no mission runs in the queue to be scheduled");
+
+                var lastExecutedMissionRun = await MissionService.ReadLastExecutedMissionRunByRobot(robot.Id);
+                if (lastExecutedMissionRun is null)
+                {
+                    _logger.LogError("Could not find last executed mission run for robot");
+                    return;
+                }
+
+                if (!lastExecutedMissionRun.IsDriveToMission())
+                {
+                    try { await ReturnToHomeService.ScheduleReturnToHomeMissionRun(robot.Id); }
+                    catch (Exception ex) when (ex is RobotNotFoundException or AreaNotFoundException or DeckNotFoundException or PoseNotFoundException) { return; }
+                }
+                else { await RobotService.UpdateCurrentArea(robot.Id, null); }
                 return;
             }