Skip to content

Commit

Permalink
Only send start_pose on first mission in queue
Browse files Browse the repository at this point in the history
  • Loading branch information
@andchiind
andchiind committed Jun 19, 2024
1 parent adcb2f2 commit bb60026
Show file tree
Hide file tree
Showing 5 changed files with 18 additions and 15 deletions.
2 changes: 1 addition & 1 deletion backend/api.test/Mocks/IsarServiceMock.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace Api.Test.Mocks
{
public class MockIsarService : IIsarService
{
public async Task<IsarMission> StartMission(Robot robot, MissionRun mission)
public async Task<IsarMission> StartMission(Robot robot, MissionRun mission, bool isFirstMissionInQueue = false)
{
await Task.Run(() => Thread.Sleep(1));
var isarServiceMissionResponse = new IsarMission(
Expand Down
11 changes: 7 additions & 4 deletions backend/api/EventHandlers/MissionEventHandler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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(); }
}
Expand All @@ -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);
Expand All @@ -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(); }
}
Expand Down Expand Up @@ -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(); }
}
Expand Down
6 changes: 3 additions & 3 deletions backend/api/Services/IsarService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ namespace Api.Services
{
public interface IIsarService
{
public Task<IsarMission> StartMission(Robot robot, MissionRun missionRun);
public Task<IsarMission> StartMission(Robot robot, MissionRun missionRun, bool isFirstMissionInQueue = false);

public Task<IsarControlMissionResponse> StopMission(Robot robot);

Expand All @@ -23,15 +23,15 @@ public class IsarService(IDownstreamApi isarApi, ILogger<IsarService> logger) :
{
public const string ServiceName = "IsarApi";

public async Task<IsarMission> StartMission(Robot robot, MissionRun missionRun)
public async Task<IsarMission> StartMission(Robot robot, MissionRun missionRun, bool isFirstMissionInQueue = false)
{
var response = await CallApi(
HttpMethod.Post,
robot.IsarUri,
"schedule/start-mission",
new
{
mission_definition = new IsarMissionDefinition(missionRun)
mission_definition = new IsarMissionDefinition(missionRun, includeStartPose: isFirstMissionInQueue)
}
);

Expand Down
10 changes: 5 additions & 5 deletions backend/api/Services/MissionSchedulingService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool> OngoingMission(string robotId);

Expand All @@ -33,7 +33,7 @@ public interface IMissionSchedulingService
public class MissionSchedulingService(ILogger<MissionSchedulingService> 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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -373,7 +373,7 @@ private async Task MoveInterruptedMissionsToQueue(IEnumerable<string> 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;
Expand Down Expand Up @@ -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}";
Expand Down
4 changes: 2 additions & 2 deletions backend/api/Services/Models/IsarMissionDefinition.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,12 @@ public IsarMissionDefinition(List<IsarTaskDefinition> 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;
}
}

Expand Down

0 comments on commit bb60026

Please sign in to comment.