930 lines
37 KiB
C#
930 lines
37 KiB
C#
using RobotNet.VDA5050.Order;
|
||
using RobotNet10.Common;
|
||
using RobotNet10.Common.Models;
|
||
using RobotNet10.NavigationTune.Shared.Models;
|
||
using RobotNet10.RobotApp.Services.Exceptions;
|
||
using RobotNet10.RobotApp.Services.Robot;
|
||
using RobotNet10.RobotApp.Services.Robot.Helper;
|
||
using RobotNet10.RobotApp.Services.Robot.Models;
|
||
using RobotNet10.RobotApp.Services.Simulation;
|
||
using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;
|
||
|
||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||
|
||
/// <summary>
|
||
/// Configuration cho Pure Pursuit controller
|
||
/// </summary>
|
||
public class PurePursuitConfig
|
||
{
|
||
/// <summary>
|
||
/// Lookahead distance minimum (m)
|
||
/// </summary>
|
||
public double LookaheadMin { get; set; } = 0.3;
|
||
|
||
/// <summary>
|
||
/// Hệ số tỷ lệ lookahead với vận tốc (s)
|
||
/// </summary>
|
||
public double Kdd { get; set; } = 1.0;
|
||
|
||
/// <summary>
|
||
/// Lookahead distance maximum (m)
|
||
/// </summary>
|
||
public double LookaheadMax { get; set; } = 2.0;
|
||
|
||
/// <summary>
|
||
/// [LEGACY] Gain cho curvature (nếu cần scale steering)
|
||
/// Note: Not used in current implementation.
|
||
/// Replaced by adaptive KCurvature for lookahead adjustment.
|
||
/// Kept for backward compatibility.
|
||
/// </summary>
|
||
public double CurvatureGain { get; set; } = 1.0;
|
||
|
||
/// <summary>
|
||
/// Ngưỡng để coi như đạt waypoint (m)
|
||
/// </summary>
|
||
public double WaypointTolerance { get; set; } = 0.1;
|
||
|
||
/// <summary>
|
||
/// Maximum angular velocity during tracking (rad/s)
|
||
/// </summary>
|
||
public double MaxAngularVelocity { get; set; } = 1.5;
|
||
|
||
/// <summary>
|
||
/// Path waypoint spacing resolution (meters)
|
||
/// </summary>
|
||
public double ResolutionSplit { get; set; } = 0.05;
|
||
|
||
#region Adaptive Lookahead Parameters
|
||
|
||
/// <summary>
|
||
/// Goal region distance - start reducing lookahead when closer than this (m)
|
||
/// Default: 1.5m
|
||
/// </summary>
|
||
public double GoalRegionDistance { get; set; } = 1.5;
|
||
|
||
/// <summary>
|
||
/// Curvature adaptation factor (higher = more lookahead reduction on curves)
|
||
/// Default: 2.0
|
||
/// </summary>
|
||
public double KCurvature { get; set; } = 2.0;
|
||
|
||
/// <summary>
|
||
/// Minimum lookahead time ratio (seconds) - for dynamic min limit
|
||
/// Default: 0.3s
|
||
/// </summary>
|
||
public double MinLookaheadTimeRatio { get; set; } = 0.3;
|
||
|
||
/// <summary>
|
||
/// Maximum lookahead time ratio (seconds) - for dynamic max limit
|
||
/// Default: 2.0s
|
||
/// </summary>
|
||
public double MaxLookaheadTimeRatio { get; set; } = 2.0;
|
||
|
||
/// <summary>
|
||
/// Switch to Stanley controller when within this distance to goal (m)
|
||
/// Default: 0.5m
|
||
/// </summary>
|
||
public double FinalApproachThreshold { get; set; } = 1;
|
||
|
||
#endregion
|
||
}
|
||
|
||
public class PurePursuit(PurePursuitConfig PurePursuitConfig, StanleyConfig StanleyConfig)
|
||
{
|
||
public OrderNode[] OrderNodes = [];
|
||
public OrderEdge[] OrderEdges = [];
|
||
public OrderNode? LastOrderNode = null;
|
||
public List<NavigationNode> Waypoints_Value = [];
|
||
|
||
private Dictionary<string, (int start, int end)>? _segmentCache;
|
||
|
||
private NavigationNode? Goal;
|
||
|
||
private int closesEdgeIndex = 0;
|
||
private int _currentWaypointAheadIndex = 0; // For Stanley controller
|
||
private bool _isApproachGoal = false; // Flag for final approach mode
|
||
|
||
// Local Planner: approach curve stored as a virtual edge
|
||
private int _approachWaypointCount = 0;
|
||
private int _connectionEdgeIndex = 0;
|
||
private OrderEdge? _approachOrderEdge; // Virtual edge for segment cache lookup
|
||
private SpaceEdge? _approachSpaceEdge; // Geometry for projection in GetClosesEdges
|
||
|
||
public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
|
||
{
|
||
if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
|
||
if (edges.Length < 1) throw new SimulationException();
|
||
if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
|
||
(OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
|
||
Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
|
||
closesEdgeIndex = 0;
|
||
_currentWaypointAheadIndex = 0;
|
||
_isApproachGoal = false;
|
||
_approachWaypointCount = 0;
|
||
_connectionEdgeIndex = 0;
|
||
_approachOrderEdge = null;
|
||
_approachSpaceEdge = null;
|
||
BuildSegmentCache();
|
||
return this;
|
||
}
|
||
|
||
public void ResetTracking()
|
||
{
|
||
closesEdgeIndex = 0;
|
||
_currentWaypointAheadIndex = 0;
|
||
_isApproachGoal = false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Sinh approach waypoints (cubic Bezier) từ vị trí robot đến path rồi prepend vào Waypoints_Value.
|
||
/// Approach curve CHỈ dựa vào vị trí (X,Y) robot và tangent tại connection point — KHÔNG dùng robotTheta.
|
||
/// Robot sẽ Rotate tại chỗ đến heading đầu curve trước khi Moving.
|
||
/// </summary>
|
||
public ApproachResult GenerateAndPrependApproachPath(
|
||
double robotX, double robotY, int closestIndex, LocalPlannerConfig config)
|
||
{
|
||
if (!config.Enabled || Waypoints_Value.Count < 2)
|
||
return ApproachResult.Disabled;
|
||
|
||
// Bước 1: Tính khoảng cách đến path
|
||
double distToPath = CalculateDistance(robotX, robotY,
|
||
Waypoints_Value[closestIndex].X, Waypoints_Value[closestIndex].Y);
|
||
|
||
if (distToPath < config.OnPathThreshold)
|
||
return ApproachResult.AlreadyOnPath;
|
||
|
||
if (distToPath > config.MaxApproachDistance)
|
||
return ApproachResult.TooFarFromPath;
|
||
|
||
// Bước 2: Tính mergeDistance (dynamic theo distToPath)
|
||
double mergeDistance = config.MergeDistanceGain * distToPath;
|
||
mergeDistance = Math.Clamp(mergeDistance, config.MergeDistanceMin, config.MergeDistanceMax);
|
||
|
||
// Bước 3: Tìm connection point trên path
|
||
int connectionIndex = closestIndex;
|
||
double accDist = 0;
|
||
while (accDist < mergeDistance && connectionIndex < Waypoints_Value.Count - 2)
|
||
{
|
||
double segLen = CalculateDistance(
|
||
Waypoints_Value[connectionIndex].X, Waypoints_Value[connectionIndex].Y,
|
||
Waypoints_Value[connectionIndex + 1].X, Waypoints_Value[connectionIndex + 1].Y);
|
||
accDist += segLen;
|
||
connectionIndex++;
|
||
}
|
||
var connectionPoint = Waypoints_Value[connectionIndex];
|
||
|
||
// Bước 4: Tính path tangent tại connection point
|
||
double pathTangent;
|
||
if (connectionIndex < Waypoints_Value.Count - 1)
|
||
{
|
||
double tx = Waypoints_Value[connectionIndex + 1].X - connectionPoint.X;
|
||
double ty = Waypoints_Value[connectionIndex + 1].Y - connectionPoint.Y;
|
||
pathTangent = Math.Atan2(ty, tx);
|
||
}
|
||
else if (connectionIndex > 0)
|
||
{
|
||
double tx = connectionPoint.X - Waypoints_Value[connectionIndex - 1].X;
|
||
double ty = connectionPoint.Y - Waypoints_Value[connectionIndex - 1].Y;
|
||
pathTangent = Math.Atan2(ty, tx);
|
||
}
|
||
else
|
||
{
|
||
return ApproachResult.Disabled;
|
||
}
|
||
|
||
// Bước 5: Tính 4 control points cho Cubic Bezier
|
||
// P0 = robot position, P3 = connection point
|
||
// P1 = dọc hướng P0→P3 (KHÔNG dùng robotTheta)
|
||
// P2 = tiếp cận theo pathTangent
|
||
double p0x = robotX, p0y = robotY;
|
||
double p3x = connectionPoint.X, p3y = connectionPoint.Y;
|
||
|
||
double dist = CalculateDistance(p0x, p0y, p3x, p3y);
|
||
if (dist < config.ResolutionSplit)
|
||
return ApproachResult.AlreadyOnPath;
|
||
|
||
// Direction P0→P3
|
||
double dirX = (p3x - p0x) / dist;
|
||
double dirY = (p3y - p0y) / dist;
|
||
|
||
double d1 = config.ControlArmRatioStart * dist;
|
||
double d2 = config.ControlArmRatioEnd * dist;
|
||
|
||
double p1x = p0x + d1 * dirX;
|
||
double p1y = p0y + d1 * dirY;
|
||
double p2x = p3x - d2 * Math.Cos(pathTangent);
|
||
double p2y = p3y - d2 * Math.Sin(pathTangent);
|
||
|
||
// Bước 6: Sample waypoints trên curve
|
||
var approachEdge = new SpaceEdge()
|
||
{
|
||
StartX = p0x, StartY = p0y,
|
||
EndX = p3x, EndY = p3y,
|
||
ControlPoint1X = p1x, ControlPoint1Y = p1y,
|
||
ControlPoint2X = p2x, ControlPoint2Y = p2y,
|
||
Degree = 3
|
||
};
|
||
|
||
double length = SpaceCompute.GetEdgeLength(approachEdge, config.ResolutionSplit);
|
||
if (length < config.ResolutionSplit)
|
||
return ApproachResult.AlreadyOnPath;
|
||
|
||
double step = config.ResolutionSplit / length;
|
||
var approachWaypoints = new List<NavigationNode>();
|
||
|
||
for (double t = 0; t < 1 - step; t += step)
|
||
{
|
||
(double x, double y) = SpaceCompute.BezierPoint(t, approachEdge);
|
||
approachWaypoints.Add(new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = string.Empty,
|
||
X = x,
|
||
Y = y,
|
||
Direction = connectionPoint.Direction,
|
||
Speed = connectionPoint.Speed
|
||
});
|
||
}
|
||
|
||
if (approachWaypoints.Count == 0)
|
||
return ApproachResult.AlreadyOnPath;
|
||
|
||
// Bước 7a: Xác định edge chứa connectionIndex (trước khi modify Waypoints_Value)
|
||
int connectionEdgeIdx = 0;
|
||
if (_segmentCache is not null)
|
||
{
|
||
for (int i = 0; i < OrderEdges.Length; i++)
|
||
{
|
||
if (_segmentCache.TryGetValue(OrderEdges[i].EdgeId, out var range)
|
||
&& connectionIndex >= range.start && connectionIndex <= range.end)
|
||
{
|
||
connectionEdgeIdx = i;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Bước 7b: Lưu approach curve như 1 virtual edge (cho projection + segment cache)
|
||
_approachSpaceEdge = approachEdge;
|
||
_approachOrderEdge = new OrderEdge
|
||
{
|
||
EdgeId = "__approach__",
|
||
Degree = 3,
|
||
ControlPoint1X = p1x, ControlPoint1Y = p1y,
|
||
ControlPoint2X = p2x, ControlPoint2Y = p2y,
|
||
Direction = connectionPoint.Direction,
|
||
Speed = connectionPoint.Speed,
|
||
};
|
||
|
||
// Bước 7c: Insert approach waypoints ngay trước connectionIndex + xóa phần trước
|
||
_approachWaypointCount = approachWaypoints.Count;
|
||
Waypoints_Value.InsertRange(connectionIndex, approachWaypoints);
|
||
if (connectionIndex > 0)
|
||
{
|
||
Waypoints_Value.RemoveRange(0, connectionIndex);
|
||
}
|
||
// Kết quả: [approach_0, ..., approach_n, connectionPoint, ..., goal]
|
||
// 0 _approachWaypointCount
|
||
|
||
_connectionEdgeIndex = connectionEdgeIdx;
|
||
closesEdgeIndex = connectionEdgeIdx;
|
||
|
||
// Bước 7d: Rebuild segment cache (approach edge + real edges từ connectionEdge trở đi)
|
||
BuildSegmentCache(_connectionEdgeIndex);
|
||
|
||
return ApproachResult.ApproachGenerated;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Rebuild toàn bộ Waypoints_Value từ OrderNodes/OrderEdges đã lưu.
|
||
/// Dùng khi cần sinh lại approach từ vị trí mới (thay vì ClearApproachWaypoints).
|
||
/// Flow: RebuildPath() → check local planner → GenerateAndPrependApproachPath() nếu cần.
|
||
/// </summary>
|
||
public void RebuildPath()
|
||
{
|
||
string? goalId = Goal?.NodeId;
|
||
|
||
Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
|
||
_approachWaypointCount = 0;
|
||
_connectionEdgeIndex = 0;
|
||
_approachOrderEdge = null;
|
||
_approachSpaceEdge = null;
|
||
BuildSegmentCache();
|
||
|
||
if (!string.IsNullOrEmpty(goalId))
|
||
UpdateGoal(goalId);
|
||
}
|
||
|
||
public void UpdateGoal(string goalId)
|
||
{
|
||
var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
|
||
if (goal is not null) Goal = goal;
|
||
}
|
||
|
||
private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
|
||
{
|
||
List<NavigationNode> navigationNode = [new()
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = nodes[0].NodeId,
|
||
X = nodes[0].X,
|
||
Y = nodes[0].Y,
|
||
Theta = nodes[0].Theta,
|
||
Direction = edges[0].Direction,
|
||
Speed = edges[0].Speed,
|
||
}];
|
||
foreach (var edge in edges)
|
||
{
|
||
var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
|
||
var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
|
||
if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
|
||
if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));
|
||
|
||
var spaceEdge = new SpaceEdge()
|
||
{
|
||
StartX = startNode.X,
|
||
StartY = startNode.Y,
|
||
EndX = endNode.X,
|
||
EndY = endNode.Y,
|
||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||
Degree = edge.Degree,
|
||
};
|
||
|
||
double length = SpaceCompute.GetEdgeLength(spaceEdge, PurePursuitConfig.ResolutionSplit);
|
||
if (length <= 0) continue;
|
||
double step = PurePursuitConfig.ResolutionSplit / length;
|
||
|
||
for (double t = step; t <= 1 - step; t += step)
|
||
{
|
||
(double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
|
||
navigationNode.Add(new()
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = string.Empty,
|
||
X = x,
|
||
Y = y,
|
||
Theta = null,
|
||
Direction = edge.Direction,
|
||
Speed = edge.Speed,
|
||
});
|
||
}
|
||
navigationNode.Add(new()
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = endNode.NodeId,
|
||
X = endNode.X,
|
||
Y = endNode.Y,
|
||
Theta = endNode.Theta,
|
||
Direction = edge.Direction,
|
||
Speed = edge.Speed,
|
||
});
|
||
}
|
||
return [.. navigationNode];
|
||
}
|
||
|
||
/// <summary>
|
||
/// Build segment cache mapping EdgeId → (startWaypointIdx, endWaypointIdx).
|
||
/// Approach edge (nếu có) được thêm vào cache trước, sau đó build các real edges từ fromEdgeIndex.
|
||
/// </summary>
|
||
private void BuildSegmentCache(int fromEdgeIndex = 0)
|
||
{
|
||
_segmentCache = [];
|
||
|
||
// Thêm approach edge vào cache nếu có
|
||
if (_approachOrderEdge is not null && _approachWaypointCount > 0)
|
||
{
|
||
_segmentCache[_approachOrderEdge.EdgeId] = (0, _approachWaypointCount);
|
||
}
|
||
|
||
for (int i = fromEdgeIndex; i < OrderEdges.Length; i++)
|
||
{
|
||
var edge = OrderEdges[i];
|
||
int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.StartNodeId);
|
||
int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.EndNodeId);
|
||
|
||
// Edge đầu tiên: start node có thể đã bị xóa
|
||
// → lấy end của approach edge nếu có, hoặc 0
|
||
if (waypointStartIdx == -1)
|
||
{
|
||
if (i == fromEdgeIndex)
|
||
waypointStartIdx = _approachOrderEdge is not null
|
||
? _segmentCache[_approachOrderEdge.EdgeId].end
|
||
: 0;
|
||
else
|
||
waypointStartIdx = _segmentCache[OrderEdges[i - 1].EdgeId].end;
|
||
}
|
||
if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;
|
||
|
||
if (waypointStartIdx > waypointEndIdx) throw new NavigationException($"Waypoint has invalid range for edge {edge.EdgeId}: start={waypointStartIdx}, end={waypointEndIdx}");
|
||
|
||
_segmentCache[edge.EdgeId] = (waypointStartIdx, waypointEndIdx);
|
||
}
|
||
}
|
||
|
||
private (OrderEdge edge, double time) GetClosesEdges(double x, double y)
|
||
{
|
||
double minDistance = double.MaxValue;
|
||
OrderEdge? edgesResult = null;
|
||
double prjTime = 0;
|
||
|
||
// Project lên approach edge nếu robot chưa vượt qua connection edge
|
||
if (_approachSpaceEdge is not null && _approachOrderEdge is not null
|
||
&& closesEdgeIndex <= _connectionEdgeIndex)
|
||
{
|
||
(_, _, var approachDist, double approachTime) = SpaceCompute.GetProjectionOnEdge(x, y, _approachSpaceEdge);
|
||
if (approachDist < minDistance)
|
||
{
|
||
minDistance = approachDist;
|
||
edgesResult = _approachOrderEdge;
|
||
prjTime = approachTime;
|
||
}
|
||
}
|
||
|
||
// Project lên các real edges (từ closesEdgeIndex)
|
||
for (int i = closesEdgeIndex; i < OrderEdges.Length; i++)
|
||
{
|
||
var startNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].StartNodeId);
|
||
var endNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].EndNodeId);
|
||
if (startNode is null || endNode is null) continue;
|
||
|
||
(_, _, var distance, double time) = SpaceCompute.GetProjectionOnEdge(x, y, new()
|
||
{
|
||
StartX = startNode.X,
|
||
StartY = startNode.Y,
|
||
EndX = endNode.X,
|
||
EndY = endNode.Y,
|
||
Degree = OrderEdges[i].Degree,
|
||
ControlPoint1X = OrderEdges[i].ControlPoint1X ?? 0,
|
||
ControlPoint1Y = OrderEdges[i].ControlPoint1Y ?? 0,
|
||
ControlPoint2X = OrderEdges[i].ControlPoint2X ?? 0,
|
||
ControlPoint2Y = OrderEdges[i].ControlPoint2Y ?? 0,
|
||
});
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
edgesResult = OrderEdges[i];
|
||
prjTime = time;
|
||
closesEdgeIndex = i;
|
||
}
|
||
}
|
||
return (edgesResult ?? OrderEdges[closesEdgeIndex], prjTime);
|
||
}
|
||
|
||
public (NavigationNode node, int index) OnNode(double x, double y)
|
||
{
|
||
// Edge-based projection thống nhất cho cả approach edge và real edges
|
||
(var closeEdge, double prjTime) = GetClosesEdges(x, y);
|
||
(var startNodeIdx, var endNodeIdx) = _segmentCache is not null && _segmentCache.TryGetValue(closeEdge.EdgeId, out var cached)
|
||
? cached
|
||
: (0, Waypoints_Value.Count - 1);
|
||
int onNodeIndex = (int)(Math.Abs(endNodeIdx - startNodeIdx) * prjTime) + startNodeIdx;
|
||
|
||
return (Waypoints_Value[onNodeIndex], onNodeIndex);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate adaptive lookahead distance based on velocity, confidence, distance to goal, and path curvature
|
||
/// Lookahead adapts to:
|
||
/// 1. Velocity (faster = look further ahead)
|
||
/// 2. Distance to goal (near goal = shorter lookahead for precision)
|
||
/// 3. Path curvature (sharp curves = shorter lookahead for tighter tracking)
|
||
/// 4. Confidence (low confidence = shorter lookahead for safety)
|
||
/// 5. Velocity-based dynamic time limits
|
||
/// </summary>
|
||
private double GetLookaheadDistance(double vHybrid, double robotX, double robotY, int closestIndex)
|
||
{
|
||
// 1. Base lookahead from velocity
|
||
double baseLookahead = PurePursuitConfig.LookaheadMin + PurePursuitConfig.Kdd * Math.Abs(vHybrid);
|
||
|
||
// 2. Distance-to-goal adaptation
|
||
double distanceToGoal = CalculateDistanceToGoal(robotX, robotY);
|
||
double goalFactor = 1.0;
|
||
if (distanceToGoal < PurePursuitConfig.GoalRegionDistance)
|
||
{
|
||
// Gradually reduce lookahead as we approach goal
|
||
// At goal: factor = 0.5, At GoalRegionDistance: factor = 1.0
|
||
goalFactor = 0.5 + 0.5 * (distanceToGoal / PurePursuitConfig.GoalRegionDistance);
|
||
}
|
||
|
||
// 3. Curvature adaptation
|
||
double curvature = CalculateCurvature(closestIndex);
|
||
// curvatureFactor ranges from 1.0 (straight) to ~0.33 (very sharp curve with KCurvature=2.0)
|
||
double curvatureFactor = 1.0 / (1.0 + PurePursuitConfig.KCurvature * curvature);
|
||
|
||
// 5. Combine all factors
|
||
double adaptiveLookahead = baseLookahead * goalFactor * curvatureFactor;
|
||
|
||
double minLookahead = PurePursuitConfig.LookaheadMin;
|
||
double maxLookahead = PurePursuitConfig.LookaheadMax;
|
||
|
||
if (distanceToGoal < PurePursuitConfig.GoalRegionDistance && Math.Abs(vHybrid) > 0.0)
|
||
{
|
||
// 6. Apply velocity-based dynamic limits
|
||
// Minimum: look at least 0.3 seconds ahead or LookaheadMin (whichever is larger)
|
||
minLookahead = Math.Max(PurePursuitConfig.LookaheadMin, Math.Abs(vHybrid) * PurePursuitConfig.MinLookaheadTimeRatio);
|
||
|
||
// Maximum: look at most 2 seconds ahead or LookaheadMax (whichever is smaller)
|
||
maxLookahead = Math.Min(PurePursuitConfig.LookaheadMax, Math.Abs(vHybrid) * PurePursuitConfig.MaxLookaheadTimeRatio);
|
||
|
||
// Ensure min < max
|
||
if (minLookahead > maxLookahead)
|
||
minLookahead = maxLookahead;
|
||
}
|
||
|
||
adaptiveLookahead = Math.Clamp(adaptiveLookahead, minLookahead, maxLookahead);
|
||
|
||
if (double.IsNaN(adaptiveLookahead) || adaptiveLookahead <= 0)
|
||
{
|
||
adaptiveLookahead = PurePursuitConfig.LookaheadMin;
|
||
}
|
||
|
||
return adaptiveLookahead;
|
||
}
|
||
|
||
public (double linearVel, double angularVel) PurePursuit_step(double X_Ref,
|
||
double Y_Ref,
|
||
double Angle_Ref,
|
||
double actualLinearVelocity,
|
||
double maxLinearVelocity)
|
||
{
|
||
if (Waypoints_Value is null || Waypoints_Value.Count < 2)
|
||
throw new NavigationException("NAV PP Waypoint not yet set");
|
||
|
||
// 1. Get closest waypoint (KEEP ORIGINAL LOGIC)
|
||
var (onNode, index) = OnNode(X_Ref, Y_Ref);
|
||
if (onNode is null || Goal is null)
|
||
throw new NavigationException("NAV PP cannot get projection node");
|
||
|
||
// 2. Calculate adaptive lookahead distance (UPGRADED)
|
||
double lookaheadDistance = GetLookaheadDistance(actualLinearVelocity, X_Ref, Y_Ref, index);
|
||
|
||
// 3. Find target point with INTERPOLATION (UPGRADED)
|
||
NavigationNode? targetPoint = FindTargetPoint(index, lookaheadDistance);
|
||
targetPoint ??= Goal;
|
||
|
||
// 4. Apply speed limit from target point if available
|
||
double linearVel = maxLinearVelocity;
|
||
double? targetSpeed = targetPoint.Speed;
|
||
|
||
if (targetSpeed.HasValue && targetSpeed.Value > 0)
|
||
{
|
||
linearVel = Math.Min(maxLinearVelocity, targetSpeed.Value);
|
||
}
|
||
|
||
// 5. Check for final approach (PREPARATION FOR STANLEY)
|
||
double distanceToGoal = CalculateDistanceToGoal(X_Ref, Y_Ref);
|
||
if (targetPoint.Id == Goal.Id || distanceToGoal <= PurePursuitConfig.FinalApproachThreshold)
|
||
{
|
||
_isApproachGoal = true;
|
||
}
|
||
|
||
// 6. Normalize theta for backward (SIMPLIFIED)
|
||
bool isBackward = onNode.Direction == RobotDirection.BACKWARD;
|
||
if (isBackward) Angle_Ref += Math.PI;
|
||
Angle_Ref = NormalizeAngle(Angle_Ref);
|
||
|
||
// 7. Switch to Stanley when approaching goal (STANLEY INTEGRATION)
|
||
if (_isApproachGoal)
|
||
{
|
||
var (linear, angular) = FinalApproachController(X_Ref, Y_Ref, Angle_Ref, Goal, actualLinearVelocity, linearVel, isBackward);
|
||
return (linear, angular);
|
||
}
|
||
|
||
// 8. Calculate angle to target
|
||
var dx = targetPoint.X - X_Ref;
|
||
var dy = targetPoint.Y - Y_Ref;
|
||
var alpha = Math.Atan2(dy, dx) - Angle_Ref;
|
||
|
||
// Normalize alpha to [-π, π] (SIMPLIFIED)
|
||
alpha = NormalizeAngle(alpha);
|
||
|
||
// 9. Pure Pursuit formula: ω = 2 * v * sin(α) / L
|
||
var angularVelocity = 2.0 * Math.Abs(actualLinearVelocity) * Math.Sin(alpha) / lookaheadDistance;
|
||
|
||
// 10. Clamp to max angular velocity
|
||
if (Math.Abs(angularVelocity) > PurePursuitConfig.MaxAngularVelocity)
|
||
{
|
||
angularVelocity = Math.Sign(angularVelocity) * PurePursuitConfig.MaxAngularVelocity;
|
||
}
|
||
|
||
// 11. Apply direction sign to velocities
|
||
if (isBackward)
|
||
{
|
||
linearVel = -linearVel;
|
||
}
|
||
|
||
Console.WriteLine($"PP: Target=({targetPoint.X:F3},{targetPoint.Y:F3}), " +
|
||
$"Pose=({X_Ref:F3},{Y_Ref:F3},{Angle_Ref * 180 / Math.PI:F2}°), " +
|
||
$"Look={lookaheadDistance:F3}, Alpha={alpha * 180 / Math.PI:F2}°, " +
|
||
$"LVel={linearVel:F3}, AngVel={angularVelocity:F3}");
|
||
|
||
return (linearVel, angularVelocity);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Stanley-based final approach controller
|
||
/// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
|
||
/// </summary>
|
||
private (double linearVel, double angularVel) FinalApproachController(
|
||
double robotX,
|
||
double robotY,
|
||
double robotTheta,
|
||
NavigationNode goal,
|
||
double actualLinearVelocity,
|
||
double maxLinearVelocity,
|
||
bool isBackward)
|
||
{
|
||
// Calculate front axle position
|
||
double frontX = robotX + StanleyConfig.WheelBase * Math.Cos(robotTheta);
|
||
double frontY = robotY + StanleyConfig.WheelBase * Math.Sin(robotTheta);
|
||
|
||
// Find closest point on path to front axle
|
||
var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
|
||
|
||
// Calculate heading at closest point (path direction)
|
||
double pathHeading = CalculatePathHeading(closestIndex);
|
||
|
||
// Calculate cross-track error (signed distance from front axle to path)
|
||
double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
|
||
if (isBackward) crossTrackError = -crossTrackError;
|
||
|
||
// Calculate heading error (path heading - robot heading)
|
||
double headingError = NormalizeAngle(pathHeading - robotTheta);
|
||
|
||
// Calculate curvature at closest point (for feedforward)
|
||
double curvature = 0;
|
||
if (StanleyConfig.EnableCurvatureFeedforward)
|
||
{
|
||
curvature = CalculateCurvature(closestIndex);
|
||
}
|
||
|
||
// Adaptive K gain: increase when close to goal for tighter tracking
|
||
double distanceToGoal = CalculateDistance(robotX, robotY, goal.X, goal.Y);
|
||
double adaptiveK = StanleyConfig.K;
|
||
if (distanceToGoal < StanleyConfig.GoalApproachDistance)
|
||
{
|
||
// Linearly increase K from K to K*GoalGainMultiplier as distance decreases
|
||
double approachRatio = 1.0 - (distanceToGoal / StanleyConfig.GoalApproachDistance);
|
||
adaptiveK = StanleyConfig.K * (1.0 + approachRatio * (StanleyConfig.GoalGainMultiplier - 1.0));
|
||
}
|
||
|
||
// Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
|
||
double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + StanleyConfig.Ks);
|
||
|
||
// Add curvature feedforward if enabled
|
||
double curvatureTerm = 0;
|
||
if (StanleyConfig.EnableCurvatureFeedforward && curvature != 0)
|
||
{
|
||
curvatureTerm = StanleyConfig.KCurvatureFF * Math.Atan(curvature * StanleyConfig.WheelBase);
|
||
}
|
||
|
||
// Total steering angle
|
||
double steeringAngle = headingError + crossTrackTerm + curvatureTerm;
|
||
|
||
// Clamp to maximum steering angle
|
||
steeringAngle = Math.Clamp(steeringAngle, -StanleyConfig.MaxSteeringAngle, StanleyConfig.MaxSteeringAngle);
|
||
|
||
// Convert steering angle to angular velocity using bicycle model
|
||
double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / StanleyConfig.WheelBase;
|
||
|
||
// Clamp angular velocity for final approach
|
||
angularVelocity = Math.Clamp(angularVelocity, -StanleyConfig.MaxAngularVelocity, StanleyConfig.MaxAngularVelocity);
|
||
|
||
// Apply direction to velocities
|
||
double linearVel = maxLinearVelocity;
|
||
if (isBackward)
|
||
{
|
||
linearVel = -linearVel;
|
||
}
|
||
|
||
// Debug output
|
||
Console.WriteLine($"FA-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
|
||
$"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
|
||
$"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
|
||
$"Curv={curvature:F3}, SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
|
||
$"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
|
||
$"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");
|
||
|
||
return (linearVel, angularVelocity);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Find target point at lookahead distance from current index with interpolation
|
||
/// This provides smooth target point selection instead of discrete waypoints
|
||
/// Speed information is interpolated to provide accurate future speed limit
|
||
/// </summary>
|
||
private NavigationNode? FindTargetPoint(int startIndex, double lookaheadDistance)
|
||
{
|
||
if (startIndex >= Waypoints_Value.Count - 1)
|
||
return Goal;
|
||
|
||
double accumulatedDistance = 0;
|
||
|
||
for (int i = startIndex; i < Waypoints_Value.Count - 1; i++)
|
||
{
|
||
double dx = Waypoints_Value[i + 1].X - Waypoints_Value[i].X;
|
||
double dy = Waypoints_Value[i + 1].Y - Waypoints_Value[i].Y;
|
||
double segmentLength = Math.Sqrt(dx * dx + dy * dy);
|
||
|
||
if (accumulatedDistance + segmentLength >= lookaheadDistance)
|
||
{
|
||
// Interpolate within this segment
|
||
double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
|
||
|
||
// Interpolate speed if both waypoints have speed info
|
||
double? interpolatedSpeed;
|
||
if (Waypoints_Value[i].Speed is { } speed1 && Waypoints_Value[i + 1].Speed is { } speed2)
|
||
{
|
||
// Linear interpolation of speed limit
|
||
interpolatedSpeed = speed1 + t * (speed2 - speed1);
|
||
}
|
||
else
|
||
{
|
||
// Use next waypoint's speed if available (upcoming constraint)
|
||
interpolatedSpeed = Waypoints_Value[i + 1].Speed ?? Waypoints_Value[i].Speed;
|
||
}
|
||
|
||
return new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = string.Empty,
|
||
X = Waypoints_Value[i].X + t * dx,
|
||
Y = Waypoints_Value[i].Y + t * dy,
|
||
Theta = null,
|
||
Direction = Waypoints_Value[i].Direction,
|
||
Speed = interpolatedSpeed // Preserve speed limit for lookahead
|
||
};
|
||
}
|
||
|
||
accumulatedDistance += segmentLength;
|
||
}
|
||
|
||
return Goal;
|
||
}
|
||
|
||
#region Helper Methods
|
||
|
||
/// <summary>
|
||
/// Calculate distance between two points
|
||
/// </summary>
|
||
private static double CalculateDistance(double x1, double y1, double x2, double y2)
|
||
{
|
||
double dx = x2 - x1;
|
||
double dy = y2 - y1;
|
||
return Math.Sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Normalize angle to [-π, π]
|
||
/// </summary>
|
||
private static double NormalizeAngle(double angle)
|
||
{
|
||
while (angle > Math.PI) angle -= 2 * Math.PI;
|
||
while (angle < -Math.PI) angle += 2 * Math.PI;
|
||
return angle;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate distance from robot to goal point
|
||
/// </summary>
|
||
private double CalculateDistanceToGoal(double robotX, double robotY)
|
||
{
|
||
if (Goal == null)
|
||
return double.MaxValue;
|
||
|
||
double dx = Goal.X - robotX;
|
||
double dy = Goal.Y - robotY;
|
||
return Math.Sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate path curvature at given waypoint index using 3-point circle fitting (Menger curvature)
|
||
/// Returns curvature in 1/meters (larger value = sharper curve)
|
||
/// </summary>
|
||
private double CalculateCurvature(int index)
|
||
{
|
||
// Need at least 3 points for curvature calculation
|
||
if (Waypoints_Value.Count < 3 || index <= 0 || index >= Waypoints_Value.Count - 1)
|
||
return 0.0;
|
||
|
||
var p1 = Waypoints_Value[index - 1];
|
||
var p2 = Waypoints_Value[index];
|
||
var p3 = Waypoints_Value[index + 1];
|
||
|
||
// Calculate vectors
|
||
double dx1 = p2.X - p1.X;
|
||
double dy1 = p2.Y - p1.Y;
|
||
double dx2 = p3.X - p2.X;
|
||
double dy2 = p3.Y - p2.Y;
|
||
|
||
// Cross product magnitude (2 * triangle area)
|
||
double cross = Math.Abs(dx1 * dy2 - dy1 * dx2);
|
||
|
||
// Side lengths of triangle
|
||
double a = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
|
||
double b = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
|
||
double c = Math.Sqrt((p3.X - p1.X) * (p3.X - p1.X) + (p3.Y - p1.Y) * (p3.Y - p1.Y));
|
||
|
||
// Menger curvature formula: k = 4 * Area / (a * b * c)
|
||
// Area of triangle = cross / 2, so k = 2 * cross / (a * b * c)
|
||
double curvature = 2.0 * cross / (a * b * c + 1e-9); // Add small epsilon to avoid division by zero
|
||
|
||
return curvature;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Stanley Helper Methods (for FinalApproachController)
|
||
|
||
/// <summary>
|
||
/// Get closest waypoint ahead to given position (for Stanley front axle tracking)
|
||
/// </summary>
|
||
private (NavigationNode point, int index) GetClosestAheadWaypoint(double x, double y)
|
||
{
|
||
if (Waypoints_Value.Count == 0)
|
||
throw new InvalidOperationException("Path not set");
|
||
|
||
double minDistance = double.MaxValue;
|
||
int closestIndex = 0;
|
||
|
||
// Start search from current index for efficiency
|
||
for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
|
||
{
|
||
double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
|
||
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
closestIndex = i;
|
||
}
|
||
}
|
||
|
||
// Also check previous waypoints in case robot moved backwards
|
||
for (int i = 0; i < _currentWaypointAheadIndex; i++)
|
||
{
|
||
double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
|
||
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
closestIndex = i;
|
||
}
|
||
}
|
||
|
||
_currentWaypointAheadIndex = closestIndex;
|
||
return (Waypoints_Value[closestIndex], closestIndex);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate path heading at given waypoint index (for Stanley)
|
||
/// Uses current point and next point to determine direction
|
||
/// </summary>
|
||
private double CalculatePathHeading(int index)
|
||
{
|
||
if (index >= Waypoints_Value.Count - 1)
|
||
{
|
||
// Last point - use previous segment direction
|
||
if (index > 0)
|
||
{
|
||
double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
|
||
double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
|
||
return Math.Atan2(dy, dx);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// Use current to next point
|
||
double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
|
||
double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
|
||
return Math.Atan2(dyNext, dxNext);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate signed cross-track error (for Stanley)
|
||
/// Positive: front axle is to the left of path
|
||
/// Negative: front axle is to the right of path
|
||
/// </summary>
|
||
private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
|
||
{
|
||
// Vector from closest point to front axle
|
||
double dx = frontX - closestPoint.X;
|
||
double dy = frontY - closestPoint.Y;
|
||
|
||
// Path direction vector
|
||
double pathDx = Math.Cos(pathHeading);
|
||
double pathDy = Math.Sin(pathHeading);
|
||
|
||
// Cross product to get signed perpendicular distance
|
||
// positive = left, negative = right
|
||
double crossTrackError = dx * pathDy - dy * pathDx;
|
||
|
||
return crossTrackError;
|
||
}
|
||
|
||
#endregion
|
||
|
||
}
|