103 lines
3.6 KiB
C#
103 lines
3.6 KiB
C#
using RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
namespace RobotNet10.NavigationTune.Scenarios;
|
|
|
|
/// <summary>
|
|
/// Straight line test scenario
|
|
/// </summary>
|
|
public class StraightLineScenario : TestScenario
|
|
{
|
|
public double Length { get; set; } = 10.0; // meters
|
|
public double StartX { get; set; } = 0.0;
|
|
public double StartY { get; set; } = 0.0;
|
|
public double StartTheta { get; set; } = 0.0; // radians
|
|
public double Resolution { get; set; } = 0.05; // meters between points
|
|
|
|
public StraightLineScenario()
|
|
{
|
|
Name = "Straight Line 10m";
|
|
Description = "Robot moves in a straight line for 10 meters";
|
|
Type = TrajectoryType.StraightLine;
|
|
}
|
|
|
|
public override List<PathPoint> GenerateReferencePath()
|
|
{
|
|
var points = new List<PathPoint>();
|
|
double absLength = Math.Abs(Length);
|
|
bool isBackward = Length < 0;
|
|
|
|
// For backward movement, reverse the direction
|
|
double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
|
|
|
|
// Normalize directionTheta to [-π, π]
|
|
while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
|
|
while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
|
|
|
|
var direction = isBackward ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
|
|
|
// Start point
|
|
points.Add(new PathPoint
|
|
{
|
|
X = StartX,
|
|
Y = StartY,
|
|
Direction = direction,
|
|
DistanceFromStart = 0.0
|
|
});
|
|
|
|
// Generate intermediate points
|
|
int numPoints = (int)(absLength / Resolution);
|
|
for (int i = 1; i <= numPoints; i++)
|
|
{
|
|
double distance = i * Resolution;
|
|
if (distance > absLength) distance = absLength;
|
|
|
|
points.Add(new PathPoint
|
|
{
|
|
X = StartX + distance * Math.Cos(directionTheta),
|
|
Y = StartY + distance * Math.Sin(directionTheta),
|
|
Direction = direction,
|
|
DistanceFromStart = distance
|
|
});
|
|
}
|
|
|
|
// Ensure end point is exactly at absLength
|
|
if (points[^1].DistanceFromStart < absLength)
|
|
{
|
|
points.Add(new PathPoint
|
|
{
|
|
X = StartX + absLength * Math.Cos(directionTheta),
|
|
Y = StartY + absLength * Math.Sin(directionTheta),
|
|
Direction = direction,
|
|
DistanceFromStart = absLength
|
|
});
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
|
|
{
|
|
var goal = GetGoalPose();
|
|
double distance = Pose2D.Distance(currentPose, goal);
|
|
if (distance <= tolerance) Console.WriteLine($"Robot is goal reached. Current pose: [{currentPose.X} - {currentPose.Y}], Goal: [{goal.X} - {goal.Y}], Distance: {distance}");
|
|
return distance <= tolerance;
|
|
}
|
|
|
|
public override Pose2D GetGoalPose()
|
|
{
|
|
double absLength = Math.Abs(Length);
|
|
bool isBackward = Length < 0;
|
|
double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
|
|
|
|
// Normalize directionTheta to [-π, π]
|
|
while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
|
|
while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
|
|
|
|
return new Pose2D(
|
|
StartX + absLength * Math.Cos(directionTheta),
|
|
StartY + absLength * Math.Sin(directionTheta),
|
|
directionTheta
|
|
);
|
|
}
|
|
}
|