Initial commit
This commit is contained in:
@@ -0,0 +1,280 @@
|
||||
using RobotNet.VDA5050.Type;
|
||||
using RobotNet10.RobotApp.Services.Robot.Actions;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Robot.Helper;
|
||||
|
||||
/// <summary>
|
||||
/// Detects conflicts between actions according to VDA5050
|
||||
/// </summary>
|
||||
public class ActionConflictDetector
|
||||
{
|
||||
// VDA5050: Counter-action pairs that conflict
|
||||
private static readonly Dictionary<ActionType, ActionType> CounterActions = new()
|
||||
{
|
||||
{ ActionType.START_CHARGING, ActionType.STOP_CHARGING },
|
||||
{ ActionType.STOP_CHARGING, ActionType.START_CHARGING },
|
||||
{ ActionType.START_PAUSE, ActionType.STOP_PAUSE },
|
||||
{ ActionType.STOP_PAUSE, ActionType.START_PAUSE },
|
||||
};
|
||||
|
||||
// Actions that target the same resource and cannot run simultaneously
|
||||
private static readonly HashSet<ActionType> LoadHandlingActions =
|
||||
[
|
||||
ActionType.PICK,
|
||||
ActionType.DROP,
|
||||
ActionType.LIFT_ROTATE,
|
||||
ActionType.ROTATE,
|
||||
ActionType.ROTATE_KEEP_LIFT
|
||||
];
|
||||
|
||||
private static readonly HashSet<ActionType> ChargingActions =
|
||||
[
|
||||
ActionType.START_CHARGING,
|
||||
ActionType.STOP_CHARGING
|
||||
];
|
||||
|
||||
// Actions that use the Navigation module - cannot run simultaneously
|
||||
private static readonly HashSet<ActionType> NavigationActions =
|
||||
[
|
||||
ActionType.DOCK_TO,
|
||||
ActionType.MOVE_STRAIGHT_TO_COOR,
|
||||
ActionType.MOVE_STRAIGHT_WITH_DISTANCE,
|
||||
ActionType.FINE_POSITIONING,
|
||||
ActionType.INIT_POSITION,
|
||||
ActionType.START_CHARGING,
|
||||
ActionType.STOP_CHARGING
|
||||
];
|
||||
|
||||
// Functional module actions - cannot run while robot is moving (navigation active)
|
||||
private static readonly HashSet<ActionType> FunctionalModuleActions =
|
||||
[
|
||||
ActionType.PICK,
|
||||
ActionType.DROP,
|
||||
ActionType.LIFT_ROTATE,
|
||||
ActionType.ROTATE,
|
||||
ActionType.ROTATE_KEEP_LIFT,
|
||||
ActionType.DOCK_TO,
|
||||
ActionType.DETECT_OBJECT,
|
||||
ActionType.START_CHARGING,
|
||||
ActionType.STOP_CHARGING,
|
||||
ActionType.FINE_POSITIONING
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// Check if instant action conflicts with any running actions (ORDER or INSTANT)
|
||||
/// </summary>
|
||||
public ConflictResult CheckConflict(
|
||||
RobotNet.VDA5050.InstantAction.Action instantAction,
|
||||
IEnumerable<RobotAction> runningActions,
|
||||
bool isOrderActive = false,
|
||||
bool isDriving = false)
|
||||
{
|
||||
if (!RobotNet.VDA5050.EnumHelper.TryParse(instantAction.ActionType, out ActionType instantType))
|
||||
{
|
||||
return ConflictResult.Invalid("Invalid action type");
|
||||
}
|
||||
|
||||
// VDA5050: cancelOrder and read-only actions must NEVER be blocked
|
||||
if (instantType == ActionType.CANCEL_ORDER ||
|
||||
instantType == ActionType.STATE_REQUEST ||
|
||||
instantType == ActionType.FACTSHEET_REQUEST)
|
||||
{
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
// Check: Navigation instant action while Order is active
|
||||
if (isOrderActive && NavigationActions.Contains(instantType))
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.NavigationOrderConflict,
|
||||
$"Navigation action {instantType} rejected - Order is active, cannot execute navigation instant actions",
|
||||
null
|
||||
);
|
||||
}
|
||||
|
||||
// Check: Functional module or navigation action while robot is driving
|
||||
if (isDriving && (FunctionalModuleActions.Contains(instantType) || NavigationActions.Contains(instantType)))
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.DrivingConflict,
|
||||
$"Action {instantType} rejected - robot is currently moving",
|
||||
null
|
||||
);
|
||||
}
|
||||
|
||||
foreach (var runningAction in runningActions)
|
||||
{
|
||||
// 1. Check counter-action conflict
|
||||
var counterConflict = CheckCounterActionConflict(instantType, runningAction);
|
||||
if (counterConflict.HasConflict)
|
||||
{
|
||||
return counterConflict;
|
||||
}
|
||||
|
||||
// 2. Check resource conflict
|
||||
var resourceConflict = CheckResourceConflict(instantAction, instantType, runningAction);
|
||||
if (resourceConflict.HasConflict)
|
||||
{
|
||||
return resourceConflict;
|
||||
}
|
||||
|
||||
// 3. Check navigation conflict (two navigation actions cannot run simultaneously)
|
||||
var navConflict = CheckNavigationConflict(instantType, runningAction);
|
||||
if (navConflict.HasConflict)
|
||||
{
|
||||
return navConflict;
|
||||
}
|
||||
|
||||
// 4. Check BlockingType conflict
|
||||
var blockingConflict = CheckBlockingTypeConflict(instantAction, runningAction);
|
||||
if (blockingConflict.HasConflict)
|
||||
{
|
||||
return blockingConflict;
|
||||
}
|
||||
}
|
||||
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
private static ConflictResult CheckCounterActionConflict(ActionType instantType, RobotAction runningAction)
|
||||
{
|
||||
if (CounterActions.TryGetValue(instantType, out var counterType) &&
|
||||
counterType == runningAction.Type)
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.CounterAction,
|
||||
$"InstantAction {instantType} conflicts with running action {runningAction.Type}",
|
||||
runningAction.Id
|
||||
);
|
||||
}
|
||||
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
private static ConflictResult CheckResourceConflict(
|
||||
RobotNet.VDA5050.InstantAction.Action instantAction,
|
||||
ActionType instantType,
|
||||
RobotAction runningAction)
|
||||
{
|
||||
// Check if both actions target load handling
|
||||
if (LoadHandlingActions.Contains(instantType) &&
|
||||
LoadHandlingActions.Contains(runningAction.Type))
|
||||
{
|
||||
// Check if same LHD (Load Handling Device)
|
||||
var instantLhd = GetParameterValue(instantAction.ActionParameters, "lhd");
|
||||
var orderLhd = GetParameterValue(runningAction.Parameters, "lhd");
|
||||
|
||||
// If both specify LHD and they're the same, or if neither specifies (default LHD)
|
||||
if (string.IsNullOrEmpty(instantLhd) || string.IsNullOrEmpty(orderLhd) ||
|
||||
instantLhd == orderLhd)
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.ResourceConflict,
|
||||
$"InstantAction {instantType} conflicts with {runningAction.Type} - same Load Handling Device",
|
||||
runningAction.Id
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Check if both actions target charging
|
||||
if (ChargingActions.Contains(instantType) &&
|
||||
ChargingActions.Contains(runningAction.Type))
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.ResourceConflict,
|
||||
$"InstantAction {instantType} conflicts with {runningAction.Type} - same charging system",
|
||||
runningAction.Id
|
||||
);
|
||||
}
|
||||
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
private static ConflictResult CheckNavigationConflict(ActionType instantType, RobotAction runningAction)
|
||||
{
|
||||
// Two navigation actions cannot run simultaneously
|
||||
if (NavigationActions.Contains(instantType) &&
|
||||
NavigationActions.Contains(runningAction.Type) &&
|
||||
!runningAction.IsCompleted)
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.NavigationConflict,
|
||||
$"Navigation action {instantType} conflicts with running navigation action {runningAction.Type}",
|
||||
runningAction.Id
|
||||
);
|
||||
}
|
||||
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
private static ConflictResult CheckBlockingTypeConflict(
|
||||
RobotNet.VDA5050.InstantAction.Action instantAction,
|
||||
RobotAction runningAction)
|
||||
{
|
||||
// HARD instant action cannot run when HARD action is running
|
||||
if (instantAction.BlockingType == BlockingType.HARD &&
|
||||
runningAction.BlockingType == BlockingType.HARD &&
|
||||
!runningAction.IsCompleted)
|
||||
{
|
||||
return ConflictResult.Conflict(
|
||||
ConflictType.BlockingTypeConflict,
|
||||
$"InstantAction (HARD) cannot run while action {runningAction.Type} (HARD) is running",
|
||||
runningAction.Id
|
||||
);
|
||||
}
|
||||
|
||||
return ConflictResult.NoConflict();
|
||||
}
|
||||
|
||||
private static string? GetParameterValue(
|
||||
RobotNet.VDA5050.InstantAction.ActionParameter[]? parameters,
|
||||
string key)
|
||||
{
|
||||
return parameters?.FirstOrDefault(p => p.Key == key)?.Value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Result of conflict detection
|
||||
/// </summary>
|
||||
public class ConflictResult
|
||||
{
|
||||
public bool HasConflict { get; init; }
|
||||
public ConflictType Type { get; init; }
|
||||
public string Description { get; init; } = "";
|
||||
public string? ConflictingActionId { get; init; }
|
||||
|
||||
public static ConflictResult NoConflict() => new() { HasConflict = false };
|
||||
|
||||
public static ConflictResult Conflict(ConflictType type, string description, string? conflictingActionId = null)
|
||||
=> new()
|
||||
{
|
||||
HasConflict = true,
|
||||
Type = type,
|
||||
Description = description,
|
||||
ConflictingActionId = conflictingActionId
|
||||
};
|
||||
|
||||
public static ConflictResult Invalid(string description)
|
||||
=> new()
|
||||
{
|
||||
HasConflict = true,
|
||||
Type = ConflictType.Invalid,
|
||||
Description = description
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Types of conflicts
|
||||
/// </summary>
|
||||
public enum ConflictType
|
||||
{
|
||||
None,
|
||||
CounterAction, // e.g., startCharging vs stopCharging
|
||||
ResourceConflict, // e.g., two pick actions on same LHD
|
||||
NavigationConflict, // e.g., two navigation actions (dockTo vs moveStraight)
|
||||
NavigationOrderConflict,// Navigation instant action while Order is active
|
||||
DrivingConflict, // Functional module action while robot is moving
|
||||
BlockingTypeConflict, // e.g., HARD vs HARD
|
||||
Invalid // Invalid action type or parameters
|
||||
}
|
||||
Reference in New Issue
Block a user