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
|
||||
}
|
||||
@@ -0,0 +1,215 @@
|
||||
using RobotNet.VDA5050.Order;
|
||||
using RobotNet.VDA5050.Type;
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.RobotApp.Services.Exceptions;
|
||||
using RobotNet10.RobotApp.Services.Robot.Models;
|
||||
using RobotNet10.RobotApp.Shared.Enums;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Robot.Helper;
|
||||
|
||||
public class OrderConverter
|
||||
{
|
||||
public static (OrderNode[] Nodes, OrderEdge[] Edges) Validate(Node[] nodes, Edge[] edges, double currentTheta)
|
||||
{
|
||||
if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length));
|
||||
if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length));
|
||||
|
||||
OrderNode[] orderNodes = [..nodes.Select(n => new OrderNode
|
||||
{
|
||||
NodeId = n.NodeId,
|
||||
SequenceId = n.SequenceId,
|
||||
X = n.NodePosition?.X ?? 0,
|
||||
Y = n.NodePosition?.Y ?? 0,
|
||||
Theta = n.NodePosition?.Theta,
|
||||
AllowedDeviationXY = n.NodePosition?.AllowedDeviationXY,
|
||||
AllowedDeviationTheta = n.NodePosition?.AllowedDeviationTheta,
|
||||
})];
|
||||
|
||||
List<OrderEdge> orderEdges = [];
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var trajectory = edge.Trajectory;
|
||||
var controlPoints = trajectory?.ControlPoints;
|
||||
orderEdges.Add(new()
|
||||
{
|
||||
EdgeId = edge.EdgeId,
|
||||
SequenceId = edge.SequenceId,
|
||||
StartNodeId = edge.StartNodeId,
|
||||
EndNodeId = edge.EndNodeId,
|
||||
Orientation = edge.Orientation,
|
||||
OrientationType = edge.OrientationType,
|
||||
RotationAllowed = edge.RotationAllowed,
|
||||
Speed = edge.MaxSpeed,
|
||||
Degree = edge.Trajectory?.Degree ?? 1,
|
||||
ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
|
||||
ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
|
||||
ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
|
||||
ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
|
||||
});
|
||||
}
|
||||
// cần xử lí để lấy direction
|
||||
var currentDirection = GetDirectionInNode(nodes[0].NodePosition?.Theta ?? currentTheta, orderNodes[0], orderNodes[1], orderEdges[0]);
|
||||
for(int i = 0; i < orderEdges.Count; i++)
|
||||
{
|
||||
currentDirection = OrientationToDirection(currentDirection, orderNodes[i], orderNodes[i + 1], orderEdges[i]);
|
||||
orderEdges[i].Direction = currentDirection;
|
||||
orderNodes[i].ContinueTheta = GetAngleInNodeStart(orderNodes[i], orderNodes[i + 1], orderEdges[i]);
|
||||
if (i > 0)
|
||||
{
|
||||
var inNodeAngle = GetAngleInNodeEnd(orderNodes[i], orderNodes[i - 1], orderEdges[i - 1]);
|
||||
if (orderNodes[i].Theta is { } theta && Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(theta)) > 0.04)
|
||||
{
|
||||
orderNodes[i].IsWaitRotating = true;
|
||||
}
|
||||
if (!orderNodes[i].IsWaitRotating && orderNodes[i].ContinueTheta is { } continueTheta)
|
||||
{
|
||||
if (Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(continueTheta)) > 0.785)
|
||||
{
|
||||
orderNodes[i].IsWaitRotating = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return (orderNodes , [..orderEdges]);
|
||||
}
|
||||
|
||||
private static RobotDirection ConvertTangentialOrientation(double orientation)
|
||||
{
|
||||
// Normalize về [0, 2*PI] để dễ xử lý
|
||||
double normalizedAngle = SpaceCompute.NormalizeRadianAngle(orientation);
|
||||
if (normalizedAngle < 0) normalizedAngle += 2 * Math.PI;
|
||||
|
||||
// Forward: orientation gần 0 (hoặc 2*PI)
|
||||
// Backward: orientation gần PI
|
||||
|
||||
// Kiểm tra gần 0 hoặc 2*PI (Forward)
|
||||
if (normalizedAngle <= Math.PI / 2 || normalizedAngle >= 3 * Math.PI / 2)
|
||||
{
|
||||
return RobotDirection.FORWARD;
|
||||
}
|
||||
// Kiểm tra gần PI (Backward)
|
||||
else
|
||||
{
|
||||
return RobotDirection.BACKWARD;
|
||||
}
|
||||
}
|
||||
|
||||
private static RobotDirection ConvertGlobalOrientation(double orientation, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
|
||||
{
|
||||
(double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
|
||||
{
|
||||
StartX = inNode.X,
|
||||
StartY = inNode.Y,
|
||||
EndX = futureNode.X,
|
||||
EndY = futureNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
});
|
||||
|
||||
var edgeAngle = Math.Atan2(futurey - inNode.Y, futurex - inNode.X);
|
||||
|
||||
// Tính góc chênh lệch giữa orientation và edge angle
|
||||
double angleDiff = SpaceCompute.NormalizeRadianAngle(orientation - edgeAngle);
|
||||
|
||||
// Nếu góc chênh lệch gần 0 -> Forward
|
||||
// Nếu góc chênh lệch gần PI -> Backward
|
||||
double absAngleDiff = Math.Abs(angleDiff);
|
||||
|
||||
if (absAngleDiff <= Math.PI / 2)
|
||||
{
|
||||
return RobotDirection.FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
return RobotDirection.BACKWARD;
|
||||
}
|
||||
}
|
||||
|
||||
private static RobotDirection GetDirectionInNode(double currentTheta, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
|
||||
{
|
||||
(double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
|
||||
{
|
||||
StartX = inNode.X,
|
||||
StartY = inNode.Y,
|
||||
EndX = futureNode.X,
|
||||
EndY = futureNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
});
|
||||
(double robotx, double roboty) =
|
||||
(
|
||||
inNode.X + Math.Cos(currentTheta),
|
||||
inNode.Y + Math.Sin(currentTheta)
|
||||
);
|
||||
|
||||
var angle = SpaceCompute.GetVectorAngle(
|
||||
inNode.X,
|
||||
inNode.Y,
|
||||
robotx,
|
||||
roboty,
|
||||
futurex,
|
||||
futurey);
|
||||
return angle > 90 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
}
|
||||
|
||||
private static double GetAngleInNodeEnd(OrderNode inNode, OrderNode oldNode, OrderEdge edge)
|
||||
{
|
||||
(double oldX, double oldY) = SpaceCompute.BezierPoint(0.9, new()
|
||||
{
|
||||
StartX = oldNode.X,
|
||||
StartY = oldNode.Y,
|
||||
EndX = inNode.X,
|
||||
EndY = inNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
});
|
||||
var dy = inNode.Y - oldY;
|
||||
var dx = inNode.X - oldX;
|
||||
return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
|
||||
}
|
||||
|
||||
private static double GetAngleInNodeStart(OrderNode inNode, OrderNode futureNode, OrderEdge edge)
|
||||
{
|
||||
(double futureX, double futureY) = SpaceCompute.BezierPoint(0.1, new()
|
||||
{
|
||||
StartX = inNode.X,
|
||||
StartY = inNode.Y,
|
||||
EndX = futureNode.X,
|
||||
EndY = futureNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
});
|
||||
var dy = futureY - inNode.Y;
|
||||
var dx = futureX - inNode.X;
|
||||
return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
|
||||
}
|
||||
|
||||
public static RobotDirection OrientationToDirection(RobotDirection currentDirection, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
|
||||
{
|
||||
if(edge.Orientation.HasValue && edge.OrientationType is not null)
|
||||
{
|
||||
switch (edge.OrientationType)
|
||||
{
|
||||
case OrientationType.TANGENTIAL:
|
||||
return ConvertTangentialOrientation(edge.Orientation.Value);
|
||||
|
||||
case OrientationType.GLOBAL:
|
||||
return ConvertGlobalOrientation(edge.Orientation.Value, inNode, futureNode, edge);
|
||||
}
|
||||
}
|
||||
if (inNode.Theta.HasValue) return GetDirectionInNode(inNode.Theta.Value, inNode, futureNode, edge);
|
||||
return currentDirection;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user