447 lines
15 KiB
C#
447 lines
15 KiB
C#
using RobotApp.VDA5050.InstantAction;
|
||
using RobotApp.VDA5050.Order;
|
||
using System.Text.Json;
|
||
using System.Text.Json.Serialization;
|
||
|
||
namespace RobotApp.Client.Services;
|
||
|
||
// ======================================================
|
||
// EDGE UI
|
||
// ======================================================
|
||
public class UiEdge
|
||
{
|
||
public string EdgeId { get; set; } = "";
|
||
public int SequenceId { get; set; }
|
||
public bool Released { get; set; } = true;
|
||
|
||
public string StartNodeId { get; set; } = "";
|
||
public string EndNodeId { get; set; } = "";
|
||
|
||
// ===== CURVE (EDITOR GENERATED) =====
|
||
public double Radius { get; set; } = 0;
|
||
public Quadrant Quadrant { get; set; }
|
||
|
||
// ===== IMPORTED TRAJECTORY =====
|
||
public bool HasTrajectory { get; set; } = false;
|
||
public UiTrajectory? Trajectory { get; set; }
|
||
|
||
// ===== UI STATE =====
|
||
public bool Expanded { get; private set; } = false;
|
||
|
||
public void MarkExpanded()
|
||
{
|
||
Expanded = true;
|
||
}
|
||
}
|
||
|
||
public class UiTrajectory
|
||
{
|
||
public int Degree { get; set; }
|
||
public double[] KnotVector { get; set; } = Array.Empty<double>();
|
||
public List<Point> ControlPoints { get; set; } = new();
|
||
}
|
||
|
||
public enum Quadrant
|
||
{
|
||
I,
|
||
II,
|
||
III,
|
||
IV
|
||
}
|
||
|
||
// ======================================================
|
||
// GEOMETRY MODELS
|
||
// ======================================================
|
||
public record Point(double X, double Y);
|
||
|
||
public record QuarterResult(
|
||
Point EndPoint,
|
||
object Trajectory
|
||
);
|
||
|
||
// ======================================================
|
||
// GEOMETRY HELPER (QUARTER CIRCLE)
|
||
// ======================================================
|
||
public static class QuarterGeometry
|
||
{
|
||
private const double K = 0.5522847498307936;
|
||
|
||
public static QuarterResult BuildQuarterTrajectory(
|
||
Point A,
|
||
double r,
|
||
Quadrant q
|
||
)
|
||
{
|
||
Point P1, P2, C;
|
||
|
||
switch (q)
|
||
{
|
||
case Quadrant.I:
|
||
P1 = new(A.X, A.Y + K * r);
|
||
P2 = new(A.X + K * r, A.Y + r);
|
||
C = new(A.X + r, A.Y + r);
|
||
break;
|
||
|
||
case Quadrant.II:
|
||
P1 = new(A.X - K * r, A.Y);
|
||
P2 = new(A.X - r, A.Y + K * r);
|
||
C = new(A.X - r, A.Y + r);
|
||
break;
|
||
|
||
case Quadrant.III:
|
||
P1 = new(A.X, A.Y - K * r);
|
||
P2 = new(A.X - K * r, A.Y - r);
|
||
C = new(A.X - r, A.Y - r);
|
||
break;
|
||
|
||
case Quadrant.IV:
|
||
P1 = new(A.X + K * r, A.Y);
|
||
P2 = new(A.X + r, A.Y - K * r);
|
||
C = new(A.X + r, A.Y - r);
|
||
break;
|
||
|
||
default:
|
||
throw new ArgumentOutOfRangeException(nameof(q));
|
||
}
|
||
|
||
return new QuarterResult(
|
||
C,
|
||
new
|
||
{
|
||
degree = 3,
|
||
knotVector = new[] { 0, 0, 0, 0, 1, 1, 1, 1 },
|
||
controlPoints = new[]
|
||
{
|
||
new { x = A.X, y = A.Y }, // P0
|
||
new { x = P1.X, y = P1.Y }, // P1
|
||
new { x = P2.X, y = P2.Y }, // P2
|
||
new { x = C.X, y = C.Y } // P3
|
||
}
|
||
}
|
||
);
|
||
}
|
||
}
|
||
// ======================================================
|
||
// ORDER MESSAGE
|
||
// ======================================================
|
||
public class OrderMessage
|
||
{
|
||
public int HeaderId { get; set; }
|
||
public string Timestamp { get; set; } = "";
|
||
public string Version { get; set; } = "v1";
|
||
public string Manufacturer { get; set; } = "PNKX";
|
||
public string SerialNumber { get; set; } = "T800-002";
|
||
public string OrderId { get; set; }
|
||
public int OrderUpdateId { get; set; }
|
||
public string? ZoneSetId { get; set; }
|
||
|
||
public List<Node> Nodes { get; set; } = new();
|
||
public List<UiEdge> Edges { get; set; } = new();
|
||
public static Node CreateCurveNode(Node startNode, UiEdge edge)
|
||
{
|
||
var A = new Point(
|
||
startNode.NodePosition.X,
|
||
startNode.NodePosition.Y
|
||
);
|
||
|
||
var result = QuarterGeometry.BuildQuarterTrajectory(
|
||
A,
|
||
edge.Radius,
|
||
edge.Quadrant
|
||
);
|
||
|
||
return new Node
|
||
{
|
||
NodeId = $"NODE_C{Guid.NewGuid():N}".Substring(0, 12),
|
||
Released = true,
|
||
NodePosition = new NodePosition
|
||
{
|
||
X = result.EndPoint.X,
|
||
Y = result.EndPoint.Y,
|
||
Theta = startNode.NodePosition.Theta,
|
||
MapId = startNode.NodePosition.MapId
|
||
}
|
||
};
|
||
}
|
||
public static OrderMessage FromSchemaObject(JsonElement root)
|
||
{
|
||
var order = new OrderMessage
|
||
{
|
||
HeaderId = root.GetProperty("headerId").GetInt32(),
|
||
Timestamp = root.GetProperty("timestamp").GetString(),
|
||
Version = root.GetProperty("version").GetString(),
|
||
Manufacturer = root.GetProperty("manufacturer").GetString(),
|
||
SerialNumber = root.GetProperty("serialNumber").GetString(),
|
||
OrderId = root.GetProperty("orderId").GetString(),
|
||
OrderUpdateId = root.GetProperty("orderUpdateId").GetInt32()
|
||
};
|
||
|
||
// ================= NODES =================
|
||
foreach (var n in root.GetProperty("nodes").EnumerateArray())
|
||
{
|
||
var node = new Node
|
||
{
|
||
NodeId = n.GetProperty("nodeId").GetString()!,
|
||
SequenceId = n.GetProperty("sequenceId").GetInt32(),
|
||
Released = n.GetProperty("released").GetBoolean(),
|
||
|
||
NodePosition = new NodePosition
|
||
{
|
||
X = n.GetProperty("nodePosition").GetProperty("x").GetDouble(),
|
||
Y = n.GetProperty("nodePosition").GetProperty("y").GetDouble(),
|
||
Theta = n.GetProperty("nodePosition").GetProperty("theta").GetDouble(),
|
||
AllowedDeviationXY = n.GetProperty("nodePosition").GetProperty("allowedDeviationXY").GetDouble(),
|
||
AllowedDeviationTheta = n.GetProperty("nodePosition").GetProperty("allowedDeviationTheta").GetDouble(),
|
||
MapId = n.GetProperty("nodePosition").GetProperty("mapId").GetString()
|
||
},
|
||
|
||
Actions = ParseActions(n)
|
||
};
|
||
|
||
order.Nodes.Add(node);
|
||
}
|
||
|
||
foreach (var e in root.GetProperty("edges").EnumerateArray())
|
||
{
|
||
var edge = new UiEdge
|
||
{
|
||
EdgeId = e.GetProperty("edgeId").GetString()!,
|
||
SequenceId = e.GetProperty("sequenceId").GetInt32(),
|
||
Released = e.GetProperty("released").GetBoolean(),
|
||
StartNodeId = e.GetProperty("startNodeId").GetString()!,
|
||
EndNodeId = e.GetProperty("endNodeId").GetString()!,
|
||
};
|
||
|
||
// ===== IMPORT TRAJECTORY =====
|
||
if (e.TryGetProperty("trajectory", out var traj))
|
||
{
|
||
edge.HasTrajectory = true;
|
||
edge.Trajectory = new UiTrajectory
|
||
{
|
||
Degree = traj.GetProperty("degree").GetInt32(),
|
||
KnotVector = traj.GetProperty("knotVector")
|
||
.EnumerateArray()
|
||
.Select(x => x.GetDouble())
|
||
.ToArray(),
|
||
|
||
ControlPoints = traj.GetProperty("controlPoints")
|
||
.EnumerateArray()
|
||
.Select(p => new Point(
|
||
p.GetProperty("x").GetDouble(),
|
||
p.GetProperty("y").GetDouble()
|
||
))
|
||
.ToList()
|
||
};
|
||
|
||
// 🔥 IMPORTED CURVE → LOCK APPLY
|
||
edge.MarkExpanded();
|
||
}
|
||
|
||
order.Edges.Add(edge);
|
||
}
|
||
|
||
return order;
|
||
}
|
||
|
||
// ================= ACTION PARSER =================
|
||
private static VDA5050.InstantAction.Action[] ParseActions(JsonElement parent)
|
||
{
|
||
if (!parent.TryGetProperty("actions", out var acts))
|
||
return Array.Empty<VDA5050.InstantAction.Action>();
|
||
|
||
return acts.EnumerateArray().Select(a =>
|
||
new VDA5050.InstantAction.Action
|
||
{
|
||
ActionId = a.GetProperty("actionId").GetString(),
|
||
ActionType = a.GetProperty("actionType").GetString(),
|
||
BlockingType = a.GetProperty("blockingType").GetString(),
|
||
|
||
ActionParameters = a.TryGetProperty("actionParameters", out var ps)
|
||
? ps.EnumerateArray()
|
||
.Select(p => new ActionParameter
|
||
{
|
||
Key = p.GetProperty("key").GetString(),
|
||
Value = p.GetProperty("value").GetString()
|
||
})
|
||
.ToArray()
|
||
: Array.Empty<ActionParameter>()
|
||
}
|
||
).ToArray();
|
||
}
|
||
public object ToSchemaObject()
|
||
{
|
||
// ================= SORT NODES BY UI SEQUENCE =================
|
||
var orderedNodes = Nodes
|
||
.OrderBy(n => n.SequenceId)
|
||
.ToList();
|
||
|
||
// ================= BUILD NODE OBJECTS =================
|
||
var nodeObjects = orderedNodes
|
||
.Select((n, index) => new
|
||
{
|
||
nodeId = n.NodeId,
|
||
sequenceId = index * 2, // ✅ NODE = EVEN
|
||
released = n.Released,
|
||
|
||
nodePosition = new
|
||
{
|
||
x = n.NodePosition.X,
|
||
y = n.NodePosition.Y,
|
||
theta = n.NodePosition.Theta,
|
||
|
||
allowedDeviationXY = n.NodePosition.AllowedDeviationXY,
|
||
allowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
|
||
|
||
mapId = string.IsNullOrWhiteSpace(n.NodePosition.MapId)
|
||
? "MAP_01"
|
||
: n.NodePosition.MapId
|
||
},
|
||
|
||
actions = n.Actions?
|
||
.Select(a => new
|
||
{
|
||
actionId = a.ActionId,
|
||
actionType = a.ActionType,
|
||
blockingType = a.BlockingType,
|
||
|
||
actionParameters = a.ActionParameters?
|
||
.Select(p => new
|
||
{
|
||
key = p.Key,
|
||
value = p.Value
|
||
})
|
||
.ToArray()
|
||
?? Array.Empty<object>()
|
||
})
|
||
.ToArray()
|
||
?? Array.Empty<object>()
|
||
})
|
||
.ToArray();
|
||
|
||
// ================= BUILD EDGE OBJECTS =================
|
||
var edgeObjects = Edges
|
||
.Select<UiEdge, object>((e, index) =>
|
||
{
|
||
int sequenceId = index * 2 + 1; // ✅ EDGE = ODD
|
||
|
||
// ---------- BASE ----------
|
||
var baseEdge = new
|
||
{
|
||
edgeId = e.EdgeId,
|
||
sequenceId,
|
||
released = true,
|
||
startNodeId = e.StartNodeId,
|
||
endNodeId = e.EndNodeId
|
||
};
|
||
|
||
// =================================================
|
||
// 1️⃣ IMPORTED TRAJECTORY
|
||
// =================================================
|
||
if (e.HasTrajectory && e.Trajectory != null)
|
||
{
|
||
return new
|
||
{
|
||
baseEdge.edgeId,
|
||
baseEdge.sequenceId,
|
||
baseEdge.released,
|
||
baseEdge.startNodeId,
|
||
baseEdge.endNodeId,
|
||
|
||
trajectory = new
|
||
{
|
||
degree = e.Trajectory.Degree,
|
||
knotVector = e.Trajectory.KnotVector,
|
||
controlPoints = e.Trajectory.ControlPoints
|
||
.Select(p => new { x = p.X, y = p.Y })
|
||
.ToArray()
|
||
},
|
||
|
||
actions = Array.Empty<object>()
|
||
};
|
||
}
|
||
|
||
// =================================================
|
||
// 2️⃣ STRAIGHT EDGE
|
||
// =================================================
|
||
if (e.Radius <= 0)
|
||
{
|
||
return new
|
||
{
|
||
baseEdge.edgeId,
|
||
baseEdge.sequenceId,
|
||
baseEdge.released,
|
||
baseEdge.startNodeId,
|
||
baseEdge.endNodeId,
|
||
actions = Array.Empty<object>()
|
||
};
|
||
}
|
||
|
||
// =================================================
|
||
// 3️⃣ GENERATED CURVE (EDITOR)
|
||
// =================================================
|
||
var startNode = orderedNodes.First(n => n.NodeId == e.StartNodeId);
|
||
|
||
var A = new Point(
|
||
startNode.NodePosition.X,
|
||
startNode.NodePosition.Y
|
||
);
|
||
|
||
var result = QuarterGeometry.BuildQuarterTrajectory(
|
||
A,
|
||
e.Radius,
|
||
e.Quadrant
|
||
);
|
||
|
||
return new
|
||
{
|
||
baseEdge.edgeId,
|
||
baseEdge.sequenceId,
|
||
baseEdge.released,
|
||
baseEdge.startNodeId,
|
||
baseEdge.endNodeId,
|
||
|
||
trajectory = result.Trajectory,
|
||
actions = Array.Empty<object>()
|
||
};
|
||
})
|
||
.ToArray();
|
||
|
||
// ================= FINAL SCHEMA OBJECT =================
|
||
return new
|
||
{
|
||
headerId = HeaderId++,
|
||
|
||
timestamp = string.IsNullOrWhiteSpace(Timestamp)
|
||
? DateTime.UtcNow.ToString("O")
|
||
: Timestamp,
|
||
|
||
version = Version,
|
||
manufacturer = Manufacturer,
|
||
serialNumber = SerialNumber,
|
||
|
||
orderId = OrderId= Guid.NewGuid().ToString(),
|
||
orderUpdateId = OrderUpdateId,
|
||
|
||
zoneSetId = string.IsNullOrWhiteSpace(ZoneSetId)
|
||
? null
|
||
: ZoneSetId,
|
||
|
||
nodes = nodeObjects,
|
||
edges = edgeObjects
|
||
};
|
||
}
|
||
}
|
||
|
||
// ======================================================
|
||
// UI ACTION PARAM
|
||
// ======================================================
|
||
public class UiActionParameter : ActionParameter
|
||
{
|
||
[JsonIgnore]
|
||
public string ValueString
|
||
{
|
||
get => Value?.ToString() ?? "";
|
||
set => Value = value;
|
||
}
|
||
}
|