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