78 lines
2.6 KiB
C#
78 lines
2.6 KiB
C#
namespace RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
/// <summary>
|
|
/// Circle test scenario (client-side version)
|
|
/// </summary>
|
|
public class CircleScenario : TestScenario
|
|
{
|
|
public double Radius { get; set; } = 2.0; // meters
|
|
public double CenterX { get; set; } = 0.0;
|
|
public double CenterY { get; set; } = 0.0;
|
|
public double StartAngle { get; set; } = 0.0; // radians
|
|
public bool Clockwise { get; set; } = true;
|
|
public double Resolution { get; set; } = 0.05; // meters between points
|
|
|
|
public CircleScenario()
|
|
{
|
|
Name = $"Circle {Radius}m Radius";
|
|
Description = $"Robot moves in a circle with radius {Radius}m";
|
|
Type = TrajectoryType.Circle;
|
|
}
|
|
|
|
public override List<PathPoint> GenerateReferencePath()
|
|
{
|
|
var points = new List<PathPoint>();
|
|
double circumference = 2 * Math.PI * Radius;
|
|
int numPoints = (int)(circumference / Resolution);
|
|
double angleStep = 2 * Math.PI / numPoints;
|
|
|
|
if (!Clockwise)
|
|
angleStep = -angleStep;
|
|
|
|
var direction = Clockwise ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
|
double distance = 0.0;
|
|
|
|
for (int i = 0; i <= numPoints; i++)
|
|
{
|
|
double angle = StartAngle + i * angleStep;
|
|
double x = CenterX + Radius * Math.Cos(angle);
|
|
double y = CenterY + Radius * Math.Sin(angle);
|
|
|
|
distance = i * Resolution;
|
|
if (distance > circumference) distance = circumference;
|
|
|
|
points.Add(new PathPoint
|
|
{
|
|
X = x,
|
|
Y = y,
|
|
Direction = direction,
|
|
DistanceFromStart = distance
|
|
});
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
|
|
{
|
|
// Check if robot is close to start position
|
|
double dx = currentPose.X - (CenterX + Radius * Math.Cos(StartAngle));
|
|
double dy = currentPose.Y - (CenterY + Radius * Math.Sin(StartAngle));
|
|
double distance = Math.Sqrt(dx * dx + dy * dy);
|
|
return distance <= tolerance;
|
|
}
|
|
|
|
public override Pose2D GetGoalPose()
|
|
{
|
|
// Goal is back at start position
|
|
double goalX = CenterX + Radius * Math.Cos(StartAngle);
|
|
double goalY = CenterY + Radius * Math.Sin(StartAngle);
|
|
double goalTheta = StartAngle + (Clockwise ? Math.PI / 2 : -Math.PI / 2);
|
|
|
|
while (goalTheta > Math.PI) goalTheta -= 2 * Math.PI;
|
|
while (goalTheta < -Math.PI) goalTheta += 2 * Math.PI;
|
|
|
|
return new Pose2D(goalX, goalY, goalTheta);
|
|
}
|
|
}
|