286 lines
9.3 KiB
C#
286 lines
9.3 KiB
C#
namespace RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
/// <summary>
|
|
/// Custom path scenario with user-defined edges
|
|
/// </summary>
|
|
public class CustomPathScenario : TestScenario
|
|
{
|
|
/// <summary>
|
|
/// List of edges defining the path
|
|
/// </summary>
|
|
public List<PathEdge> Edges { get; set; } = new();
|
|
|
|
/// <summary>
|
|
/// Resolution for splitting edges into points (meters)
|
|
/// </summary>
|
|
public double Resolution { get; set; } = 0.05; // meters between points
|
|
|
|
public CustomPathScenario()
|
|
{
|
|
Name = "Custom Path";
|
|
Description = "User-defined path with custom edges";
|
|
Type = TrajectoryType.Custom;
|
|
}
|
|
|
|
public override List<PathPoint> GenerateReferencePath()
|
|
{
|
|
var points = new List<PathPoint>();
|
|
|
|
if (Edges.Count == 0)
|
|
return points;
|
|
|
|
double cumulativeDistance = 0.0;
|
|
|
|
// Process each edge
|
|
foreach (var edge in Edges)
|
|
{
|
|
var edgePoints = SplitEdge(edge, Resolution);
|
|
|
|
if (edgePoints.Count == 0)
|
|
continue;
|
|
|
|
// Adjust cumulative distance for first point
|
|
if (points.Count > 0)
|
|
{
|
|
// Calculate distance from last point to first point of this edge
|
|
double dx = edgePoints[0].X - points[^1].X;
|
|
double dy = edgePoints[0].Y - points[^1].Y;
|
|
double connectionDistance = Math.Sqrt(dx * dx + dy * dy);
|
|
cumulativeDistance = points[^1].DistanceFromStart + connectionDistance;
|
|
}
|
|
else
|
|
{
|
|
cumulativeDistance = 0.0;
|
|
}
|
|
|
|
// Add points from this edge
|
|
for (int i = 0; i < edgePoints.Count; i++)
|
|
{
|
|
if (i == 0 && points.Count > 0)
|
|
{
|
|
// Skip first point if it's the same as last point (edge connection)
|
|
double dx = edgePoints[i].X - points[^1].X;
|
|
double dy = edgePoints[i].Y - points[^1].Y;
|
|
if (Math.Sqrt(dx * dx + dy * dy) < 0.001)
|
|
continue;
|
|
}
|
|
|
|
if (i > 0)
|
|
{
|
|
// Calculate distance from previous point
|
|
double dx = edgePoints[i].X - edgePoints[i - 1].X;
|
|
double dy = edgePoints[i].Y - edgePoints[i - 1].Y;
|
|
double segmentDistance = Math.Sqrt(dx * dx + dy * dy);
|
|
cumulativeDistance += segmentDistance;
|
|
}
|
|
|
|
// Use direction from edge
|
|
RobotDirection direction = edge.Direction;
|
|
|
|
points.Add(new PathPoint
|
|
{
|
|
X = edgePoints[i].X,
|
|
Y = edgePoints[i].Y,
|
|
Direction = direction,
|
|
DistanceFromStart = cumulativeDistance
|
|
});
|
|
}
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Split an edge into points based on resolution
|
|
/// </summary>
|
|
private List<(double X, double Y, double Theta)> SplitEdge(PathEdge edge, double resolution)
|
|
{
|
|
var points = new List<(double X, double Y, double Theta)>();
|
|
|
|
// Calculate edge length
|
|
double edgeLength = CalculateEdgeLength(edge);
|
|
|
|
if (edgeLength <= 0)
|
|
{
|
|
// Single point at start
|
|
double theta = CalculateThetaAt(edge, 0.0);
|
|
points.Add((edge.StartX, edge.StartY, theta));
|
|
return points;
|
|
}
|
|
|
|
// Calculate number of points based on resolution
|
|
int numPoints = Math.Max(1, (int)(edgeLength / resolution));
|
|
|
|
for (int i = 0; i <= numPoints; i++)
|
|
{
|
|
double t = numPoints > 0 ? i / numPoints : 0.0;
|
|
|
|
var (x, y) = CalculatePointAt(edge, t);
|
|
double theta = CalculateThetaAt(edge, t);
|
|
|
|
points.Add((x, y, theta));
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate point on edge at parameter t (0.0 to 1.0)
|
|
/// </summary>
|
|
private (double X, double Y) CalculatePointAt(PathEdge edge, double t)
|
|
{
|
|
t = Math.Clamp(t, 0.0, 1.0);
|
|
|
|
return edge.Degree switch
|
|
{
|
|
1 => CalculateLinearPoint(edge, t),
|
|
2 => CalculateQuadraticBezierPoint(edge, t),
|
|
3 => CalculateCubicBezierPoint(edge, t),
|
|
_ => CalculateLinearPoint(edge, t) // Default to linear
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Linear interpolation (Degree 1)
|
|
/// </summary>
|
|
private (double X, double Y) CalculateLinearPoint(PathEdge edge, double t)
|
|
{
|
|
double x = edge.StartX + t * (edge.EndX - edge.StartX);
|
|
double y = edge.StartY + t * (edge.EndY - edge.StartY);
|
|
return (x, y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Quadratic Bezier curve (Degree 2)
|
|
/// P(t) = (1-t)²P₀ + 2(1-t)tP₁ + t²P₂
|
|
/// </summary>
|
|
private (double X, double Y) CalculateQuadraticBezierPoint(PathEdge edge, double t)
|
|
{
|
|
if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue)
|
|
{
|
|
// Fallback to linear if control point not provided
|
|
return CalculateLinearPoint(edge, t);
|
|
}
|
|
|
|
double oneMinusT = 1.0 - t;
|
|
double x = oneMinusT * oneMinusT * edge.StartX +
|
|
2 * oneMinusT * t * edge.ControlPoint1X.Value +
|
|
t * t * edge.EndX;
|
|
double y = oneMinusT * oneMinusT * edge.StartY +
|
|
2 * oneMinusT * t * edge.ControlPoint1Y.Value +
|
|
t * t * edge.EndY;
|
|
return (x, y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cubic Bezier curve (Degree 3)
|
|
/// P(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃
|
|
/// </summary>
|
|
private (double X, double Y) CalculateCubicBezierPoint(PathEdge edge, double t)
|
|
{
|
|
if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue ||
|
|
!edge.ControlPoint2X.HasValue || !edge.ControlPoint2Y.HasValue)
|
|
{
|
|
// Fallback to quadratic or linear if control points not provided
|
|
if (edge.ControlPoint1X.HasValue && edge.ControlPoint1Y.HasValue)
|
|
return CalculateQuadraticBezierPoint(edge, t);
|
|
return CalculateLinearPoint(edge, t);
|
|
}
|
|
|
|
double oneMinusT = 1.0 - t;
|
|
double oneMinusT2 = oneMinusT * oneMinusT;
|
|
double oneMinusT3 = oneMinusT2 * oneMinusT;
|
|
double t2 = t * t;
|
|
double t3 = t2 * t;
|
|
|
|
double x = oneMinusT3 * edge.StartX +
|
|
3 * oneMinusT2 * t * edge.ControlPoint1X.Value +
|
|
3 * oneMinusT * t2 * edge.ControlPoint2X.Value +
|
|
t3 * edge.EndX;
|
|
double y = oneMinusT3 * edge.StartY +
|
|
3 * oneMinusT2 * t * edge.ControlPoint1Y.Value +
|
|
3 * oneMinusT * t2 * edge.ControlPoint2Y.Value +
|
|
t3 * edge.EndY;
|
|
return (x, y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate tangent angle (theta) at parameter t
|
|
/// </summary>
|
|
private double CalculateThetaAt(PathEdge edge, double t)
|
|
{
|
|
const double epsilon = 0.001;
|
|
double t1 = Math.Clamp(t, 0.0, 1.0);
|
|
double t2 = Math.Clamp(t + epsilon, 0.0, 1.0);
|
|
|
|
var (x1, y1) = CalculatePointAt(edge, t1);
|
|
var (x2, y2) = CalculatePointAt(edge, t2);
|
|
|
|
double dx = x2 - x1;
|
|
double dy = y2 - y1;
|
|
double theta = Math.Atan2(dy, dx);
|
|
|
|
return NormalizeAngle(theta);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate approximate length of edge
|
|
/// </summary>
|
|
private double CalculateEdgeLength(PathEdge edge)
|
|
{
|
|
// For linear: direct distance
|
|
if (edge.Degree == 1)
|
|
{
|
|
double dx = edge.EndX - edge.StartX;
|
|
double dy = edge.EndY - edge.StartY;
|
|
return Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
|
|
// For curves: approximate by sampling
|
|
const int samples = 20;
|
|
double length = 0.0;
|
|
var (prevX, prevY) = CalculatePointAt(edge, 0.0);
|
|
|
|
for (int i = 1; i <= samples; i++)
|
|
{
|
|
double t = i / samples;
|
|
var (x, y) = CalculatePointAt(edge, t);
|
|
double dx = x - prevX;
|
|
double dy = y - prevY;
|
|
length += Math.Sqrt(dx * dx + dy * dy);
|
|
prevX = x;
|
|
prevY = y;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
|
|
{
|
|
if (Edges.Count == 0)
|
|
return false;
|
|
|
|
var goal = GetGoalPose();
|
|
double distance = Pose2D.Distance(currentPose, goal);
|
|
return distance <= tolerance;
|
|
}
|
|
|
|
public override Pose2D GetGoalPose()
|
|
{
|
|
if (Edges.Count == 0)
|
|
return new Pose2D(0, 0, 0);
|
|
|
|
var lastEdge = Edges[^1];
|
|
double theta = CalculateThetaAt(lastEdge, 1.0);
|
|
return new Pose2D(lastEdge.EndX, lastEdge.EndY, theta);
|
|
}
|
|
|
|
private static double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2 * Math.PI;
|
|
return angle;
|
|
}
|
|
}
|
|
|