216 lines
8.8 KiB
C#
216 lines
8.8 KiB
C#
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;
|
|
}
|
|
}
|