694 lines
32 KiB
C#
694 lines
32 KiB
C#
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Navigation.CSharp;
|
|
using RobotNet10.RobotApp.Services.Simulation;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.Shared.Geometry;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Navigation;
|
|
|
|
public partial class CSharpNavigation
|
|
{
|
|
private bool IsBackToPath = false;
|
|
private double? BackToAngle;
|
|
private int _overshootCounter = 0;
|
|
private readonly int _overshootOut = 5;
|
|
|
|
private void Rotating()
|
|
{
|
|
if (RotatePID is not null)
|
|
{
|
|
double Error = SpaceCompute.NormalizeRadianAngle(TargetAngle - Localization.Theta);
|
|
|
|
// Skip initial rotation nếu heading error đủ nhỏ
|
|
if (_isInitialRotation)
|
|
{
|
|
double initialThresholdRad = (NavCog?.InitialRotationThreshold ?? 5.0) * Math.PI / 180.0;
|
|
if (Math.Abs(Error) < initialThresholdRad)
|
|
{
|
|
_isInitialRotation = false;
|
|
VelController.SetVelocity(0, 0);
|
|
if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
|
|
{
|
|
ResetOvershootState();
|
|
NavState = NavigationState.Moving;
|
|
}
|
|
else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
|
|
{
|
|
ResetOvershootState();
|
|
NavState = NavigationState.MovingStraight;
|
|
}
|
|
else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
|
|
{
|
|
ResetOvershootState();
|
|
NavState = NavigationState.Docking;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Logger.LogInformation($"Navigation Reached initial heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
|
|
Dispose();
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
double headingToleranceRad = (NavCog?.HeadingTolerance ?? 3.0) * Math.PI / 180.0;
|
|
if (Math.Abs(Error) < headingToleranceRad)
|
|
{
|
|
_isInitialRotation = false;
|
|
if (IsBackToPath && BackToAngle.HasValue)
|
|
{
|
|
TargetAngle = BackToAngle.Value;
|
|
BackToAngle = null;
|
|
IsBackToPath = false;
|
|
}
|
|
else
|
|
{
|
|
ResetOvershootState();
|
|
if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
|
|
{
|
|
NavState = NavigationState.Moving;
|
|
}
|
|
else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
|
|
{
|
|
NavState = NavigationState.MovingStraight;
|
|
}
|
|
else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
|
|
{
|
|
NavState = NavigationState.Docking;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Logger.LogInformation($"Navigation Reached heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
|
|
Dispose();
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var SpeedCal = RotatePID.PID_step(Error, NavCog?.RotateAngularVelocity ?? 0.1, -(NavCog?.RotateAngularVelocity ?? 0.1), CycleHandlerMilliseconds / 1000.0);
|
|
VelController.SetVelocity(0, SpeedCal);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void Moving()
|
|
{
|
|
if (MovePID is not null && MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
|
|
{
|
|
var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Localization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
|
|
var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Localization.X - GoalRotate.X, 2) + Math.Pow(Localization.Y - GoalRotate.Y, 2));
|
|
var reachedRadius = NavCog?.ReachedRadius ?? 0.05;
|
|
var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? reachedRadius : GoalRotate.AllowedDeviationXY ?? 0.1;
|
|
|
|
// Overshoot detection: phát hiện robot đi qua goal
|
|
var overshootDetectionRadius = NavCog?.MovingOvershootDetectionRadius ?? 0.5;
|
|
if (DistanceToGoal < overshootDetectionRadius)
|
|
{
|
|
if (DistanceToGoal < _oldDistanceToGoal)
|
|
{
|
|
_wasApproaching = true;
|
|
_overshootCounter = 0;
|
|
}
|
|
else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
|
|
{
|
|
_overshootCounter++;
|
|
if (_wasApproaching && _overshootCounter >= _overshootOut)
|
|
{
|
|
_wasApproaching = false;
|
|
VelController.SetVelocity(0, 0);
|
|
|
|
// Kiểm tra vị trí overshoot có chấp nhận được không
|
|
double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
|
|
if (DistanceToGoal > acceptanceRadius)
|
|
{
|
|
Logger.LogError($"Moving overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
return;
|
|
}
|
|
|
|
Logger.LogWarning($"Overshoot detected at distance {DistanceToGoal:F4}m (within acceptance={acceptanceRadius}m), transitioning to final rotation");
|
|
if (MovePurePursuit.OrderNodes[^1].Theta is { } overshootTheta)
|
|
{
|
|
TargetAngle = overshootTheta;
|
|
NavState = NavigationState.Rotating;
|
|
RotatePID?.Reset();
|
|
MovePurePursuit = null;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Dispose();
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
else _overshootCounter = 0;
|
|
}
|
|
_oldDistanceToGoal = DistanceToGoal;
|
|
|
|
if (DistanceToCheckingNode > deviation)
|
|
{
|
|
double dt = CycleHandlerMilliseconds / 1000.0;
|
|
double decelerationDist = NavCog?.DecelerationDistance ?? 5.0;
|
|
double maxLinearVel = DistanceToCheckingNode > decelerationDist
|
|
? MaxLinearVelocity
|
|
: MovePID.PID_step(DistanceToCheckingNode, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
|
|
maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
(double linearVelActual, _) = VelController.ActualVelocity;
|
|
(double LinearVel, double AngularVel) = MovePurePursuit.PurePursuit_step(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
|
|
|
|
// Clamp angular velocity
|
|
AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
|
|
|
|
// Clamp linear velocity (giữ dấu cho backward)
|
|
var linearSign = Math.Sign(LinearVel);
|
|
LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
VelController.SetVelocity(LinearVel, AngularVel);
|
|
}
|
|
else if (DistanceToGoal < reachedRadius)
|
|
{
|
|
VelController.SetVelocity(0, 0);
|
|
if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
|
|
{
|
|
TargetAngle = theta;
|
|
NavState = NavigationState.Rotating;
|
|
RotatePID?.Reset();
|
|
MovePurePursuit = null;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Logger.LogInformation($"Navigation Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
|
|
Dispose();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
double? targetAngle = null;
|
|
if (GoalRotate.Theta is { } theta)
|
|
{
|
|
targetAngle = theta;
|
|
ProcessedRotations.Add(GoalRotate.NodeId);
|
|
BackToAngle = GoalRotate?.ContinueTheta;
|
|
IsBackToPath = true;
|
|
}
|
|
|
|
var newGoalRotate = FindNextRotateGoal();
|
|
if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
|
|
{
|
|
GoalRotate = newGoalRotate;
|
|
UpdateGoal(newGoalRotate.NodeId);
|
|
}
|
|
|
|
if (targetAngle.HasValue)
|
|
{
|
|
TargetAngle = targetAngle.Value;
|
|
NavState = NavigationState.Rotating;
|
|
RotatePID?.Reset();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void Docking()
|
|
{
|
|
if (MovePID is not null && DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Goal != null)
|
|
{
|
|
var dockCfg = DockToController.DockConfig;
|
|
|
|
// --- Continuous goal update from detection session ---
|
|
TryUpdateDockGoalFromSession(NavCog.DockToMaxSpeed);
|
|
|
|
var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - DockToController.Goal.X, 2) + Math.Pow(Localization.Y - DockToController.Goal.Y, 2));
|
|
var deviation = dockCfg.ReachedRadius;
|
|
|
|
// Overshoot detection: phát hiện robot đi qua goal
|
|
var dockOvershootRadius = dockCfg.DockingOvershootDetectionRadius;
|
|
if (DistanceToGoal < dockOvershootRadius)
|
|
{
|
|
if (DistanceToGoal < _oldDistanceToGoal)
|
|
{
|
|
_wasApproaching = true;
|
|
_overshootCounter = 0;
|
|
}
|
|
else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
|
|
{
|
|
_overshootCounter++;
|
|
if (_wasApproaching && _overshootCounter >= _overshootOut)
|
|
{
|
|
_wasApproaching = false;
|
|
_overshootCounter = 0;
|
|
VelController.SetVelocity(0, 0);
|
|
|
|
if (_hasLoad)
|
|
{
|
|
// When loaded: no FinePositioning allowed, go to Error
|
|
Logger.LogError($"Docking overshoot with load at distance {DistanceToGoal:F4}m. No FinePositioning allowed.");
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
return;
|
|
}
|
|
|
|
Logger.LogWarning(
|
|
$"Overshoot detected at distance {DistanceToGoal:F4}m. " +
|
|
$"Entering FinePositioning (attempt {_finePositioningRetryCount + 1}/{_finePositioningMaxRetries}).");
|
|
|
|
// Transition to FinePositioning instead of giving up
|
|
_finePositioningCycleCount = 0;
|
|
_finePositioningIsAligning = true;
|
|
_finePositionRotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
|
_oldDistanceToGoal = double.MaxValue;
|
|
_fpWasApproaching = false;
|
|
_fpOvershootCounter = 0;
|
|
NavState = NavigationState.FinePositioning;
|
|
return;
|
|
}
|
|
}
|
|
else _overshootCounter = 0;
|
|
}
|
|
_oldDistanceToGoal = DistanceToGoal;
|
|
|
|
if (DistanceToGoal > deviation)
|
|
{
|
|
double dt = CycleHandlerMilliseconds / 1000.0;
|
|
double dockDecelerationDist = dockCfg.DecelerationDistance;
|
|
double maxLinearVel = DistanceToGoal > dockDecelerationDist
|
|
? MaxLinearVelocity
|
|
: MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
|
|
maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
(double linearVelActual, _) = VelController.ActualVelocity;
|
|
(double LinearVel, double AngularVel) = DockToController.FinalApproachController(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
|
|
|
|
// Clamp angular velocity
|
|
AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
|
|
|
|
// Clamp linear velocity (giữ dấu cho backward)
|
|
var linearSign = Math.Sign(LinearVel);
|
|
LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
VelController.SetVelocity(LinearVel, AngularVel);
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"Dock To reached. Pose= ({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance to goal: {DistanceToGoal}");
|
|
VelController.SetVelocity(0, 0);
|
|
if (!_hasLoad && DockToController.Goal.Theta is { } theta)
|
|
{
|
|
TargetAngle = theta;
|
|
NavState = NavigationState.Rotating;
|
|
RotatePID?.Reset();
|
|
DockToController = null;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Logger.LogInformation($"DockTo Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
|
|
Dispose();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void MovingStraight()
|
|
{
|
|
if (MovePID is not null && MoveStraightController is not null
|
|
&& MoveStraightController.Waypoints_Value is not null && MoveStraightController.Goal != null)
|
|
{
|
|
var cfg = MoveStraightController.DockConfig;
|
|
var DistanceToGoal = Math.Sqrt(
|
|
Math.Pow(Localization.X - MoveStraightController.Goal.X, 2) +
|
|
Math.Pow(Localization.Y - MoveStraightController.Goal.Y, 2));
|
|
var deviation = cfg.ReachedRadius;
|
|
|
|
// Overshoot detection (same pattern as Moving)
|
|
var overshootRadius = cfg.DockingOvershootDetectionRadius;
|
|
if (DistanceToGoal < overshootRadius)
|
|
{
|
|
if (DistanceToGoal < _oldDistanceToGoal)
|
|
{
|
|
_wasApproaching = true;
|
|
_overshootCounter = 0;
|
|
}
|
|
else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
|
|
{
|
|
_overshootCounter++;
|
|
if (_wasApproaching && _overshootCounter >= _overshootOut)
|
|
{
|
|
_wasApproaching = false;
|
|
VelController.SetVelocity(0, 0);
|
|
|
|
double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
|
|
if (DistanceToGoal > acceptanceRadius)
|
|
{
|
|
Logger.LogError($"MovingStraight overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
return;
|
|
}
|
|
|
|
Logger.LogWarning($"MovingStraight overshoot at {DistanceToGoal:F4}m (within acceptance). Completing.");
|
|
NavState = NavigationState.Completed;
|
|
Dispose();
|
|
return;
|
|
}
|
|
}
|
|
else _overshootCounter = 0;
|
|
}
|
|
_oldDistanceToGoal = DistanceToGoal;
|
|
|
|
if (DistanceToGoal > deviation)
|
|
{
|
|
double dt = CycleHandlerMilliseconds / 1000.0;
|
|
double decelerationDist = cfg.DecelerationDistance;
|
|
double maxLinearVel = DistanceToGoal > decelerationDist
|
|
? MaxLinearVelocity
|
|
: MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
|
|
maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
(double linearVelActual, _) = VelController.ActualVelocity;
|
|
(double LinearVel, double AngularVel) = MoveStraightController.FinalApproachController(
|
|
Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
|
|
|
|
AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
|
|
|
|
// Clamp linear velocity (giữ dấu cho backward)
|
|
var linearSign = Math.Sign(LinearVel);
|
|
LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
|
|
|
|
VelController.SetVelocity(LinearVel, AngularVel);
|
|
}
|
|
else
|
|
{
|
|
Logger.LogInformation($"MoveStraight reached. Pose=({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance: {DistanceToGoal}");
|
|
VelController.SetVelocity(0, 0);
|
|
NavState = NavigationState.Completed;
|
|
Dispose();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// FinePositioning: Cơ chế retry chính xác khi Docking overshoot.
|
|
/// Phase 1 (Align): Xoay tại chỗ hướng về goal theo góc nhỏ nhất (forward hoặc backward).
|
|
/// Hướng được lock 1 lần khi vào align, không thay đổi trong suốt quá trình xoay.
|
|
/// Phase 2 (Advance): Tiến thẳng về goal ở DockToRetrySpeed với P-correction trên angular velocity.
|
|
/// Success: distance ≤ ReachedRadius → final rotation hoặc Completed.
|
|
/// Failure: hết retry hoặc timeout → Error.
|
|
/// </summary>
|
|
private void FinePositioning()
|
|
{
|
|
if (DockToController?.Goal is not { } goal)
|
|
{
|
|
Logger.LogError("FinePositioning: DockToController or Goal is null. Aborting.");
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
return;
|
|
}
|
|
|
|
// Continuously update goal from dock session (same as Docking phase)
|
|
var updatedGoal = TryUpdateDockGoalFromSession(NavCog.DockToRetrySpeed);
|
|
if (updatedGoal is not null) goal = updatedGoal;
|
|
|
|
// Per-attempt time-based timeout
|
|
_finePositioningCycleCount++;
|
|
int elapsedMs = _finePositioningCycleCount * CycleHandlerMilliseconds;
|
|
if (elapsedMs > _finePositioningTimeoutMs)
|
|
{
|
|
_finePositioningRetryCount++;
|
|
Logger.LogWarning(
|
|
$"FinePositioning attempt {_finePositioningRetryCount} timed out " +
|
|
$"(elapsed {elapsedMs}ms > {_finePositioningTimeoutMs}ms).");
|
|
|
|
if (_finePositioningRetryCount >= _finePositioningMaxRetries)
|
|
{
|
|
Logger.LogError(
|
|
$"FinePositioning exhausted all {_finePositioningMaxRetries} retries. Transitioning to Error.");
|
|
VelController.SetVelocity(0, 0);
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
}
|
|
else
|
|
{
|
|
VelController.SetVelocity(0, 0);
|
|
_finePositioningCycleCount = 0;
|
|
_finePositioningIsAligning = true;
|
|
_finePositionRotatePID?.Reset();
|
|
_oldDistanceToGoal = double.MaxValue;
|
|
_fpWasApproaching = false;
|
|
_fpOvershootCounter = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
double robotX = Localization.X;
|
|
double robotY = Localization.Y;
|
|
double robotTheta = Localization.Theta;
|
|
|
|
double dx = goal.X - robotX;
|
|
double dy = goal.Y - robotY;
|
|
double distanceToGoal = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
// Success check
|
|
double reachedRadius = DockToController.DockConfig.ReachedRadius;
|
|
if (distanceToGoal <= reachedRadius)
|
|
{
|
|
Logger.LogInformation(
|
|
$"FinePositioning SUCCESS: distance={distanceToGoal:F4}m, " +
|
|
$"Pose({robotX:F3}, {robotY:F3}, {robotTheta:F3}) → Goal({goal.X:F3}, {goal.Y:F3}, {goal.Theta:F3}, {_dockSession?.Goal?.Header.FrameId}, {_dockSession?.Goal?.Header.Stamp}). ");
|
|
VelController.SetVelocity(0, 0);
|
|
|
|
if (goal.Theta is { } finalTheta)
|
|
{
|
|
TargetAngle = finalTheta;
|
|
NavState = NavigationState.Rotating;
|
|
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
|
DockToController = null;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
DockToController = null;
|
|
Dispose();
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Chọn hướng tiếp cận có góc xoay nhỏ nhất (forward hoặc backward)
|
|
// Direction chỉ được chọn 1 lần khi vào align phase (cycle đầu tiên),
|
|
// sau đó giữ nguyên suốt align + advance để tránh flip do sensor noise.
|
|
double rawHeading = Math.Atan2(dy, dx);
|
|
|
|
if (_finePositioningIsAligning && _finePositioningCycleCount == 1)
|
|
{
|
|
double fwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading) - robotTheta;
|
|
if (fwdErr > Math.PI) fwdErr -= 2 * Math.PI;
|
|
else if (fwdErr < -Math.PI) fwdErr += 2 * Math.PI;
|
|
|
|
double bwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI) - robotTheta;
|
|
if (bwdErr > Math.PI) bwdErr -= 2 * Math.PI;
|
|
else if (bwdErr < -Math.PI) bwdErr += 2 * Math.PI;
|
|
|
|
_finePositioningDirection = Math.Abs(fwdErr) <= Math.Abs(bwdErr)
|
|
? RobotDirection.FORWARD
|
|
: RobotDirection.BACKWARD;
|
|
}
|
|
|
|
// Tính heading error theo hướng đã lock
|
|
double targetHeading = _finePositioningDirection == RobotDirection.BACKWARD
|
|
? SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI)
|
|
: SpaceCompute.NormalizeRadianAngle(rawHeading);
|
|
|
|
double headingError = targetHeading - robotTheta;
|
|
if (headingError > Math.PI) headingError -= 2 * Math.PI;
|
|
else if (headingError < -Math.PI) headingError += 2 * Math.PI;
|
|
|
|
double FineAlignThresholdRad = (DockToController.DockConfig.FineAlignThresholdDegrees) * Math.PI / 180.0;
|
|
double ReAlignThresholdRad = (DockToController.DockConfig.ReAlignThresholdDegrees) * Math.PI / 180.0;
|
|
|
|
// Overshoot detection during advance phase — consecutive increase pattern
|
|
if (!_finePositioningIsAligning)
|
|
{
|
|
if (distanceToGoal < _oldDistanceToGoal)
|
|
{
|
|
_fpWasApproaching = true;
|
|
_fpOvershootCounter = 0;
|
|
}
|
|
else if (_fpWasApproaching && distanceToGoal > _oldDistanceToGoal)
|
|
{
|
|
_fpOvershootCounter++;
|
|
if (_fpOvershootCounter >= DockToController.DockConfig.FinePositioningOvershootCount)
|
|
{
|
|
_finePositioningRetryCount++;
|
|
_fpWasApproaching = false;
|
|
_fpOvershootCounter = 0;
|
|
VelController.SetVelocity(0, 0);
|
|
|
|
if (_finePositioningRetryCount >= _finePositioningMaxRetries)
|
|
{
|
|
Logger.LogError("FinePositioning: repeated overshoot during advance. Giving up.");
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
return;
|
|
}
|
|
|
|
Logger.LogWarning(
|
|
$"FinePositioning: overshoot during advance (dist={distanceToGoal:F4}m). " +
|
|
$"Retry {_finePositioningRetryCount}/{_finePositioningMaxRetries}.");
|
|
_finePositioningIsAligning = true;
|
|
_finePositionRotatePID?.Reset();
|
|
_finePositioningCycleCount = 0;
|
|
_oldDistanceToGoal = double.MaxValue;
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_fpOvershootCounter = 0;
|
|
}
|
|
}
|
|
_oldDistanceToGoal = distanceToGoal;
|
|
|
|
// Phase 1: ALIGN — xoay tại chỗ theo góc nhỏ nhất
|
|
if (_finePositioningIsAligning)
|
|
{
|
|
if (Math.Abs(headingError) < FineAlignThresholdRad)
|
|
{
|
|
_finePositioningIsAligning = false;
|
|
VelController.SetVelocity(0, 0);
|
|
Logger.LogInformation(
|
|
$"FinePositioning aligned ({_finePositioningDirection}): " +
|
|
$"headingError={headingError * 180 / Math.PI:F1}°. Advancing.");
|
|
}
|
|
else
|
|
{
|
|
_finePositionRotatePID ??= new PID(NavigationConfig.GetRotatePidConfig());
|
|
double rotateAngularVel = NavCog?.DockToRotateSpeed ?? 0.05;
|
|
double angularCmd = _finePositionRotatePID.PID_step(
|
|
headingError, rotateAngularVel, -rotateAngularVel, CycleHandlerMilliseconds / 1000.0);
|
|
VelController.SetVelocity(0, angularCmd);
|
|
Console.WriteLine(
|
|
$"FinePos-Align({_finePositioningDirection}): " +
|
|
$"HeadErr={headingError * 180 / Math.PI:F1}°, AngVel={angularCmd:F4}, Dist={distanceToGoal:F4}m");
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Phase 2: ADVANCE — tiến thẳng về goal theo hướng đã chọn ở Phase 1
|
|
if (Math.Abs(headingError) > ReAlignThresholdRad)
|
|
{
|
|
_finePositioningIsAligning = true;
|
|
_finePositionRotatePID?.Reset();
|
|
VelController.SetVelocity(0, 0);
|
|
Logger.LogWarning($"FinePositioning heading drift: {headingError * 180 / Math.PI:F1}°. Re-aligning.");
|
|
return;
|
|
}
|
|
|
|
double minLinVel = NavCog?.DockToRetrySpeed ?? 0.05;
|
|
double linearCmd = _finePositioningDirection == RobotDirection.BACKWARD ? -minLinVel : minLinVel;
|
|
|
|
// Corrective angular velocity proportional to heading error
|
|
// Dùng Max(|actualVel|, minLinVel) để đảm bảo correction không bằng 0 khi vừa bắt đầu advance
|
|
(double linearVelActual, _) = VelController.ActualVelocity;
|
|
double effectiveVel = Math.Max(Math.Abs(linearVelActual), minLinVel);
|
|
double headingGain = DockToController.DockConfig.AdvanceHeadingCorrectionGain;
|
|
double dockAdvanceMaxAngVel = DockToController.DockConfig.DockToAdvanceMaxAngularVelocity;
|
|
double advanceAngularCorrection = Math.Clamp(headingError * headingGain * effectiveVel, -dockAdvanceMaxAngVel, dockAdvanceMaxAngVel);
|
|
|
|
VelController.SetVelocity(linearCmd, advanceAngularCorrection);
|
|
Console.WriteLine(
|
|
$"FinePos-Advance({_finePositioningDirection}): LinVel={linearCmd:F4}, AngCorr={advanceAngularCorrection:F4}, " +
|
|
$"Dist={distanceToGoal:F4}m, HeadErr={headingError * 180 / Math.PI:F1}°");
|
|
}
|
|
|
|
private NavigationNode? TryUpdateDockGoalFromSession(double speed)
|
|
{
|
|
if (_dockSession is null || DockToController?.DockConfig is not { } dockCfg)
|
|
return null;
|
|
|
|
var snapshot = _dockSession.Goal;
|
|
if (!snapshot.HasValue) return null;
|
|
|
|
var pose = snapshot.Value.Pose;
|
|
|
|
var oldGoal = DockToController!.Goal;
|
|
double dxGoal = pose.Position.X - oldGoal.X;
|
|
double dyGoal = pose.Position.Y - oldGoal.Y;
|
|
double distShift = Math.Sqrt(dxGoal * dxGoal + dyGoal * dyGoal);
|
|
|
|
double newTheta = pose.Orientation.ToYawRadian();
|
|
double oldTheta = oldGoal.Theta ?? 0;
|
|
double angleShift = Math.Abs(SpaceCompute.NormalizeRadianAngle(newTheta - oldTheta));
|
|
double maxAngleShiftRad = dockCfg.MaxGoalAngleShiftDegrees * Math.PI / 180.0;
|
|
|
|
if (distShift > dockCfg.MaxGoalPositionShift || angleShift > maxAngleShiftRad)
|
|
return null;
|
|
|
|
var updatedGoal = new NavigationNode()
|
|
{
|
|
Id = Guid.NewGuid(),
|
|
X = pose.Position.X,
|
|
Y = pose.Position.Y,
|
|
Speed = speed,
|
|
Theta = newTheta,
|
|
};
|
|
|
|
var newStartNode = GetDockToStartNode(
|
|
Localization.X, Localization.Y,
|
|
updatedGoal.X, updatedGoal.Y,
|
|
updatedGoal.Theta ?? 0,
|
|
dockCfg.DockToLength);
|
|
|
|
DockToController.WithPath(newStartNode, updatedGoal);
|
|
return updatedGoal;
|
|
}
|
|
|
|
private void NavigationHandler()
|
|
{
|
|
try
|
|
{
|
|
switch (NavState)
|
|
{
|
|
case NavigationState.Rotating:
|
|
Rotating();
|
|
break;
|
|
case NavigationState.Moving:
|
|
Moving();
|
|
break;
|
|
case NavigationState.Docking:
|
|
Docking();
|
|
break;
|
|
case NavigationState.MovingStraight:
|
|
MovingStraight();
|
|
break;
|
|
case NavigationState.FinePositioning:
|
|
FinePositioning();
|
|
break;
|
|
case NavigationState.Paused:
|
|
case NavigationState.SafetyStop:
|
|
if (!_idleVelocityZeroSent)
|
|
{
|
|
VelController.SetVelocity(0, 0);
|
|
_idleVelocityZeroSent = true;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
NavState = NavigationState.Error;
|
|
Dispose();
|
|
Logger.LogError($"Error in DifferentialNavigation: {ex.Message}");
|
|
}
|
|
}
|
|
}
|