From bb60026c16cf5549615ea8343876dec5c3af2ebc Mon Sep 17 00:00:00 2001 From: andchiind Date: Mon, 17 Jun 2024 13:22:08 +0200 Subject: [PATCH 1/2] Only send start_pose on first mission in queue --- backend/api.test/Mocks/IsarServiceMock.cs | 2 +- backend/api/EventHandlers/MissionEventHandler.cs | 11 +++++++---- backend/api/Services/IsarService.cs | 6 +++--- backend/api/Services/MissionSchedulingService.cs | 10 +++++----- backend/api/Services/Models/IsarMissionDefinition.cs | 4 ++-- 5 files changed, 18 insertions(+), 15 deletions(-) diff --git a/backend/api.test/Mocks/IsarServiceMock.cs b/backend/api.test/Mocks/IsarServiceMock.cs index 2ad1bd4e0..89b27c821 100644 --- a/backend/api.test/Mocks/IsarServiceMock.cs +++ b/backend/api.test/Mocks/IsarServiceMock.cs @@ -9,7 +9,7 @@ namespace Api.Test.Mocks { public class MockIsarService : IIsarService { - public async Task StartMission(Robot robot, MissionRun mission) + public async Task StartMission(Robot robot, MissionRun mission, bool isFirstMissionInQueue = false) { await Task.Run(() => Thread.Sleep(1)); var isarServiceMissionResponse = new IsarMission( diff --git a/backend/api/EventHandlers/MissionEventHandler.cs b/backend/api/EventHandlers/MissionEventHandler.cs index 9dabc773f..5b56b14a5 100644 --- a/backend/api/EventHandlers/MissionEventHandler.cs +++ b/backend/api/EventHandlers/MissionEventHandler.cs @@ -72,9 +72,11 @@ private async void OnMissionRunCreated(object? sender, MissionRunCreatedEventArg return; } + bool isFirstMissionInQueue = false; if (!await LocalizationService.RobotIsLocalized(missionRun.Robot.Id)) { + isFirstMissionInQueue = true; if (missionRun.Robot.RobotCapabilities != null && !missionRun.Robot.RobotCapabilities.Contains(RobotCapabilitiesEnum.localize)) { await RobotService.UpdateCurrentArea(missionRun.Robot.Id, missionRun.Area); @@ -122,7 +124,7 @@ or IsarCommunicationException await CancelReturnToHomeOnNewMissionSchedule(missionRun); _startMissionSemaphore.WaitOne(); - try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(missionRun.Robot.Id); } + try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(missionRun.Robot.Id, isFirstMissionInQueue: isFirstMissionInQueue); } catch (MissionRunNotFoundException) { return; } finally { _startMissionSemaphore.Release(); } } @@ -139,7 +141,7 @@ private async void OnRobotAvailable(object? sender, RobotAvailableEventArgs e) if (robot.CurrentMissionId != null) { - var stuckMission = await MissionService.ReadById(robot.CurrentMissionId); + var stuckMission = await MissionService.ReadById(robot.CurrentMissionId!); if (stuckMission == null) { _logger.LogError("MissionRun with ID: {MissionId} was not found in the database", robot.CurrentMissionId); @@ -153,8 +155,9 @@ private async void OnRobotAvailable(object? sender, RobotAvailableEventArgs e) } } + bool isFirstMissionInQueue = robot.CurrentArea == null; _startMissionSemaphore.WaitOne(); - try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); } + try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id, isFirstMissionInQueue: isFirstMissionInQueue); } catch (MissionRunNotFoundException) { return; } finally { _startMissionSemaphore.Release(); } } @@ -267,7 +270,7 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB catch (RobotNotFoundException) { return; } _startMissionSemaphore.WaitOne(); - try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); } + try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id, isFirstMissionInQueue: robot.CurrentArea == null); } catch (MissionRunNotFoundException) { return; } finally { _startMissionSemaphore.Release(); } } diff --git a/backend/api/Services/IsarService.cs b/backend/api/Services/IsarService.cs index b91678943..a5027e55a 100644 --- a/backend/api/Services/IsarService.cs +++ b/backend/api/Services/IsarService.cs @@ -8,7 +8,7 @@ namespace Api.Services { public interface IIsarService { - public Task StartMission(Robot robot, MissionRun missionRun); + public Task StartMission(Robot robot, MissionRun missionRun, bool isFirstMissionInQueue = false); public Task StopMission(Robot robot); @@ -23,7 +23,7 @@ public class IsarService(IDownstreamApi isarApi, ILogger logger) : { public const string ServiceName = "IsarApi"; - public async Task StartMission(Robot robot, MissionRun missionRun) + public async Task StartMission(Robot robot, MissionRun missionRun, bool isFirstMissionInQueue = false) { var response = await CallApi( HttpMethod.Post, @@ -31,7 +31,7 @@ public async Task StartMission(Robot robot, MissionRun missionRun) "schedule/start-mission", new { - mission_definition = new IsarMissionDefinition(missionRun) + mission_definition = new IsarMissionDefinition(missionRun, includeStartPose: isFirstMissionInQueue) } ); diff --git a/backend/api/Services/MissionSchedulingService.cs b/backend/api/Services/MissionSchedulingService.cs index 57fb4aaa2..1d3a52a9b 100644 --- a/backend/api/Services/MissionSchedulingService.cs +++ b/backend/api/Services/MissionSchedulingService.cs @@ -8,7 +8,7 @@ namespace Api.Services { public interface IMissionSchedulingService { - public Task StartNextMissionRunIfSystemIsAvailable(string robotId); + public Task StartNextMissionRunIfSystemIsAvailable(string robotId, bool isFirstMissionInQueue = false); public Task OngoingMission(string robotId); @@ -33,7 +33,7 @@ public interface IMissionSchedulingService public class MissionSchedulingService(ILogger logger, IMissionRunService missionRunService, IRobotService robotService, IAreaService areaService, IIsarService isarService, ILocalizationService localizationService, IReturnToHomeService returnToHomeService, ISignalRService signalRService) : IMissionSchedulingService { - public async Task StartNextMissionRunIfSystemIsAvailable(string robotId) + public async Task StartNextMissionRunIfSystemIsAvailable(string robotId, bool isFirstMissionInQueue = false) { logger.LogInformation("Starting next mission run if system is available for robot ID: {RobotId}", robotId); var robot = await robotService.ReadById(robotId); @@ -109,7 +109,7 @@ public async Task StartNextMissionRunIfSystemIsAvailable(string robotId) if (missionRun == null) { return; } } - try { await StartMissionRun(missionRun); } + try { await StartMissionRun(missionRun, isFirstMissionInQueue: isFirstMissionInQueue); } catch (Exception ex) when ( ex is MissionException or RobotNotFoundException @@ -373,7 +373,7 @@ private async Task MoveInterruptedMissionsToQueue(IEnumerable interrupte } } - private async Task StartMissionRun(MissionRun queuedMissionRun) + private async Task StartMissionRun(MissionRun queuedMissionRun, bool isFirstMissionInQueue = false) { string robotId = queuedMissionRun.Robot.Id; string missionRunId = queuedMissionRun.Id; @@ -409,7 +409,7 @@ private async Task StartMissionRun(MissionRun queuedMissionRun) } IsarMission isarMission; - try { isarMission = await isarService.StartMission(robot, missionRun); } + try { isarMission = await isarService.StartMission(robot, missionRun, isFirstMissionInQueue: isFirstMissionInQueue); } catch (HttpRequestException e) { string errorMessage = $"Could not reach ISAR at {robot.IsarUri}"; diff --git a/backend/api/Services/Models/IsarMissionDefinition.cs b/backend/api/Services/Models/IsarMissionDefinition.cs index ef81a5c8d..6cf12beca 100644 --- a/backend/api/Services/Models/IsarMissionDefinition.cs +++ b/backend/api/Services/Models/IsarMissionDefinition.cs @@ -27,12 +27,12 @@ public IsarMissionDefinition(List tasks) Tasks = tasks; } - public IsarMissionDefinition(MissionRun missionRun) + public IsarMissionDefinition(MissionRun missionRun, bool includeStartPose = false) { Id = missionRun.IsarMissionId; Name = missionRun.Name; Tasks = missionRun.Tasks.Select(task => new IsarTaskDefinition(task, missionRun)).ToList(); - StartPose = missionRun.Area.DefaultLocalizationPose != null ? new IsarPose(missionRun.Area.DefaultLocalizationPose.Pose) : null; + StartPose = includeStartPose && missionRun.Area.DefaultLocalizationPose != null ? new IsarPose(missionRun.Area.DefaultLocalizationPose.Pose) : null; } } From 09096414914cda2bdc156986e2e146d820bd6407 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= Date: Wed, 19 Jun 2024 12:17:11 +0200 Subject: [PATCH 2/2] Add more delocalisation situations --- .../EventHandlers/IsarConnectionEventHandler.cs | 1 + backend/api/EventHandlers/MqttEventHandler.cs | 14 ++++++++++++++ 2 files changed, 15 insertions(+) diff --git a/backend/api/EventHandlers/IsarConnectionEventHandler.cs b/backend/api/EventHandlers/IsarConnectionEventHandler.cs index 0d72cffc2..9c49c48b7 100644 --- a/backend/api/EventHandlers/IsarConnectionEventHandler.cs +++ b/backend/api/EventHandlers/IsarConnectionEventHandler.cs @@ -150,6 +150,7 @@ private async void OnTimeoutEvent(IsarRobotHeartbeatMessage robotHeartbeatMessag { await RobotService.UpdateRobotIsarConnected(robot.Id, false); await RobotService.UpdateCurrentMissionId(robot.Id, null); + await RobotService.UpdateCurrentArea(robot.Id, null); } catch (RobotNotFoundException) { return; } } diff --git a/backend/api/EventHandlers/MqttEventHandler.cs b/backend/api/EventHandlers/MqttEventHandler.cs index 30d21e055..189409f59 100644 --- a/backend/api/EventHandlers/MqttEventHandler.cs +++ b/backend/api/EventHandlers/MqttEventHandler.cs @@ -86,6 +86,7 @@ private async void OnIsarStatus(object? sender, MqttReceivedArgs mqttArgs) _logger.LogInformation("Updated status for robot {Name} to {Status}", robot.Name, isarStatus.Status); if (isarStatus.Status == RobotStatus.Available) missionSchedulingService.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id)); + else if (isarStatus.Status == RobotStatus.Offline) await robotService.UpdateCurrentArea(robot.Id, null); } private async void OnIsarRobotInfo(object? sender, MqttReceivedArgs mqttArgs) @@ -290,6 +291,19 @@ private async void OnIsarMissionUpdate(object? sender, MqttReceivedArgs mqttArgs } } + if (flotillaMissionRun.IsReturnHomeMission() && (flotillaMissionRun.Status == MissionStatus.Cancelled || flotillaMissionRun.Status == MissionStatus.Failed)) + { + try + { + await robotService.UpdateCurrentArea(robot.Id, null); + } + catch (RobotNotFoundException) + { + _logger.LogError("Could not find robot '{RobotName}' with ID '{Id}'", robot.Name, robot.Id); + return; + } + } + try { await robotService.UpdateCurrentMissionId(robot.Id, null); } catch (RobotNotFoundException) {