update
This commit is contained in:
parent
49c0c1ab39
commit
5c1851e92f
|
|
@ -270,7 +270,7 @@
|
||||||
</MudChip>
|
</MudChip>
|
||||||
</MudTd>
|
</MudTd>
|
||||||
<MudTd>
|
<MudTd>
|
||||||
<MudText Typo="Typo.body2" Class="text-truncate" Style="max-width: 300px;" Title="@context.Description">
|
<MudText Typo="Typo.body2" Class="text-truncate" Style="max-width: 300px;">
|
||||||
@context.Description
|
@context.Description
|
||||||
</MudText>
|
</MudText>
|
||||||
</MudTd>
|
</MudTd>
|
||||||
|
|
@ -309,7 +309,7 @@
|
||||||
<MudTh Style="text-align:right">Status</MudTh>
|
<MudTh Style="text-align:right">Status</MudTh>
|
||||||
</HeaderContent>
|
</HeaderContent>
|
||||||
<RowTemplate>
|
<RowTemplate>
|
||||||
<MudTd><MudText Typo="Typo.body2" Class="text-truncate" Title="@context.ActionType">@context.ActionType</MudText></MudTd>
|
<MudTd><MudText Typo="Typo.body2" Class="text-truncate">@context.ActionType</MudText></MudTd>
|
||||||
<MudTd><MudText Typo="Typo.caption">@context.ActionId</MudText></MudTd>
|
<MudTd><MudText Typo="Typo.caption">@context.ActionId</MudText></MudTd>
|
||||||
<MudTd Style="text-align:right">
|
<MudTd Style="text-align:right">
|
||||||
<MudChip T="string"
|
<MudChip T="string"
|
||||||
|
|
|
||||||
|
|
@ -38,8 +38,7 @@
|
||||||
<MudIconButton Icon="@Icons.Material.Filled.Delete"
|
<MudIconButton Icon="@Icons.Material.Filled.Delete"
|
||||||
Color="Color.Error"
|
Color="Color.Error"
|
||||||
Size="Size.Small"
|
Size="Size.Small"
|
||||||
OnClick="@(() => RemoveEdgeAsync(edge))"
|
OnClick="@(() => RemoveEdgeAsync(edge))" />
|
||||||
StopPropagation="true" />
|
|
||||||
</div>
|
</div>
|
||||||
</TitleContent>
|
</TitleContent>
|
||||||
|
|
||||||
|
|
@ -88,47 +87,6 @@
|
||||||
}
|
}
|
||||||
</MudSelect>
|
</MudSelect>
|
||||||
</MudItem>
|
</MudItem>
|
||||||
|
|
||||||
@* <!-- Radius -->
|
|
||||||
<MudItem xs="6">
|
|
||||||
<MudNumericField T="double"
|
|
||||||
Value="@edge.Radius"
|
|
||||||
ValueChanged="@((double v) => SetValue(() => edge.Radius = v))"
|
|
||||||
Immediate="true"
|
|
||||||
Min="0"
|
|
||||||
Step="0.1"
|
|
||||||
Label="Radius (0 = straight line)" />
|
|
||||||
</MudItem>
|
|
||||||
|
|
||||||
<!-- Quadrant -->
|
|
||||||
@if (edge.Radius > 0)
|
|
||||||
{
|
|
||||||
<MudItem xs="6">
|
|
||||||
<MudSelect T="Quadrant"
|
|
||||||
Value="@edge.Quadrant"
|
|
||||||
ValueChanged="@((Quadrant v) => SetValue(() => edge.Quadrant = v))"
|
|
||||||
Label="Quadrant">
|
|
||||||
<MudSelectItem Value="Quadrant.I">I</MudSelectItem>
|
|
||||||
<MudSelectItem Value="Quadrant.II">II</MudSelectItem>
|
|
||||||
<MudSelectItem Value="Quadrant.III">III</MudSelectItem>
|
|
||||||
<MudSelectItem Value="Quadrant.IV">IV</MudSelectItem>
|
|
||||||
</MudSelect>
|
|
||||||
</MudItem>
|
|
||||||
}
|
|
||||||
|
|
||||||
<!-- Apply Curve -->
|
|
||||||
@if (!edge.HasTrajectory && edge.Radius > 0 && !edge.Expanded)
|
|
||||||
{
|
|
||||||
<MudItem xs="12">
|
|
||||||
<MudButton Color="Color.Primary"
|
|
||||||
Variant="Variant.Outlined"
|
|
||||||
StartIcon="@Icons.Material.Filled.Merge"
|
|
||||||
OnClick="@(() => ApplyCurveAsync(edge))">
|
|
||||||
Apply Curve (generate node)
|
|
||||||
</MudButton>
|
|
||||||
</MudItem>
|
|
||||||
} *@
|
|
||||||
|
|
||||||
</MudGrid>
|
</MudGrid>
|
||||||
</ChildContent>
|
</ChildContent>
|
||||||
</MudExpansionPanel>
|
</MudExpansionPanel>
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,7 @@
|
||||||
<MudPaper Class="pa-4 h-100 d-flex flex-column overflow-hidden" Elevation="2">
|
<MudPaper Class="pa-4 h-100 d-flex flex-column overflow-hidden" Elevation="2">
|
||||||
<MudStack Row AlignItems="AlignItems.Center" Justify="Justify.SpaceBetween"
|
<MudStack Row AlignItems="AlignItems.Center" Justify="Justify.SpaceBetween"
|
||||||
Class="mb-4 flex-shrink-0">
|
Class="mb-4 flex-shrink-0">
|
||||||
<MudText Typo="Typo.h6">📄 JSON Output (/order)</MudText>
|
<MudText Typo="Typo.h6">Output (/order)</MudText>
|
||||||
|
|
||||||
<div class="d-flex gap-2">
|
<div class="d-flex gap-2">
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -86,6 +86,7 @@
|
||||||
PropertyNamingPolicy = JsonNamingPolicy.CamelCase,
|
PropertyNamingPolicy = JsonNamingPolicy.CamelCase,
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
private async Task OpenImportDialog()
|
private async Task OpenImportDialog()
|
||||||
{
|
{
|
||||||
var dialog = await DialogService.ShowAsync<ImportOrderDialog>(
|
var dialog = await DialogService.ShowAsync<ImportOrderDialog>(
|
||||||
|
|
@ -177,22 +178,19 @@
|
||||||
|
|
||||||
void RemoveAction(Node node, VDA5050.InstantAction.Action action)
|
void RemoveAction(Node node, VDA5050.InstantAction.Action action)
|
||||||
{
|
{
|
||||||
node.Actions = node.Actions?.Where(a => a != action).ToArray()
|
node.Actions = node.Actions?.Where(a => a != action).ToArray() ?? [];
|
||||||
?? Array.Empty<VDA5050.InstantAction.Action>();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AddActionParameter(VDA5050.InstantAction.Action act)
|
void AddActionParameter(VDA5050.InstantAction.Action act)
|
||||||
{
|
{
|
||||||
var list = (act.ActionParameters ?? Array.Empty<ActionParameter>()).ToList();
|
var list = (act.ActionParameters ?? []).ToList();
|
||||||
list.Add(new UiActionParameter());
|
list.Add(new UiActionParameter());
|
||||||
act.ActionParameters = list.ToArray();
|
act.ActionParameters = list.ToArray();
|
||||||
}
|
}
|
||||||
|
|
||||||
void RemoveActionParameter(VDA5050.InstantAction.Action act, ActionParameter param)
|
void RemoveActionParameter(VDA5050.InstantAction.Action act, ActionParameter param)
|
||||||
{
|
{
|
||||||
act.ActionParameters =
|
act.ActionParameters = act.ActionParameters?.Where(p => p != param).ToArray() ?? [];
|
||||||
act.ActionParameters?.Where(p => p != param).ToArray()
|
|
||||||
?? Array.Empty<ActionParameter>();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ================= SEND / COPY =================
|
// ================= SEND / COPY =================
|
||||||
|
|
@ -302,7 +300,7 @@
|
||||||
_copyCts?.Cancel();
|
_copyCts?.Cancel();
|
||||||
_copyCts = new();
|
_copyCts = new();
|
||||||
|
|
||||||
await JS.InvokeVoidAsync("navigator.clipboard.writeText", OrderJson);
|
await JS.InvokeVoidAsync("copyToClipboardFallback", OrderJson);
|
||||||
|
|
||||||
copied = true;
|
copied = true;
|
||||||
StateHasChanged();
|
StateHasChanged();
|
||||||
|
|
|
||||||
|
|
@ -1,127 +1,9 @@
|
||||||
using RobotApp.VDA5050.InstantAction;
|
using RobotApp.VDA5050.InstantAction;
|
||||||
using RobotApp.VDA5050.Order;
|
using RobotApp.VDA5050.Order;
|
||||||
using System.Text.Json;
|
|
||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
using System.Xml.Linq;
|
|
||||||
|
|
||||||
namespace RobotApp.Client.Services;
|
namespace RobotApp.Client.Services;
|
||||||
|
|
||||||
// ======================================================
|
|
||||||
// EDGE UI
|
|
||||||
// ======================================================
|
|
||||||
public class UiEdge
|
|
||||||
{
|
|
||||||
public string EdgeId { get; set; } = "";
|
|
||||||
public int SequenceId { get; set; }
|
|
||||||
public bool Released { get; set; } = true;
|
|
||||||
|
|
||||||
public string StartNodeId { get; set; } = "";
|
|
||||||
public string EndNodeId { get; set; } = "";
|
|
||||||
|
|
||||||
// ===== CURVE (EDITOR GENERATED) =====
|
|
||||||
public double Radius { get; set; } = 0;
|
|
||||||
public Quadrant Quadrant { get; set; }
|
|
||||||
|
|
||||||
// ===== IMPORTED TRAJECTORY =====
|
|
||||||
public bool HasTrajectory { get; set; } = false;
|
|
||||||
public UiTrajectory? Trajectory { get; set; }
|
|
||||||
|
|
||||||
// ===== UI STATE =====
|
|
||||||
public bool Expanded { get; private set; } = false;
|
|
||||||
|
|
||||||
public void MarkExpanded()
|
|
||||||
{
|
|
||||||
Expanded = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public class UiTrajectory
|
|
||||||
{
|
|
||||||
public int Degree { get; set; }
|
|
||||||
public double[] KnotVector { get; set; } = Array.Empty<double>();
|
|
||||||
public List<Point> ControlPoints { get; set; } = new();
|
|
||||||
}
|
|
||||||
|
|
||||||
public enum Quadrant
|
|
||||||
{
|
|
||||||
I,
|
|
||||||
II,
|
|
||||||
III,
|
|
||||||
IV
|
|
||||||
}
|
|
||||||
|
|
||||||
// ======================================================
|
|
||||||
// GEOMETRY MODELS
|
|
||||||
// ======================================================
|
|
||||||
public record Point(double X, double Y);
|
|
||||||
|
|
||||||
public record QuarterResult(
|
|
||||||
Point EndPoint,
|
|
||||||
object Trajectory
|
|
||||||
);
|
|
||||||
|
|
||||||
// ======================================================
|
|
||||||
// GEOMETRY HELPER (QUARTER CIRCLE)
|
|
||||||
// ======================================================
|
|
||||||
public static class QuarterGeometry
|
|
||||||
{
|
|
||||||
private const double K = 0.5522847498307936;
|
|
||||||
|
|
||||||
public static QuarterResult BuildQuarterTrajectory(
|
|
||||||
Point A,
|
|
||||||
double r,
|
|
||||||
Quadrant q
|
|
||||||
)
|
|
||||||
{
|
|
||||||
Point P1, P2, C;
|
|
||||||
|
|
||||||
switch (q)
|
|
||||||
{
|
|
||||||
case Quadrant.I:
|
|
||||||
P1 = new(A.X, A.Y + K * r);
|
|
||||||
P2 = new(A.X + K * r, A.Y + r);
|
|
||||||
C = new(A.X + r, A.Y + r);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case Quadrant.II:
|
|
||||||
P1 = new(A.X - K * r, A.Y);
|
|
||||||
P2 = new(A.X - r, A.Y + K * r);
|
|
||||||
C = new(A.X - r, A.Y + r);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case Quadrant.III:
|
|
||||||
P1 = new(A.X, A.Y - K * r);
|
|
||||||
P2 = new(A.X - K * r, A.Y - r);
|
|
||||||
C = new(A.X - r, A.Y - r);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case Quadrant.IV:
|
|
||||||
P1 = new(A.X + K * r, A.Y);
|
|
||||||
P2 = new(A.X + r, A.Y - K * r);
|
|
||||||
C = new(A.X + r, A.Y - r);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
throw new ArgumentOutOfRangeException(nameof(q));
|
|
||||||
}
|
|
||||||
|
|
||||||
return new QuarterResult(
|
|
||||||
C,
|
|
||||||
new
|
|
||||||
{
|
|
||||||
degree = 3,
|
|
||||||
knotVector = new[] { 0, 0, 0, 0, 1, 1, 1, 1 },
|
|
||||||
controlPoints = new[]
|
|
||||||
{
|
|
||||||
new { x = A.X, y = A.Y }, // P0
|
|
||||||
new { x = P1.X, y = P1.Y }, // P1
|
|
||||||
new { x = P2.X, y = P2.Y }, // P2
|
|
||||||
new { x = C.X, y = C.Y } // P3
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// ======================================================
|
// ======================================================
|
||||||
// ORDER MESSAGE
|
// ORDER MESSAGE
|
||||||
// ======================================================
|
// ======================================================
|
||||||
|
|
@ -141,110 +23,6 @@ public class OrderMessage
|
||||||
|
|
||||||
public OrderMsg ToSchemaObject()
|
public OrderMsg ToSchemaObject()
|
||||||
{
|
{
|
||||||
// ================= SORT NODES BY UI SEQUENCE =================
|
|
||||||
var orderedNodes = Nodes
|
|
||||||
.OrderBy(n => n.SequenceId)
|
|
||||||
.ToList();
|
|
||||||
|
|
||||||
// ================= BUILD NODE OBJECTS =================
|
|
||||||
var nodeObjects = orderedNodes
|
|
||||||
.Select((n, index) => new Node
|
|
||||||
{
|
|
||||||
NodeId = n.NodeId,
|
|
||||||
SequenceId = index * 2, // ✅ NODE = EVEN
|
|
||||||
Released = n.Released,
|
|
||||||
|
|
||||||
NodePosition = new NodePosition
|
|
||||||
{
|
|
||||||
X = n.NodePosition.X,
|
|
||||||
Y = n.NodePosition.Y,
|
|
||||||
Theta = n.NodePosition.Theta,
|
|
||||||
|
|
||||||
AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
|
|
||||||
AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
|
|
||||||
|
|
||||||
MapId = string.IsNullOrWhiteSpace(n.NodePosition.MapId)
|
|
||||||
? "MAP_01"
|
|
||||||
: n.NodePosition.MapId
|
|
||||||
},
|
|
||||||
|
|
||||||
Actions = n.Actions?
|
|
||||||
.Select(a => new VDA5050.InstantAction.Action
|
|
||||||
{
|
|
||||||
ActionId = a.ActionId,
|
|
||||||
ActionType = a.ActionType,
|
|
||||||
BlockingType = a.BlockingType,
|
|
||||||
|
|
||||||
ActionParameters = a.ActionParameters?
|
|
||||||
.Select(p => new ActionParameter
|
|
||||||
{
|
|
||||||
Key = p.Key,
|
|
||||||
Value = p.Value
|
|
||||||
})
|
|
||||||
.ToArray()
|
|
||||||
?? []
|
|
||||||
})
|
|
||||||
.ToArray()
|
|
||||||
?? []
|
|
||||||
})
|
|
||||||
.ToArray();
|
|
||||||
|
|
||||||
// ================= BUILD EDGE OBJECTS =================
|
|
||||||
Edge[] edgeObjects = Edges
|
|
||||||
.Select((e, index) =>
|
|
||||||
{
|
|
||||||
int sequenceId = index * 2 + 1; // ✅ EDGE = ODD
|
|
||||||
|
|
||||||
// ---------- BASE ----------
|
|
||||||
var baseEdge = new
|
|
||||||
{
|
|
||||||
edgeId = e.EdgeId,
|
|
||||||
sequenceId,
|
|
||||||
released = true,
|
|
||||||
startNodeId = e.StartNodeId,
|
|
||||||
endNodeId = e.EndNodeId
|
|
||||||
};
|
|
||||||
|
|
||||||
// =================================================
|
|
||||||
// 1️⃣ IMPORTED TRAJECTORY
|
|
||||||
// =================================================
|
|
||||||
if (e.Trajectory != null)
|
|
||||||
{
|
|
||||||
return new Edge
|
|
||||||
{
|
|
||||||
EdgeId = baseEdge.edgeId,
|
|
||||||
SequenceId = baseEdge.sequenceId,
|
|
||||||
Released= baseEdge.released,
|
|
||||||
StartNodeId= baseEdge.startNodeId,
|
|
||||||
EndNodeId= baseEdge.endNodeId,
|
|
||||||
|
|
||||||
Trajectory = new Trajectory
|
|
||||||
{
|
|
||||||
Degree = e.Trajectory.Degree,
|
|
||||||
KnotVector = e.Trajectory.KnotVector,
|
|
||||||
ControlPoints = e.Trajectory.ControlPoints
|
|
||||||
.Select(p => new ControlPoint { X = p.X, Y = p.Y , Weight = 1})
|
|
||||||
.ToArray()
|
|
||||||
},
|
|
||||||
|
|
||||||
Actions = []
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
return new Edge
|
|
||||||
{
|
|
||||||
EdgeId = baseEdge.edgeId,
|
|
||||||
SequenceId = baseEdge.sequenceId,
|
|
||||||
Released = baseEdge.released,
|
|
||||||
StartNodeId = baseEdge.startNodeId,
|
|
||||||
EndNodeId = baseEdge.endNodeId,
|
|
||||||
|
|
||||||
Actions = []
|
|
||||||
};
|
|
||||||
})
|
|
||||||
.ToArray();
|
|
||||||
|
|
||||||
// ================= FINAL SCHEMA OBJECT =================
|
|
||||||
return new OrderMsg
|
return new OrderMsg
|
||||||
{
|
{
|
||||||
HeaderId = (uint)HeaderId++,
|
HeaderId = (uint)HeaderId++,
|
||||||
|
|
@ -262,8 +40,8 @@ public class OrderMessage
|
||||||
? null
|
? null
|
||||||
: ZoneSetId,
|
: ZoneSetId,
|
||||||
|
|
||||||
Nodes = nodeObjects,
|
Nodes = [..Nodes],
|
||||||
Edges = edgeObjects,
|
Edges = [..Edges],
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1 +1,10 @@
|
||||||
|
window.copyToClipboardFallback = function (text) {
|
||||||
|
const textarea = document.createElement('textarea');
|
||||||
|
textarea.value = text;
|
||||||
|
textarea.style.position = 'fixed';
|
||||||
|
textarea.style.opacity = '0';
|
||||||
|
document.body.appendChild(textarea);
|
||||||
|
textarea.select();
|
||||||
|
document.execCommand('copy');
|
||||||
|
document.body.removeChild(textarea);
|
||||||
|
}
|
||||||
|
|
@ -13,5 +13,5 @@ public class EdgeState
|
||||||
public string EdgeDescription { get; set; } = string.Empty;
|
public string EdgeDescription { get; set; } = string.Empty;
|
||||||
[Required]
|
[Required]
|
||||||
public bool Released { get; set; }
|
public bool Released { get; set; }
|
||||||
public Trajectory Trajectory { get; set; } = new();
|
public Trajectory? Trajectory { get; set; }
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -480,17 +480,22 @@ public class RobotOrderController(INavigation NavigationManager,
|
||||||
for (int i = 0; i < nodes.Count - 1; i++)
|
for (int i = 0; i < nodes.Count - 1; i++)
|
||||||
{
|
{
|
||||||
if (edges[i] is null) return (NodeStates, [.. pathEdges]);
|
if (edges[i] is null) return (NodeStates, [.. pathEdges]);
|
||||||
|
|
||||||
|
var trajectory = edges[i].Trajectory;
|
||||||
|
var controlPoints = trajectory?.ControlPoints;
|
||||||
|
|
||||||
|
|
||||||
pathEdges.Add(new()
|
pathEdges.Add(new()
|
||||||
{
|
{
|
||||||
StartX = nodes[i].NodePosition.X,
|
StartX = nodes[i].NodePosition.X,
|
||||||
StartY = nodes[i].NodePosition.Y,
|
StartY = nodes[i].NodePosition.Y,
|
||||||
EndX = nodes[i + 1].NodePosition.X,
|
EndX = nodes[i + 1].NodePosition.X,
|
||||||
EndY = nodes[i + 1].NodePosition.Y,
|
EndY = nodes[i + 1].NodePosition.Y,
|
||||||
ControlPoint1X = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
|
||||||
Degree = edges[i].Trajectory is null ? 1 : edges[i].Trajectory.Degree,
|
Degree = trajectory is null ? 1 : trajectory.Degree,
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -21,11 +21,11 @@ public class RobotPathPlanner(IConfiguration Configuration)
|
||||||
Y1 = inNode.NodePosition.Y,
|
Y1 = inNode.NodePosition.Y,
|
||||||
X2 = futureNode.NodePosition.X,
|
X2 = futureNode.NodePosition.X,
|
||||||
Y2 = futureNode.NodePosition.Y,
|
Y2 = futureNode.NodePosition.Y,
|
||||||
ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
||||||
TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
|
TrajectoryDegree = edge.Trajectory is null ? TrajectoryDegree.One : edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
|
||||||
});
|
});
|
||||||
(double robotx, double roboty) =
|
(double robotx, double roboty) =
|
||||||
(
|
(
|
||||||
|
|
@ -61,30 +61,46 @@ public class RobotPathPlanner(IConfiguration Configuration)
|
||||||
navigationNodes[0].Direction = GetDirectionInNode(currentTheta, nodes[0], nodes[1], edges[0]);
|
navigationNodes[0].Direction = GetDirectionInNode(currentTheta, nodes[0], nodes[1], edges[0]);
|
||||||
for (int i = 1; i < nodes.Length - 1; i++)
|
for (int i = 1; i < nodes.Length - 1; i++)
|
||||||
{
|
{
|
||||||
|
var trajectory = edges[i - 1].Trajectory;
|
||||||
|
var controlPoints = trajectory?.ControlPoints;
|
||||||
(double lastx, double lasty) = MathExtensions.Curve(0.1, new()
|
(double lastx, double lasty) = MathExtensions.Curve(0.1, new()
|
||||||
{
|
{
|
||||||
X1 = nodes[i - 1].NodePosition.X,
|
X1 = nodes[i - 1].NodePosition.X,
|
||||||
Y1 = nodes[i - 1].NodePosition.Y,
|
Y1 = nodes[i - 1].NodePosition.Y,
|
||||||
X2 = nodes[i].NodePosition.X,
|
X2 = nodes[i].NodePosition.X,
|
||||||
Y2 = nodes[i].NodePosition.Y,
|
Y2 = nodes[i].NodePosition.Y,
|
||||||
ControlPoint1X = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
|
||||||
TrajectoryDegree = edges[i - 1].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i - 1].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
|
TrajectoryDegree = trajectory?.Degree switch
|
||||||
|
{
|
||||||
|
1 => TrajectoryDegree.One,
|
||||||
|
2 => TrajectoryDegree.Two,
|
||||||
|
_ => TrajectoryDegree.Three
|
||||||
|
},
|
||||||
});
|
});
|
||||||
|
|
||||||
|
trajectory = edges[i].Trajectory;
|
||||||
|
controlPoints = trajectory?.ControlPoints;
|
||||||
(double futurex, double futurey) = MathExtensions.Curve(0.1, new()
|
(double futurex, double futurey) = MathExtensions.Curve(0.1, new()
|
||||||
{
|
{
|
||||||
X1 = nodes[i].NodePosition.X,
|
X1 = nodes[i].NodePosition.X,
|
||||||
Y1 = nodes[i].NodePosition.Y,
|
Y1 = nodes[i].NodePosition.Y,
|
||||||
X2 = nodes[i + 1].NodePosition.X,
|
X2 = nodes[i + 1].NodePosition.X,
|
||||||
Y2 = nodes[i + 1].NodePosition.Y,
|
Y2 = nodes[i + 1].NodePosition.Y,
|
||||||
ControlPoint1X = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
|
||||||
TrajectoryDegree = edges[i].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
|
TrajectoryDegree = trajectory?.Degree switch
|
||||||
|
{
|
||||||
|
1 => TrajectoryDegree.One,
|
||||||
|
2 => TrajectoryDegree.Two,
|
||||||
|
_ => TrajectoryDegree.Three
|
||||||
|
},
|
||||||
});
|
});
|
||||||
|
|
||||||
var angle = MathExtensions.GetVectorAngle(
|
var angle = MathExtensions.GetVectorAngle(
|
||||||
nodes[i].NodePosition.X,
|
nodes[i].NodePosition.X,
|
||||||
nodes[i].NodePosition.Y,
|
nodes[i].NodePosition.Y,
|
||||||
|
|
@ -128,11 +144,11 @@ public class RobotPathPlanner(IConfiguration Configuration)
|
||||||
Y1 = startNode.Y,
|
Y1 = startNode.Y,
|
||||||
X2 = endNode.X,
|
X2 = endNode.X,
|
||||||
Y2 = endNode.Y,
|
Y2 = endNode.Y,
|
||||||
ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
||||||
TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three
|
TrajectoryDegree = edge.Trajectory is null ? TrajectoryDegree.One : edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three
|
||||||
};
|
};
|
||||||
|
|
||||||
double length = EdgeCalculatorModel.GetEdgeLength();
|
double length = EdgeCalculatorModel.GetEdgeLength();
|
||||||
|
|
@ -194,11 +210,11 @@ public class RobotPathPlanner(IConfiguration Configuration)
|
||||||
Y1 = startNode.NodePosition.Y,
|
Y1 = startNode.NodePosition.Y,
|
||||||
X2 = endNode.NodePosition.X,
|
X2 = endNode.NodePosition.X,
|
||||||
Y2 = endNode.NodePosition.Y,
|
Y2 = endNode.NodePosition.Y,
|
||||||
ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
ControlPoint1X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
|
||||||
ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
ControlPoint1Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
||||||
ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
ControlPoint2X = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
|
||||||
ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
ControlPoint2Y = edge.Trajectory is not null && edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
||||||
TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three
|
TrajectoryDegree = edge.Trajectory is null ? TrajectoryDegree.One : edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three
|
||||||
};
|
};
|
||||||
|
|
||||||
double length = EdgeCalculatorModel.GetEdgeLength();
|
double length = EdgeCalculatorModel.GetEdgeLength();
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user