108 lines
4.1 KiB
C#
108 lines
4.1 KiB
C#
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>()
|
|
};
|
|
}
|
|
}
|