Initial commit

2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/VDA5050ToOrderDataConverter.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/VDA5050ToOrderDataConverter.cs
@@ -0,0 +1,107 @@
+using System.Text.Json;
+using RobotNet.VDA5050.Type;
+
+namespace RobotNet10.RobotApp.Navigation;
+
+// Use aliases so Node/Edge resolve to VDA5050 (this file is in Navigation namespace which has native Node/Edge structs).
+using VdaNode = RobotNet.VDA5050.Order.Node;
+using VdaEdge = RobotNet.VDA5050.Order.Edge;
+using VdaTrajectory = RobotNet.VDA5050.Order.Trajectory;
+using VdaOrderMsg = RobotNet.VDA5050.Order.OrderMsg;
+
+/// <summary>
+/// Converts VDA5050 order (Node[]/Edge[] from MQTT) to OrderData for Navigation C API (MoveToOrder).
+/// </summary>
+public static class VDA5050ToOrderDataConverter
+{
+    /// <summary>
+    /// Convert VDA5050 nodes and edges (e.g. from OrderMsg received via MQTT) to OrderData for navigation.
+    /// </summary>
+    public static OrderData ToOrderData(VdaNode[] nodes, VdaEdge[] edges, VdaOrderMsg? orderMsg = null)
+    {
+        var data = new OrderData
+        {
+            HeaderId = (int)(orderMsg?.HeaderId ?? 0),
+            Timestamp = orderMsg?.Timestamp.ToString("o") ?? DateTime.UtcNow.ToString("o"),
+            Version = orderMsg?.Version ?? "2.1.0",
+            Manufacturer = orderMsg?.Manufacturer ?? string.Empty,
+            SerialNumber = orderMsg?.SerialNumber ?? string.Empty,
+            OrderId = orderMsg?.OrderId ?? string.Empty,
+            OrderUpdateId = (uint)(orderMsg?.OrderUpdateId ?? 0),
+            ZoneSetId = orderMsg?.ZoneSetId,
+            Nodes = new List<OrderNodeData>(nodes.Length),
+            Edges = new List<OrderEdgeData>(edges.Length)
+        };
+
+        foreach (var n in nodes)
+            data.Nodes.Add(ToOrderNodeData(n));
+        foreach (var e in edges)
+            data.Edges.Add(ToOrderEdgeData(e));
+
+        return data;
+    }
+
+    private static OrderNodeData ToOrderNodeData(VdaNode n)
+    {
+        var node = new OrderNodeData
+        {
+            NodeId = n.NodeId,
+            SequenceId = n.SequenceId,
+            NodeDescription = n.NodeDescription ?? string.Empty,
+            Released = n.Released,
+            NodePosition = n.NodePosition != null ? new OrderNodePositionData
+            {
+                X = n.NodePosition.X,
+                Y = n.NodePosition.Y,
+                Theta = n.NodePosition.Theta,
+                AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
+                AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
+                MapId = n.NodePosition.MapId,
+                MapDescription = n.NodePosition.MapDescription
+            } : null,
+            Actions = new List<OrderActionData>()
+        };
+        return node;
+    }
+
+    private static OrderEdgeData ToOrderEdgeData(VdaEdge e)
+    {
+        var edge = new OrderEdgeData
+        {
+            EdgeId = e.EdgeId,
+            SequenceId = e.SequenceId,
+            EdgeDescription = e.EdgeDescription,
+            Released = e.Released,
+            StartNodeId = e.StartNodeId,
+            EndNodeId = e.EndNodeId,
+            MaxSpeed = e.MaxSpeed,
+            MaxHeight = e.MaxHeight,
+            MinHeight = e.MinHeight,
+            Orientation = e.Orientation ?? 0,
+            OrientationType = e.OrientationType.HasValue ? JsonSerializer.SerializeToElement((int)e.OrientationType.Value) : null,
+            Direction = e.Direction ?? string.Empty,
+            RotationAllowed = e.RotationAllowed,
+            MaxRotationSpeed = e.MaxRotationSpeed,
+            Length = e.Length ?? 0,
+            Trajectory = e.Trajectory != null ? ToOrderTrajectoryData(e.Trajectory) : null,
+            Actions = new List<OrderActionData>()
+        };
+        return edge;
+    }
+
+    private static OrderTrajectoryData? ToOrderTrajectoryData(VdaTrajectory? t)
+    {
+        if (t == null) return null;
+        return new OrderTrajectoryData
+        {
+            Degree = (uint)t.Degree,
+            KnotVector = t.KnotVector?.ToList() ?? new List<double>(),
+            ControlPoints = t.ControlPoints?.Select(cp => new OrderControlPointData
+            {
+                X = cp.X,
+                Y = cp.Y,
+                Weight = cp.Weight ?? 1.0
+            }).ToList() ?? new List<OrderControlPointData>()
+        };
+    }
+}