122 lines
3.3 KiB
C#
122 lines
3.3 KiB
C#
namespace RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
/// <summary>
|
|
/// Base class for test scenarios
|
|
/// </summary>
|
|
public abstract class TestScenario
|
|
{
|
|
public Guid Id { get; set; } = Guid.NewGuid();
|
|
public string Name { get; set; } = string.Empty;
|
|
public string Description { get; set; } = string.Empty;
|
|
public TrajectoryType Type { get; set; }
|
|
public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
|
|
public bool IsDefault { get; set; }
|
|
|
|
/// <summary>
|
|
/// Generate reference path for this scenario
|
|
/// </summary>
|
|
public abstract List<PathPoint> GenerateReferencePath();
|
|
|
|
/// <summary>
|
|
/// Check if goal is reached
|
|
/// </summary>
|
|
public abstract bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f);
|
|
|
|
/// <summary>
|
|
/// Get goal pose
|
|
/// </summary>
|
|
public abstract Pose2D GetGoalPose();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trajectory type
|
|
/// </summary>
|
|
public enum TrajectoryType
|
|
{
|
|
StraightLine = 1,
|
|
Circle = 2,
|
|
Square = 3,
|
|
SCurve = 4,
|
|
Custom = 99
|
|
}
|
|
|
|
/// <summary>
|
|
/// Direction for robot movement
|
|
/// </summary>
|
|
public enum RobotDirection
|
|
{
|
|
FORWARD,
|
|
BACKWARD
|
|
}
|
|
|
|
/// <summary>
|
|
/// Point on reference path
|
|
/// </summary>
|
|
public class PathPoint
|
|
{
|
|
public double X { get; set; }
|
|
public double Y { get; set; }
|
|
public RobotDirection Direction { get; set; } = RobotDirection.FORWARD;
|
|
public double DistanceFromStart { get; set; } // cumulative distance
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reference path
|
|
/// </summary>
|
|
public class ReferencePath
|
|
{
|
|
public List<PathPoint> Points { get; set; } = new();
|
|
public double TotalLength { get; set; }
|
|
|
|
public PathPoint? GetClosestPoint(Pose2D pose)
|
|
{
|
|
if (Points.Count == 0) return null;
|
|
|
|
double minDistance = double.MaxValue;
|
|
PathPoint? closest = null;
|
|
|
|
foreach (var point in Points)
|
|
{
|
|
double dx = pose.X - point.X;
|
|
double dy = pose.Y - point.Y;
|
|
double distance = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
closest = point;
|
|
}
|
|
}
|
|
|
|
return closest;
|
|
}
|
|
|
|
public PathPoint? GetPointAtDistance(double distance)
|
|
{
|
|
if (Points.Count == 0) return null;
|
|
if (distance <= 0) return Points[0];
|
|
if (distance >= TotalLength) return Points[^1];
|
|
|
|
// Find segment containing this distance
|
|
for (int i = 0; i < Points.Count - 1; i++)
|
|
{
|
|
if (distance >= Points[i].DistanceFromStart && distance <= Points[i + 1].DistanceFromStart)
|
|
{
|
|
// Interpolate
|
|
double segmentLength = Points[i + 1].DistanceFromStart - Points[i].DistanceFromStart;
|
|
double t = (distance - Points[i].DistanceFromStart) / segmentLength;
|
|
|
|
return new PathPoint
|
|
{
|
|
X = Points[i].X + t * (Points[i + 1].X - Points[i].X),
|
|
Y = Points[i].Y + t * (Points[i + 1].Y - Points[i].Y),
|
|
Direction = Points[i].Direction, // Use direction from start point
|
|
DistanceFromStart = distance
|
|
};
|
|
}
|
|
}
|
|
|
|
return Points[^1];
|
|
}
|
|
}
|