463 lines
21 KiB
C#
463 lines
21 KiB
C#
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.Order;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
using RobotNet10.GlobalPathPlanner.Model;
|
|
using RobotNet10.MapManager.Data;
|
|
using Edge = RobotNet10.MapManager.Data.Edge;
|
|
using Node = RobotNet10.MapManager.Data.Node;
|
|
|
|
namespace RobotNet10.FleetManager.Services.TrafficControl.Helpers;
|
|
|
|
/// <summary>
|
|
/// Helper class to convert path planner results to RobotRoute
|
|
/// </summary>
|
|
public static class RouteConverter
|
|
{
|
|
/// <summary>
|
|
/// Convert path planner result (GlobalNode[], GlobalEdge[]) to RobotRoute
|
|
/// </summary>
|
|
public static RobotRoute ConvertToRobotRoute(
|
|
string robotId,
|
|
GlobalNode[] pathNodes,
|
|
GlobalEdge[] pathEdges,
|
|
List<Node> allNodes,
|
|
List<Edge> allEdges,
|
|
double? lastAngle,
|
|
object? logger = null, // Accept any logger type for flexibility
|
|
Guid? vehicleTypeId = null, // VehicleTypeId to get VehicleProperties
|
|
string? mapId = null) // MapId (LevelId as string) for NodePosition
|
|
{
|
|
// Log virtual node creation
|
|
if (logger != null)
|
|
{
|
|
try
|
|
{
|
|
var loggerType = logger.GetType();
|
|
var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
|
|
infoMethod?.Invoke(logger, [$"ConvertToRobotRoute: {pathNodes.Length} node, {pathEdges.Length}"]);
|
|
}
|
|
catch { }
|
|
}
|
|
var route = new RobotRoute
|
|
{
|
|
RobotId = robotId,
|
|
OrderId = Guid.NewGuid().ToString(), // Generate new order ID
|
|
OrderUpdateId = 0,
|
|
CreatedAt = DateTime.UtcNow,
|
|
LastUpdated = DateTime.UtcNow
|
|
};
|
|
|
|
var segments = new List<RouteSegment>();
|
|
var nodeMap = allNodes.ToDictionary(n => n.Id, n => n);
|
|
var edgeMap = allEdges.ToDictionary(e => e.Id, e => e);
|
|
|
|
// Get LevelId from first available node (for creating virtual nodes/edges)
|
|
var levelId = allNodes.FirstOrDefault()?.LevelId ?? Guid.Empty;
|
|
|
|
// Create segments following VDA5050 pattern:
|
|
// Node (seq 0), Edge (seq 1), Node (seq 2), Edge (seq 3), ..., Node (seq N)
|
|
// Route: n nodes, n-1 edges
|
|
|
|
int sequenceId = 0;
|
|
|
|
for (int i = 0; i < pathNodes.Length; i++)
|
|
{
|
|
var globalNode = pathNodes[i];
|
|
|
|
// Check if node exists in map, if not (first node when robot is on edge), create virtual node
|
|
if (!nodeMap.TryGetValue(globalNode.Id, out Node? node))
|
|
{
|
|
// This is a virtual node created by A* when robot is on an edge
|
|
// Only the first node can be virtual
|
|
if (i == 0)
|
|
{
|
|
// Get LevelId from next node if available, otherwise use from allNodes
|
|
if (pathNodes.Length > 1 && nodeMap.TryGetValue(pathNodes[1].Id, out var nextNode))
|
|
{
|
|
levelId = nextNode.LevelId;
|
|
}
|
|
|
|
// Create virtual node from GlobalNode
|
|
node = new Node
|
|
{
|
|
Id = globalNode.Id,
|
|
LevelId = levelId,
|
|
NodeId = globalNode.Id.ToString(),
|
|
NodeName = "Virtual Start Node",
|
|
X = globalNode.X,
|
|
Y = globalNode.Y
|
|
};
|
|
|
|
// Log virtual node creation
|
|
if (logger != null)
|
|
{
|
|
try
|
|
{
|
|
var loggerType = logger.GetType();
|
|
var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
|
|
infoMethod?.Invoke(logger, [$"Created virtual start node {globalNode.Id} at ({globalNode.X:F2}, {globalNode.Y:F2}) - robot is on edge"]);
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Node not found and not first node - this is an error
|
|
if (logger != null)
|
|
{
|
|
try
|
|
{
|
|
var loggerType = logger.GetType();
|
|
var errorMethod = loggerType.GetMethod("Error", [typeof(string)]);
|
|
errorMethod?.Invoke(logger, [$"Node {globalNode.Id} not found in map at index {i}"]);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore if Error method doesn't exist
|
|
}
|
|
}
|
|
throw new InvalidOperationException($"Node {globalNode.Id} not found in map at index {i}");
|
|
}
|
|
}
|
|
|
|
// Get NodeVehicleProperty for this vehicle type if available
|
|
NodeVehicleProperty? nodeVehicleProperty = null;
|
|
if (vehicleTypeId.HasValue && node.VehicleProperties != null)
|
|
{
|
|
nodeVehicleProperty = node.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
|
|
}
|
|
|
|
// Parse actions from NodeVehicleProperty
|
|
RobotNet.VDA5050.InstantAction.Action[] nodeActions = [];
|
|
if (!string.IsNullOrEmpty(nodeVehicleProperty?.Actions))
|
|
{
|
|
try
|
|
{
|
|
var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]>(nodeVehicleProperty.Actions, JsonOptionExtends.Read);
|
|
if (parsedActions != null)
|
|
{
|
|
nodeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
|
|
{
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
ActionDescription = a.ActionDescription,
|
|
ActionParameters = [..a.ActionParameters],
|
|
BlockingType = a.BlockingType,
|
|
ActionType = a.ActionType
|
|
})];
|
|
}
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
// Create VDA5050 Node with full information
|
|
var vdaNode = new RobotNet.VDA5050.Order.Node
|
|
{
|
|
NodeId = node.Id.ToString(),
|
|
SequenceId = sequenceId,
|
|
Released = false,
|
|
NodeDescription = node.NodeDescription ?? string.Empty,
|
|
NodePosition = new NodePosition
|
|
{
|
|
X = node.X,
|
|
Y = node.Y,
|
|
Theta = i == pathNodes.Length - 1 && lastAngle.HasValue ? lastAngle.Value : nodeVehicleProperty?.Theta,
|
|
AllowedDeviationXY = nodeVehicleProperty?.AllowedDeviationXY,
|
|
AllowedDeviationTheta = nodeVehicleProperty?.AllowedDeviationTheta,
|
|
MapId = node.MapId ?? mapId ?? string.Empty
|
|
},
|
|
Actions = nodeActions
|
|
};
|
|
|
|
// Add node segment (even sequence IDs) with VDA5050 Node
|
|
var nodeSegment = new RouteSegment
|
|
{
|
|
NodeId = node.Id,
|
|
EdgeId = null,
|
|
StartNodeId = node.Id,
|
|
EndNodeId = null,
|
|
Released = false,
|
|
VdaNode = vdaNode,
|
|
VdaEdge = null
|
|
};
|
|
|
|
segments.Add(nodeSegment);
|
|
sequenceId++;
|
|
|
|
// Add edge segment if not the last node
|
|
if (i < pathNodes.Length - 1 && i < pathEdges.Length)
|
|
{
|
|
var globalEdge = pathEdges[i];
|
|
|
|
// Find edge by Id or by StartNodeId and EndNodeId
|
|
Edge? edge = allEdges.FirstOrDefault(e => e.StartNodeId == globalEdge.StartNodeId &&
|
|
e.EndNodeId == globalEdge.EndNodeId);
|
|
if (edge is null && edgeMap.TryGetValue(globalEdge.Id, out Edge? value))
|
|
{
|
|
edge = value;
|
|
if(i == 0)
|
|
{
|
|
edge.StartNodeId = globalEdge.StartNodeId;
|
|
edge.EndNodeId = globalEdge.EndNodeId;
|
|
edge.VehicleProperties = [];
|
|
}
|
|
}
|
|
|
|
// If edge not found and this is the first edge (i == 0), create virtual edge
|
|
if (edge == null && i == 0)
|
|
{
|
|
// Create virtual edge from GlobalEdge
|
|
edge = new Edge
|
|
{
|
|
Id = globalEdge.Id,
|
|
LevelId = levelId,
|
|
EdgeId = globalEdge.Id.ToString(), // Virtual edge identifier
|
|
StartNodeId = globalEdge.StartNodeId, // Current node (may be virtual)
|
|
EndNodeId = globalEdge.EndNodeId,
|
|
EdgeDescription = "Virtual Start Edge",
|
|
};
|
|
|
|
// Log virtual edge creation
|
|
if (logger != null)
|
|
{
|
|
try
|
|
{
|
|
var loggerType = logger.GetType();
|
|
var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
|
|
infoMethod?.Invoke(logger, [$"Created virtual start edge from node {globalEdge.StartNodeId} to node {globalEdge.EndNodeId} - robot is on edge"]);
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
|
|
if (edge != null)
|
|
{
|
|
// Get EdgeVehicleProperty for this vehicle type if available
|
|
EdgeVehicleProperty? edgeVehicleProperty = null;
|
|
if (vehicleTypeId.HasValue && edge.VehicleProperties != null)
|
|
{
|
|
edgeVehicleProperty = edge.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
|
|
}
|
|
|
|
// Calculate edge length (Euclidean distance between start and end nodes)
|
|
var dx = pathNodes[i].X - pathNodes[i + 1].X;
|
|
var dy = pathNodes[i].Y - pathNodes[i + 1].Y;
|
|
double edgeLength = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
// Build trajectory from EdgeVehicleProperty fields
|
|
var startNode = pathNodes[i];
|
|
var endNode = pathNodes[i + 1];
|
|
Trajectory? trajectory = null;
|
|
if (edgeVehicleProperty?.TrajectoryDegree.HasValue == true)
|
|
{
|
|
var degree = edgeVehicleProperty.TrajectoryDegree.Value;
|
|
|
|
// Build control points array based on degree
|
|
List<ControlPoint> controlPoints =
|
|
[
|
|
// Always add start node as first control point
|
|
new() {
|
|
X = startNode.X,
|
|
Y = startNode.Y,
|
|
Weight = 1.0
|
|
}
|
|
];
|
|
|
|
// Add control point 1 for degree 2 and 3
|
|
if (degree >= 2 && edgeVehicleProperty.TrajectoryControlPoint1X.HasValue && edgeVehicleProperty.TrajectoryControlPoint1Y.HasValue)
|
|
{
|
|
controlPoints.Add(new ControlPoint
|
|
{
|
|
X = edgeVehicleProperty.TrajectoryControlPoint1X.Value,
|
|
Y = edgeVehicleProperty.TrajectoryControlPoint1Y.Value,
|
|
Weight = 1.0
|
|
});
|
|
}
|
|
else if (degree >= 2)
|
|
{
|
|
// Default: midpoint between start and end
|
|
controlPoints.Add(new ControlPoint
|
|
{
|
|
X = (startNode.X + endNode.X) / 2.0,
|
|
Y = (startNode.Y + endNode.Y) / 2.0,
|
|
Weight = 1.0
|
|
});
|
|
}
|
|
|
|
// Add control point 2 for degree 3
|
|
if (degree >= 3 && edgeVehicleProperty.TrajectoryControlPoint2X.HasValue && edgeVehicleProperty.TrajectoryControlPoint2Y.HasValue)
|
|
{
|
|
controlPoints.Add(new ControlPoint
|
|
{
|
|
X = edgeVehicleProperty.TrajectoryControlPoint2X.Value,
|
|
Y = edgeVehicleProperty.TrajectoryControlPoint2Y.Value,
|
|
Weight = 1.0
|
|
});
|
|
}
|
|
else if (degree >= 3)
|
|
{
|
|
// Default: one-third point from start
|
|
controlPoints.Add(new ControlPoint
|
|
{
|
|
X = startNode.X + (endNode.X - startNode.X) / 3.0,
|
|
Y = startNode.Y + (endNode.Y - startNode.Y) / 3.0,
|
|
Weight = 1.0
|
|
});
|
|
}
|
|
|
|
// Always add end node as last control point
|
|
controlPoints.Add(new ControlPoint
|
|
{
|
|
X = endNode.X,
|
|
Y = endNode.Y,
|
|
Weight = 1.0
|
|
});
|
|
|
|
// Build knot vector based on degree
|
|
double[] knotVector = degree switch
|
|
{
|
|
1 => [0, 0, 1, 1],
|
|
2 => [0, 0, 0, 1, 1, 1],
|
|
3 => [0, 0, 0, 0, 1, 1, 1, 1],
|
|
_ => [0, 0, 1, 1] // Default to degree 1
|
|
};
|
|
|
|
trajectory = new Trajectory
|
|
{
|
|
Degree = degree,
|
|
KnotVector = knotVector,
|
|
ControlPoints = [.. controlPoints]
|
|
};
|
|
}
|
|
|
|
// Build corridor from EdgeVehicleProperty fields
|
|
Corridor? corridor = null;
|
|
if (edgeVehicleProperty != null &&
|
|
(edgeVehicleProperty.CorridorLeftWidth.HasValue ||
|
|
edgeVehicleProperty.CorridorRightWidth.HasValue ||
|
|
edgeVehicleProperty.CorridorRefPoint.HasValue))
|
|
{
|
|
corridor = new Corridor
|
|
{
|
|
LeftWidth = edgeVehicleProperty.CorridorLeftWidth ?? 0.0,
|
|
RightWidth = edgeVehicleProperty.CorridorRightWidth ?? 0.0,
|
|
CorridorRefPoint = edgeVehicleProperty.CorridorRefPoint ?? RobotNet.VDA5050.Type.CorridorRefPoint.KINEMATICCENTER
|
|
};
|
|
}
|
|
|
|
// Parse actions from EdgeVehicleProperty
|
|
RobotNet.VDA5050.InstantAction.Action[] edgeActions = [];
|
|
if (!string.IsNullOrEmpty(edgeVehicleProperty?.Actions))
|
|
{
|
|
try
|
|
{
|
|
var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]?>(edgeVehicleProperty.Actions, JsonOptionExtends.Read);
|
|
if (parsedActions != null)
|
|
{
|
|
edgeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
|
|
{
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
ActionDescription = a.ActionDescription,
|
|
ActionParameters = [..a.ActionParameters],
|
|
BlockingType = a.BlockingType,
|
|
ActionType = a.ActionType
|
|
})];
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
// Ignore parse errors, use empty array
|
|
}
|
|
}
|
|
|
|
// tính toán orientation cho robot
|
|
|
|
// Create VDA5050 Edge with full information
|
|
var vdaEdge = new RobotNet.VDA5050.Order.Edge
|
|
{
|
|
EdgeId = edge.Id.ToString(),
|
|
SequenceId = sequenceId,
|
|
Released = false,
|
|
EdgeDescription = edge.EdgeDescription,
|
|
StartNodeId = edge.StartNodeId.ToString(),
|
|
EndNodeId = edge.EndNodeId.ToString(),
|
|
MaxSpeed = edgeVehicleProperty?.MaxSpeed,
|
|
MaxHeight = edgeVehicleProperty?.MaxHeight,
|
|
MinHeight = edgeVehicleProperty?.MinHeight ,
|
|
Orientation = startNode.Orientation == Orientation.FORWARD ? 0 : startNode.Orientation == Orientation.BACKWARD ? Math.PI : null,
|
|
OrientationType = RobotNet.VDA5050.Type.OrientationType.TANGENTIAL,
|
|
Direction = string.Empty, // Not in EdgeVehicleProperty
|
|
RotationAllowed = edgeVehicleProperty?.RotationAllowed ,
|
|
MaxRotationSpeed = edgeVehicleProperty?.MaxRotationSpeed,
|
|
Length = edgeLength,
|
|
Trajectory = trajectory,
|
|
Corridor = corridor,
|
|
Actions = edgeActions
|
|
};
|
|
|
|
// Add edge segment (odd sequence IDs) with VDA5050 Edge
|
|
var edgeSegment = new RouteSegment
|
|
{
|
|
NodeId = edge.EndNodeId, // Target node of this edge
|
|
EdgeId = edge.Id,
|
|
StartNodeId = edge.StartNodeId,
|
|
EndNodeId = edge.EndNodeId,
|
|
Released = false,
|
|
VdaNode = null, // Edge segment doesn't have node
|
|
VdaEdge = vdaEdge
|
|
};
|
|
|
|
segments.Add(edgeSegment);
|
|
sequenceId++;
|
|
}
|
|
else
|
|
{
|
|
// Edge not found and not first edge - this is an error
|
|
if (logger != null)
|
|
{
|
|
try
|
|
{
|
|
var loggerType = logger.GetType();
|
|
var warningMethod = loggerType.GetMethod("Warning", [typeof(string)]);
|
|
warningMethod?.Invoke(logger, [$"Edge not found for path segment {i}: StartNodeId={globalEdge.StartNodeId}, EndNodeId={globalEdge.EndNodeId}"]);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore if Warning method doesn't exist
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
route.FullRoute = segments;
|
|
route.CurrentSegmentIndex = 0;
|
|
|
|
return route;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Split route into Base and Horizon
|
|
/// </summary>
|
|
public static void SplitRouteIntoBaseAndHorizon(RobotRoute route, int baseSegmentCount)
|
|
{
|
|
if (route.FullRoute.Count == 0)
|
|
return;
|
|
|
|
// Ensure baseSegmentCount doesn't exceed available segments
|
|
var actualBaseCount = Math.Min(baseSegmentCount, route.FullRoute.Count - 1);
|
|
if (actualBaseCount < 1)
|
|
actualBaseCount = 1; // At least 1 segment in base
|
|
|
|
// Split: Base gets first N segments, Horizon gets the rest
|
|
route.Base = [.. route.FullRoute.Take(actualBaseCount)];
|
|
route.Horizon = [.. route.FullRoute.Skip(actualBaseCount)];
|
|
|
|
// Mark base segments as released
|
|
foreach (var segment in route.Base)
|
|
{
|
|
segment.Released = true;
|
|
}
|
|
}
|
|
}
|
|
|