Skip to content

Commit

Permalink
Evaluate mission run status before updating
Browse files Browse the repository at this point in the history
  • Loading branch information
@oysand
oysand committed Jun 21, 2024
1 parent 659d905 commit e6a3a1f
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 96 deletions.
43 changes: 43 additions & 0 deletions backend/api.test/EventHandlers/TestMissionEventHandler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -324,6 +324,49 @@ public async void QueuedMissionsAreAbortedWhenLocalizationFails()
Assert.Equal(MissionStatus.Aborted, postTestMissionRun!.Status);
}

[Fact]
public async void QueuedMissionsAreNotAbortedWhenRobotAvailableHappensAtTheSameTimeAsOnIsarMissionCompleted()
{
// Arrange
var installation = await _databaseUtilities.NewInstallation();
var plant = await _databaseUtilities.NewPlant(installation.InstallationCode);
var deck = await _databaseUtilities.NewDeck(installation.InstallationCode, plant.PlantCode);
var area = await _databaseUtilities.NewArea(installation.InstallationCode, plant.PlantCode, deck.Name);
var robot = await _databaseUtilities.NewRobot(RobotStatus.Available, installation, null);
var missionRun1 = await _databaseUtilities.NewMissionRun(installation.InstallationCode, robot, area, true);
var missionRun2 = await _databaseUtilities.NewMissionRun(installation.InstallationCode, robot, area, true);

Thread.Sleep(100);

var missionRunCreatedEventArgs = new MissionRunCreatedEventArgs(missionRun1.Id);
_missionRunService.RaiseEvent(nameof(MissionRunService.MissionRunCreated), missionRunCreatedEventArgs);

Thread.Sleep(100);

// Act
var mqttEventArgs = new MqttReceivedArgs(
new IsarMissionMessage
{
RobotName = robot.Name,
IsarId = robot.IsarId,
MissionId = missionRun1.IsarMissionId,
Status = "successful",
Timestamp = DateTime.UtcNow
});
var robotAvailableEventArgs = new RobotAvailableEventArgs(robot.Id);

_mqttService.RaiseEvent(nameof(MqttService.MqttIsarMissionReceived), mqttEventArgs);
_missionSchedulingService.RaiseEvent(nameof(MissionSchedulingService.RobotAvailable), robotAvailableEventArgs);
Thread.Sleep(500);

// Assert
var postTestMissionRun1 = await _missionRunService.ReadById(missionRun1.Id);
Assert.Equal(MissionRunType.Localization, postTestMissionRun1!.MissionRunType);
Assert.Equal(MissionStatus.Successful, postTestMissionRun1!.Status);
var postTestMissionRun2 = await _missionRunService.ReadById(missionRun2.Id);
Assert.Equal(MissionStatus.Ongoing, postTestMissionRun2!.Status);
}

[Fact]
public async void LocalizationMissionCompletesAfterPressingSendToSafeZoneButton()
{
Expand Down
68 changes: 39 additions & 29 deletions backend/api/EventHandlers/MqttEventHandler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -242,56 +242,66 @@ private async void OnIsarMissionUpdate(object? sender, MqttReceivedArgs mqttArgs
return;
}

MissionRun flotillaMissionRun;
try { flotillaMissionRun = await missionRunService.UpdateMissionRunStatusByIsarMissionId(isarMission.MissionId, status); }
catch (MissionRunNotFoundException) { return; }

_logger.LogInformation(
"Mission '{Id}' (ISARMissionID='{IsarMissionId}') status updated to '{Status}' for robot '{RobotName}' with ISAR id '{IsarId}'",
flotillaMissionRun.Id, isarMission.MissionId, isarMission.Status, isarMission.RobotName, isarMission.IsarId
);

if (!flotillaMissionRun.IsCompleted) return;

var robot = await robotService.ReadByIsarId(isarMission.IsarId);
if (robot is null)
var flotillaMissionRun = await missionRunService.ReadByIsarMissionId(isarMission.MissionId);
if (flotillaMissionRun is null)
{
_logger.LogError("Could not find robot '{RobotName}' with ISAR id '{IsarId}'", isarMission.RobotName, isarMission.IsarId);
string errorMessage = $"Mission with isar mission Id {isarMission.IsarId} was not found";
_logger.LogError("{Message}", errorMessage);
return;
}

if (flotillaMissionRun.Status == status) { return; }

if (flotillaMissionRun.IsLocalizationMission())
{
if (flotillaMissionRun.Status != MissionStatus.Successful)
if (status == MissionStatus.Successful || status == MissionStatus.PartiallySuccessful)
{
try
{
await robotService.UpdateCurrentArea(robot.Id, null);
await robotService.UpdateCurrentArea(flotillaMissionRun.Robot.Id, flotillaMissionRun.Area);
}
catch (RobotNotFoundException)
{
_logger.LogError("Could not find robot '{RobotName}' with ID '{Id}'", robot.Name, robot.Id);
_logger.LogError("Could not find robot '{RobotName}' with ID '{Id}'", flotillaMissionRun.Robot.Name, flotillaMissionRun.Robot.Id);
return;
}

signalRService.ReportGeneralFailToSignalR(robot, "Failed Localization Mission", $"Failed localization mission for robot {robot.Name}.");
_logger.LogError("Localization mission for robot '{RobotName}' failed.", isarMission.RobotName);
}
else
else if (status == MissionStatus.Aborted || status == MissionStatus.Cancelled || status == MissionStatus.Failed)
{
try
{
await robotService.UpdateCurrentArea(robot.Id, flotillaMissionRun.Area);
await robotService.UpdateCurrentArea(flotillaMissionRun.Robot.Id, null);
}
catch (RobotNotFoundException)
{
_logger.LogError("Could not find robot '{RobotName}' with ID '{Id}'", robot.Name, robot.Id);
_logger.LogError("Could not find robot '{RobotName}' with ID '{Id}'", flotillaMissionRun.Robot.Name, flotillaMissionRun.Robot.Id);
return;
}

signalRService.ReportGeneralFailToSignalR(flotillaMissionRun.Robot, "Failed Localization Mission", $"Failed localization mission for robot {flotillaMissionRun.Robot.Name}.");
_logger.LogError("Localization mission for robot '{RobotName}' failed.", isarMission.RobotName);
}
}

if (flotillaMissionRun.IsReturnHomeMission() && (flotillaMissionRun.Status == MissionStatus.Cancelled || flotillaMissionRun.Status == MissionStatus.Failed))
MissionRun updatedFlotillaMissionRun;
try { updatedFlotillaMissionRun = await missionRunService.UpdateMissionRunStatusByIsarMissionId(isarMission.MissionId, status); }
catch (MissionRunNotFoundException) { return; }

_logger.LogInformation(
"Mission '{Id}' (ISARMissionID='{IsarMissionId}') status updated to '{Status}' for robot '{RobotName}' with ISAR id '{IsarId}'",
updatedFlotillaMissionRun.Id, isarMission.MissionId, isarMission.Status, isarMission.RobotName, isarMission.IsarId
);

if (!updatedFlotillaMissionRun.IsCompleted) return;

var robot = await robotService.ReadByIsarId(isarMission.IsarId);
if (robot is null)
{
_logger.LogError("Could not find robot '{RobotName}' with ISAR id '{IsarId}'", isarMission.RobotName, isarMission.IsarId);
return;
}

if (updatedFlotillaMissionRun.IsReturnHomeMission() && (updatedFlotillaMissionRun.Status == MissionStatus.Cancelled || updatedFlotillaMissionRun.Status == MissionStatus.Failed))
{
try
{
Expand All @@ -311,19 +321,19 @@ private async void OnIsarMissionUpdate(object? sender, MqttReceivedArgs mqttArgs
return;
}

_logger.LogInformation("Robot '{Id}' ('{Name}') - completed mission run {MissionRunId}", robot.IsarId, robot.Name, flotillaMissionRun.Id);
_logger.LogInformation("Robot '{Id}' ('{Name}') - completed mission run {MissionRunId}", robot.IsarId, robot.Name, updatedFlotillaMissionRun.Id);
missionSchedulingService.TriggerMissionCompleted(new MissionCompletedEventArgs(robot.Id));

if (flotillaMissionRun.MissionId == null)
if (updatedFlotillaMissionRun.MissionId == null)
{
_logger.LogInformation("Mission run {missionRunId} does not have a mission definition assosiated with it", flotillaMissionRun.Id);
_logger.LogInformation("Mission run {missionRunId} does not have a mission definition assosiated with it", updatedFlotillaMissionRun.Id);
return;
}

try { await lastMissionRunService.SetLastMissionRun(flotillaMissionRun.Id, flotillaMissionRun.MissionId); }
try { await lastMissionRunService.SetLastMissionRun(updatedFlotillaMissionRun.Id, updatedFlotillaMissionRun.MissionId); }
catch (MissionNotFoundException)
{
_logger.LogError("Mission not found when setting last mission run for mission definition {missionId}", flotillaMissionRun.MissionId);
_logger.LogError("Mission not found when setting last mission run for mission definition {missionId}", updatedFlotillaMissionRun.MissionId);
return;
}

Expand Down
67 changes: 0 additions & 67 deletions backend/api/Services/LocalizationService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@ namespace Api.Services
{
public interface ILocalizationService
{
public Task<MissionRun> CreateLocalizationMissionInArea(string robotId, string areaId);
public Task EnsureRobotIsOnSameInstallationAsMission(Robot robot, MissionDefinition missionDefinition);
public Task<bool> RobotIsLocalized(string robotId);
public Task<bool> RobotIsOnSameDeckAsMission(string robotId, string areaId);
Expand Down Expand Up @@ -45,72 +44,6 @@ public async Task<bool> RobotIsLocalized(string robotId)
return robot.CurrentArea is not null;
}

public async Task<MissionRun> CreateLocalizationMissionInArea(string robotId, string areaId)
{
var robot = await robotService.ReadById(robotId);
if (robot is null)
{
string errorMessage = $"The robot with ID {robotId} was not found";
logger.LogError("{Message}", errorMessage);
throw new RobotNotFoundException(errorMessage);
}

var area = await areaService.ReadById(areaId);
if (area is null)
{
string errorMessage = $"The area with ID {areaId} was not found";
logger.LogError("{Message}", errorMessage);
throw new AreaNotFoundException(errorMessage);
}

if (area.Deck?.DefaultLocalizationPose?.Pose is null)
{
const string ErrorMessage = "The mission area is not associated with any deck or that deck does not have a localization pose";
logger.LogError("{Message}", ErrorMessage);
throw new DeckNotFoundException(ErrorMessage);
}
if (robot.Status is not RobotStatus.Available)
{
string errorMessage = $"Robot '{robot.Id}' is not available as the status is {robot.Status}";
logger.LogWarning("{Message}", errorMessage);
throw new RobotNotAvailableException(errorMessage);
}
if (robot.Deprecated)
{
string errorMessage = $"Robot '{robot.Id}' is deprecated and cannot localize";
logger.LogWarning("{Message}", errorMessage);
throw new RobotNotAvailableException(errorMessage);
}

var localizationMissionRun = new MissionRun
{
Name = "Localization mission",
Robot = robot,
MissionRunType = MissionRunType.Localization,
InstallationCode = area.Installation.InstallationCode,
Area = area,
Status = MissionStatus.Pending,
DesiredStartTime = DateTime.UtcNow,
Tasks = new List<MissionTask>
{
new(new Pose(area.Deck.DefaultLocalizationPose.Pose), MissionTaskType.Localization)
},
Map = new MapMetadata()
};
await mapService.AssignMapToMission(localizationMissionRun);

try
{
logger.LogWarning("Starting localization mission");
await missionRunService.Create(localizationMissionRun, triggerCreatedMissionRunEvent: false);
}
catch (UnsupportedRobotCapabilityException)
{
logger.LogError($"Unsupported robot capability detected when starting localisation mission for robot {localizationMissionRun.Robot.Name}. This should not happen.");
}
return localizationMissionRun;
}

public async Task<bool> RobotIsOnSameDeckAsMission(string robotId, string areaId)
{
var robot = await robotService.ReadById(robotId);
Expand Down

0 comments on commit e6a3a1f

Please sign in to comment.