Skip to content

Commit

Permalink
Improve localization when safe zone button pressed
Browse files Browse the repository at this point in the history
  • Loading branch information
@mrica-equinor
mrica-equinor committed Mar 22, 2024
1 parent 258a243 commit 52c6bb7
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 32 deletions.
47 changes: 18 additions & 29 deletions backend/api/EventHandlers/MissionEventHandler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,17 +159,17 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
try { await MissionScheduling.FreezeMissionRunQueueForRobot(e.RobotId); }
catch (RobotNotFoundException) { return; }

var area = await FindRobotArea(robot.Id);
var area = await FindRelevantRobotAreaForSafePositionMission(robot.Id);
if (area == null) { return; }

try { await MissionScheduling.ScheduleMissionToReturnToSafePosition(e.RobotId, area.Id); }
try { await MissionScheduling.ScheduleMissionToDriveToSafePosition(e.RobotId, area.Id); }
catch (SafeZoneException ex)
{
_logger.LogError(ex, "Failed to schedule return to safe zone mission on robot {RobotName} because: {ErrorMessage}", robot.Name, ex.Message);
SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Failed to send {robot.Name} to a safe zone");
}

if (await MissionService.OngoingLocalizationMissionRunExists(e.RobotId)) { return; }
if (await MissionService.PendingOrOngoingLocalizationMissionRunExists(e.RobotId)) { return; }
try { await MissionScheduling.StopCurrentMissionRun(e.RobotId); }
catch (RobotNotFoundException) { return; }
catch (MissionRunNotFoundException)
Expand All @@ -194,7 +194,10 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
return;
}

MissionScheduling.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id));
_startMissionSemaphore.WaitOne();
try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); }
catch (MissionRunNotFoundException) { return; }
finally { _startMissionSemaphore.Release(); }
}

private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyButtonPressedForRobotEventArgs e)
Expand All @@ -210,29 +213,13 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB
try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); }
catch (RobotNotFoundException) { return; }

if (!await LocalizationService.RobotIsLocalized(robot.Id))
{
_logger.LogError($"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised.");
SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised.");
return;
}

var area = await AreaService.ReadById(robot.CurrentArea!.Id);
if (area == null)
{
_logger.LogError("Could not find area with ID {AreaId}", robot.CurrentArea!.Id);
SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised");
}

if (await MissionScheduling.OngoingMission(robot.Id))
{
_logger.LogInformation("Robot {RobotName} was unfrozen but the mission to return to safe zone will be completed before further missions are started", robot.Id);
}

MissionScheduling.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id));
_startMissionSemaphore.WaitOne();
try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); }
catch (MissionRunNotFoundException) { return; }
finally { _startMissionSemaphore.Release(); }
}

private async Task<Area?> FindRobotArea(string robotId)
private async Task<Area?> FindRelevantRobotAreaForSafePositionMission(string robotId)
{
var robot = await RobotService.ReadById(robotId);
if (robot == null)
Expand All @@ -244,18 +231,20 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB
if (!await LocalizationService.RobotIsLocalized(robotId))
{

if (await MissionService.OngoingLocalizationMissionRunExists(robotId))
if (await MissionService.PendingOrOngoingLocalizationMissionRunExists(robotId))
{
var localizationMission = await MissionService.ReadAll(
var missionRuns = await MissionService.ReadAll(
new MissionRunQueryStringParameters
{
Statuses = [MissionStatus.Ongoing],
Statuses = [MissionStatus.Ongoing, MissionStatus.Pending],
RobotId = robot.Id,
OrderBy = "DesiredStartTime",
PageSize = 100
});

return localizationMission.FirstOrDefault()?.Area ?? null;
var localizationMission = missionRuns.Find(missionRun => missionRun.IsLocalizationMission());

return localizationMission?.Area ?? null;
}

_logger.LogError("Robot {RobotName} is not localized and no localization mission is ongoing.", robot.Name);
Expand Down
20 changes: 20 additions & 0 deletions backend/api/Services/MissionRunService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ public interface IMissionRunService

public Task<bool> OngoingLocalizationMissionRunExists(string robotId);

public Task<bool> PendingOrOngoingLocalizationMissionRunExists(string robotId);

public Task<bool> PendingOrOngoingReturnToHomeMissionRunExists(string robotId);

public Task<MissionRun> Update(MissionRun mission);
Expand Down Expand Up @@ -185,6 +187,24 @@ public async Task<bool> OngoingLocalizationMissionRunExists(string robotId)
return false;
}

public async Task<bool> PendingOrOngoingLocalizationMissionRunExists(string robotId)
{
var pendingMissionRuns = await ReadMissionRunQueue(robotId);
foreach (var pendingMissionRun in pendingMissionRuns)
{
if (pendingMissionRun.IsLocalizationMission()) { return true; }
}
var ongoingMissionRuns = await GetMissionRunsWithSubModels()
.Where(missionRun => missionRun.Robot.Id == robotId && missionRun.Status == MissionStatus.Ongoing)
.OrderBy(missionRun => missionRun.DesiredStartTime)
.ToListAsync();
foreach (var ongoingMissionRun in ongoingMissionRuns)
{
if (ongoingMissionRun.IsLocalizationMission()) { return true; }
}
return false;
}

public async Task<bool> PendingOrOngoingReturnToHomeMissionRunExists(string robotId)
{
var pendingMissionRuns = await ReadMissionRunQueue(robotId);
Expand Down
6 changes: 3 additions & 3 deletions backend/api/Services/MissionSchedulingService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ public interface IMissionSchedulingService

public Task AbortAllScheduledMissions(string robotId, string? abortReason = null);

public Task ScheduleMissionToReturnToSafePosition(string robotId, string areaId);
public Task ScheduleMissionToDriveToSafePosition(string robotId, string areaId);

public Task UnfreezeMissionRunQueueForRobot(string robotId);

Expand Down Expand Up @@ -212,7 +212,7 @@ public async Task AbortAllScheduledMissions(string robotId, string? abortReason)
}
}

public async Task ScheduleMissionToReturnToSafePosition(string robotId, string areaId)
public async Task ScheduleMissionToDriveToSafePosition(string robotId, string areaId)
{
var area = await areaService.ReadById(areaId);
if (area == null)
Expand Down Expand Up @@ -443,7 +443,7 @@ private async Task<bool> TheSystemIsAvailableToRunAMission(string robotId, strin
throw new MissionRunNotFoundException(errorMessage);
}

if (robot.MissionQueueFrozen && missionRun.MissionRunPriority != MissionRunPriority.Emergency)
if (robot.MissionQueueFrozen && missionRun.MissionRunPriority != MissionRunPriority.Emergency && missionRun.MissionRunPriority != MissionRunPriority.Localization)
{
logger.LogInformation("Mission run {MissionRunId} was not started as the mission run queue for robot {RobotName} is frozen", missionRun.Id, robot.Name);
return false;
Expand Down

0 comments on commit 52c6bb7

Please sign in to comment.