Initial commit
This commit is contained in:
@@ -0,0 +1,215 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user