Initial commit
This commit is contained in:
@@ -0,0 +1,80 @@
|
||||
using RobotNet10.NavigationTune.Shared.Models;
|
||||
|
||||
namespace RobotNet10.NavigationTune.Scenarios;
|
||||
|
||||
/// <summary>
|
||||
/// Circle test scenario
|
||||
/// </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);
|
||||
if (distance <= tolerance) Console.WriteLine($"Robot is goal reached. Current pose: [{currentPose.X} - {currentPose.Y}], Goal: [{CenterX + Radius * Math.Cos(StartAngle)} - {CenterY + Radius * Math.Sin(StartAngle)}], Distance: {distance}");
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,182 @@
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.Common.Models;
|
||||
using RobotNet10.NavigationTune.Shared.Models;
|
||||
|
||||
namespace RobotNet10.NavigationTune.Scenarios;
|
||||
|
||||
/// <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; } = [];
|
||||
|
||||
/// <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;
|
||||
|
||||
// Process each edge
|
||||
foreach (var edge in Edges)
|
||||
{
|
||||
var edgePoints = SplitEdge(edge, Resolution);
|
||||
|
||||
if (edgePoints.Count == 0)
|
||||
continue;
|
||||
|
||||
double cumulativeDistance;
|
||||
// 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;
|
||||
}
|
||||
|
||||
points.Add(new PathPoint
|
||||
{
|
||||
X = edgePoints[i].X,
|
||||
Y = edgePoints[i].Y,
|
||||
Direction = edge.Direction,
|
||||
DistanceFromStart = cumulativeDistance
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Split an edge into points based on resolution
|
||||
/// </summary>
|
||||
private static List<(double X, double Y)> SplitEdge(PathEdge edge, double resolution)
|
||||
{
|
||||
var points = new List<(double X, double Y)>();
|
||||
|
||||
// Calculate edge length
|
||||
SpaceEdge spaceEdge = new()
|
||||
{
|
||||
StartX = edge.StartX,
|
||||
StartY = edge.StartY,
|
||||
EndX = edge.EndX,
|
||||
EndY = edge.EndY,
|
||||
Degree = edge.Degree,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
};
|
||||
double edgeLength = SpaceCompute.GetEdgeLength(spaceEdge, 0.1);
|
||||
|
||||
if (edgeLength <= 0)
|
||||
{
|
||||
points.Add((edge.StartX, edge.StartY));
|
||||
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 * 1.0 / numPoints : 0.0;
|
||||
var point = SpaceCompute.BezierPoint(t, spaceEdge);
|
||||
|
||||
points.Add((point.X, point.Y));
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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);
|
||||
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()
|
||||
{
|
||||
if (Edges.Count == 0)
|
||||
return new Pose2D(0, 0, 0);
|
||||
|
||||
var lastEdge = Edges[^1];
|
||||
// Calculate theta from direction (FORWARD = 0, BACKWARD = PI)
|
||||
double goalTheta = 0;
|
||||
if (Edges.Count > 0)
|
||||
{
|
||||
// Calculate direction from last edge
|
||||
if (Edges.Count > 1)
|
||||
{
|
||||
var prevEdge = Edges[^2];
|
||||
double dx = lastEdge.EndX - prevEdge.EndX;
|
||||
double dy = lastEdge.EndY - prevEdge.EndY;
|
||||
goalTheta = Math.Atan2(dy, dx);
|
||||
}
|
||||
else
|
||||
{
|
||||
double dx = lastEdge.EndX - lastEdge.StartX;
|
||||
double dy = lastEdge.EndY - lastEdge.StartY;
|
||||
goalTheta = Math.Atan2(dy, dx);
|
||||
}
|
||||
}
|
||||
return new Pose2D(lastEdge.EndX, lastEdge.EndY, goalTheta);
|
||||
}
|
||||
|
||||
private static double NormalizeAngle(double angle)
|
||||
{
|
||||
while (angle > Math.PI) angle -= 2 * Math.PI;
|
||||
while (angle < -Math.PI) angle += 2 * Math.PI;
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
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
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user