Initial commit
This commit is contained in:
@@ -0,0 +1,693 @@
|
||||
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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user