875 lines
35 KiB
C#
875 lines
35 KiB
C#
using RobotNet.VDA5050.Order;
|
||
using RobotNet10.Common;
|
||
using RobotNet10.RobotApp.Detection;
|
||
using RobotNet10.RobotApp.Interfaces;
|
||
using RobotNet10.RobotApp.Services.ConfigManager;
|
||
using RobotNet10.RobotApp.Services.Exceptions;
|
||
using RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||
using RobotNet10.RobotApp.Services.Robot.Models;
|
||
using RobotNet10.RobotApp.Services.Simulation;
|
||
using RobotNet10.RobotApp.Shared.Enums;
|
||
using RobotNet10.Shared.Geometry;
|
||
|
||
namespace RobotNet10.RobotApp.Services.Navigation;
|
||
|
||
public class NavigationConfig
|
||
{
|
||
public double MaxLinearVelocity { get; set; }
|
||
public double MaxAngularVelocity { get; set; }
|
||
public double MinLinearVelocity { get; set; }
|
||
public double RotateAngularVelocity { get; set; }
|
||
public double Acceleration { get; set; } = 0.5;
|
||
public double Deceleration { get; set; } = 0.5;
|
||
public double ReachedRadius { get; set; } = 0.03;
|
||
public double HeadingTolerance { get; set; } = 3.0;
|
||
public double InitialRotationThreshold { get; set; } = 5.0;
|
||
public double DockToMaxSpeed { get; set; } = 0.3;
|
||
public double DockToRetrySpeed { get; set; } = 0.05;
|
||
public double DockToRotateSpeed { get; set; } = 0.05;
|
||
public Dictionary<SafetySpeed, double> SafetySpeedMap { get; set; } = [];
|
||
/// <summary>
|
||
/// Maximum distance (meters) from goal at which a Moving overshoot is still accepted as Completed.
|
||
/// Default: 0.15m
|
||
///
|
||
/// Meaning: When overshoot is detected during Moving, if robot is within this radius of the goal,
|
||
/// navigation proceeds to final rotation → Completed. Otherwise → Error.
|
||
///
|
||
/// ↑ Increase (0.2-0.3):
|
||
/// ✓ More tolerant of overshoot — fewer Error states
|
||
/// ✗ Robot may report Completed at a position far from goal
|
||
///
|
||
/// ↓ Decrease (0.05-0.1):
|
||
/// ✓ Higher positional accuracy requirement
|
||
/// ✗ More likely to trigger Error on minor overshoot
|
||
///
|
||
/// Tuning Tips:
|
||
/// - Should be ≥ ReachedRadius
|
||
/// - Must be ≤ MovingOvershootDetectionRadius to be meaningful
|
||
/// - For high-precision tasks: 0.05-0.1
|
||
/// - For general navigation: 0.15-0.2
|
||
/// </summary>
|
||
public double OvershootAcceptanceRadius { get; set; } = 0.15;
|
||
|
||
/// <summary>
|
||
/// Distance (meters) from goal at which overshoot detection begins during Moving.
|
||
/// Default: 0.5m
|
||
///
|
||
/// Meaning: Overshoot detection only activates when robot is within this radius of the final goal.
|
||
/// Outside this radius, distance fluctuations are ignored.
|
||
///
|
||
/// ↑ Increase (0.8-1.0):
|
||
/// ✓ Earlier overshoot detection
|
||
/// ✗ May false-trigger on path curvature near goal
|
||
///
|
||
/// ↓ Decrease (0.2-0.3):
|
||
/// ✓ Fewer false triggers
|
||
/// ✗ Late detection — robot may travel further past goal before stopping
|
||
///
|
||
/// Tuning Tips:
|
||
/// - Should be > OvershootAcceptanceRadius
|
||
/// - Typical: 2-5x the ReachedRadius
|
||
/// - If robot has high inertia/speed: increase to 0.8-1.0
|
||
/// </summary>
|
||
public double MovingOvershootDetectionRadius { get; set; } = 0.5;
|
||
|
||
/// <summary>
|
||
/// Distance (meters) from checkpoint at which PID deceleration begins during Moving.
|
||
/// Default: 5.0m
|
||
///
|
||
/// Meaning: When distance to checkpoint > this value, robot runs at MaxLinearVelocity.
|
||
/// Below this distance, PID ramps velocity down proportionally.
|
||
///
|
||
/// ↑ Increase (7-10):
|
||
/// ✓ Earlier, smoother deceleration
|
||
/// ✗ Slower average speed on long paths
|
||
///
|
||
/// ↓ Decrease (2-3):
|
||
/// ✓ Faster average speed — stays at max longer
|
||
/// ✗ Sharper deceleration, may overshoot on heavy robots
|
||
///
|
||
/// Tuning Tips:
|
||
/// - Depends on MaxLinearVelocity and robot mass/inertia
|
||
/// - Rule of thumb: stopping distance ≈ v² / (2 × deceleration)
|
||
/// - Heavy/fast robot: 7-10m; Light/slow robot: 2-3m
|
||
/// </summary>
|
||
public double DecelerationDistance { get; set; } = 5.0;
|
||
|
||
/// <summary>
|
||
/// Maximum linear velocity (m/s) when robot is carrying a load during Moving.
|
||
/// Default: 0.3 m/s
|
||
///
|
||
/// When hasLoad=true, MaxLinearVelocity is capped at min(MaxLinearVelocity, LoadedMaxLinearVelocity).
|
||
/// </summary>
|
||
public double LoadedMaxLinearVelocity { get; set; } = 0.3;
|
||
|
||
/// <summary>
|
||
/// Maximum heading error (degrees) allowed when starting MoveStraight or Docking with a load.
|
||
/// Default: 10.0 degrees
|
||
///
|
||
/// When hasLoad=true, the robot cannot rotate to correct heading before MoveStraight/Docking.
|
||
/// If the heading error exceeds this threshold at start, navigation transitions to Error.
|
||
/// </summary>
|
||
public double LoadedHeadingErrorThresholdDegrees { get; set; } = 10.0;
|
||
}
|
||
|
||
public partial class CSharpNavigation : INavigation, IDisposable
|
||
{
|
||
public bool IsReady { get; private set; }
|
||
public bool Driving => NavState is NavigationState.Rotating or NavigationState.Moving or NavigationState.Docking or NavigationState.FinePositioning or NavigationState.MovingStraight or NavigationState.SafetyStop;
|
||
public double VelocityX => VelController.ActualVelocity.Linear;
|
||
public double VelocityY { get; private set; }
|
||
public double Omega => VelController.ActualVelocity.Angular;
|
||
public NavigationState State => NavState;
|
||
public IReadOnlyList<NavigationNode>? CurrentWaypoints => MovePurePursuit?.Waypoints_Value ?? MoveStraightController?.Waypoints_Value ?? DockToController?.Waypoints_Value;
|
||
|
||
// DockTo monitoring properties (read-only, for NavigationMonitor)
|
||
public bool IsDockingActive => NavState is NavigationState.Docking or NavigationState.FinePositioning;
|
||
public NavigationNode? DockGoal => DockToController?.Goal;
|
||
public string DockPhase => NavState switch
|
||
{
|
||
NavigationState.Docking => "Approaching",
|
||
NavigationState.FinePositioning when _finePositioningIsAligning => "Aligning",
|
||
NavigationState.FinePositioning => "Advancing",
|
||
_ => ""
|
||
};
|
||
public string DockDirection => DockToController?.DockConfig?.DockToDirection.ToString() ?? "";
|
||
public int DockRetryCount => _finePositioningRetryCount;
|
||
public int DockMaxRetries => _finePositioningMaxRetries;
|
||
public int DockWaypointCount => DockToController?.Waypoints_Value?.Count ?? 0;
|
||
public NavigationNode? DockStartNode => DockToController?.StartNode;
|
||
public IReadOnlyList<NavigationNode>? DockWaypoints => DockToController?.Waypoints_Value;
|
||
|
||
public event Action<NavigationState>? OnNavigationFinished;
|
||
|
||
private readonly ILocalization Localization;
|
||
public readonly IVelocityController VelController;
|
||
private readonly INavigationConfig NavigationConfig;
|
||
private readonly ILogger<CSharpNavigation> Logger;
|
||
|
||
private NavigationState NavState = NavigationState.Idle;
|
||
private NavigationState ResumeState = NavigationState.Idle;
|
||
|
||
// Safety stop: saves previous state for Refresh() to resume from
|
||
private NavigationState _safetyStopPreviousState = NavigationState.Idle;
|
||
|
||
private WatchThread<CSharpNavigation>? NavThread = null;
|
||
private const int CycleHandlerMilliseconds = 30;
|
||
|
||
private PID? MovePID;
|
||
private PurePursuit? MovePurePursuit;
|
||
private DockToController? DockToController;
|
||
private DockToController? MoveStraightController;
|
||
private IDetectSession? _dockSession;
|
||
|
||
private OrderNode? GoalRotate;
|
||
private OrderNode? CurrentBaseNode;
|
||
private HashSet<string> ProcessedRotations = [];
|
||
|
||
private double TargetAngle = 0;
|
||
private PID? RotatePID;
|
||
|
||
private readonly NavigationConfig NavCog;
|
||
private double MaxLinearVelocity = 0;
|
||
|
||
// Overshoot detection
|
||
private double _oldDistanceToGoal = double.MaxValue;
|
||
private bool _wasApproaching = false;
|
||
|
||
// Initial rotation skip
|
||
private bool _isInitialRotation = false;
|
||
|
||
// Fine Positioning state
|
||
private int _finePositioningRetryCount = 0;
|
||
private int _finePositioningMaxRetries = 3;
|
||
private int _finePositioningCycleCount = 0;
|
||
private int _finePositioningTimeoutMs = 6000;
|
||
private bool _finePositioningIsAligning = true;
|
||
private PID? _finePositionRotatePID = null;
|
||
private RobotDirection _finePositioningDirection = RobotDirection.FORWARD;
|
||
private bool _fpWasApproaching = false;
|
||
private int _fpOvershootCounter = 0;
|
||
|
||
private int _disposed = 0;
|
||
|
||
// HasLoad: robot is carrying a load — affects speed limits, rotation, and FinePositioning
|
||
private bool _hasLoad = false;
|
||
|
||
// Tracks whether velocity (0,0) has already been sent when entering Paused/SafetyStop.
|
||
// Prevents Navigation from continuously overwriting ManualControl velocity commands.
|
||
private bool _idleVelocityZeroSent = false;
|
||
|
||
private void ResetOvershootState()
|
||
{
|
||
_oldDistanceToGoal = double.MaxValue;
|
||
_wasApproaching = false;
|
||
_overshootCounter = 0;
|
||
}
|
||
|
||
public CSharpNavigation(IServiceProvider ServiceProvider)
|
||
{
|
||
Localization = ServiceProvider.GetRequiredService<ILocalization>();
|
||
VelController = ServiceProvider.GetRequiredService<IVelocityController>();
|
||
NavigationConfig = ServiceProvider.GetRequiredService<INavigationConfig>();
|
||
Logger = ServiceProvider.GetRequiredService<ILogger<CSharpNavigation>>();
|
||
NavCog = NavigationConfig.GetNavigationConfig();
|
||
MaxLinearVelocity = NavCog.MaxLinearVelocity;
|
||
}
|
||
|
||
public void Dispose()
|
||
{
|
||
if (Interlocked.Exchange(ref _disposed, 1) != 0) return;
|
||
HandleNavigationStop();
|
||
Clear();
|
||
OnNavigationFinished?.Invoke(NavState);
|
||
GC.SuppressFinalize(this);
|
||
}
|
||
|
||
public void Start()
|
||
{
|
||
IsReady = true;
|
||
}
|
||
|
||
public void Stop()
|
||
{
|
||
Dispose();
|
||
}
|
||
|
||
protected void HandleNavigationStart()
|
||
{
|
||
NavThread = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
|
||
NavThread.Start();
|
||
}
|
||
|
||
protected void HandleNavigationStop()
|
||
{
|
||
NavThread?.Dispose();
|
||
NavThread = null;
|
||
}
|
||
|
||
public void CancelMovement()
|
||
{
|
||
NavState = NavigationState.Canceled;
|
||
Dispose();
|
||
}
|
||
|
||
public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
|
||
{
|
||
var nodes = order.Nodes;
|
||
var edges = order.Edges;
|
||
if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
|
||
|
||
_hasLoad = hasLoad;
|
||
NavState = NavigationState.Initializing;
|
||
if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
|
||
}
|
||
VelController.LoadConfig();
|
||
VelController.SetAcceleration(NavCog.Acceleration);
|
||
VelController.SetDeceleration(NavCog.Deceleration);
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
var ppConfig = NavigationConfig.GetPurepursuitConfig();
|
||
MovePurePursuit = new PurePursuit(NavigationConfig.GetPurepursuitConfig(), NavigationConfig.GetStanleyConig()).WithPath(nodes, edges, Localization.Theta);
|
||
|
||
// Reset overshoot detection state
|
||
ResetOvershootState();
|
||
_isInitialRotation = true;
|
||
|
||
(_, int index) = MovePurePursuit.OnNode(Localization.X, Localization.Y);
|
||
if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
|
||
{
|
||
NavState = NavigationState.Completed;
|
||
Dispose();
|
||
return;
|
||
}
|
||
|
||
// === Local Planner: sinh approach path ===
|
||
var lpConfig = NavigationConfig.GetLocalPlannerConfig();
|
||
double heading;
|
||
|
||
var approachResult = lpConfig.Enabled
|
||
? MovePurePursuit.GenerateAndPrependApproachPath(
|
||
Localization.X, Localization.Y, index, lpConfig)
|
||
: ApproachResult.Disabled;
|
||
|
||
if (approachResult == ApproachResult.ApproachGenerated)
|
||
{
|
||
// Heading = hướng tiếp tuyến đầu approach curve (P0 → P1)
|
||
var wp0 = MovePurePursuit.Waypoints_Value[0];
|
||
var wp1 = MovePurePursuit.Waypoints_Value[1];
|
||
heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
|
||
|
||
if (wp0.Direction == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
}
|
||
else if (approachResult == ApproachResult.AlreadyOnPath)
|
||
{
|
||
// Robot đã trên path → dùng path tangent tại closest waypoint
|
||
var wp = MovePurePursuit.Waypoints_Value[index];
|
||
var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
|
||
heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
|
||
|
||
if (wp.Direction == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
}
|
||
else
|
||
{
|
||
// TooFarFromPath / Disabled: heading hướng về lookahead point
|
||
double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
|
||
var targetPoint = FindLookaheadTarget(index, lookahead);
|
||
targetPoint ??= MovePurePursuit.Waypoints_Value[^1];
|
||
|
||
heading = Math.Atan2(targetPoint.Y - Localization.Y, targetPoint.X - Localization.X);
|
||
if (targetPoint.Direction == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
}
|
||
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
// Cap speed when loaded
|
||
if (_hasLoad)
|
||
{
|
||
MaxLinearVelocity = Math.Min(MaxLinearVelocity, NavCog.LoadedMaxLinearVelocity);
|
||
}
|
||
|
||
Rotate(heading);
|
||
}
|
||
|
||
public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
|
||
{
|
||
if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
|
||
|
||
_hasLoad = hasLoad;
|
||
NavState = NavigationState.Initializing;
|
||
if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
|
||
}
|
||
VelController.LoadConfig();
|
||
VelController.SetAcceleration(NavCog.Acceleration);
|
||
VelController.SetDeceleration(NavCog.Deceleration);
|
||
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
var straightCfg = NavigationConfig.GetMoveStraightConfig().Clone();
|
||
if (direction.HasValue) straightCfg.DockToDirection = direction.Value;
|
||
|
||
var startNode = new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
X = Localization.X,
|
||
Y = Localization.Y,
|
||
};
|
||
|
||
var goalNode = new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
X = x,
|
||
Y = y,
|
||
};
|
||
|
||
MoveStraightController = new DockToController(straightCfg).WithPath(startNode, goalNode);
|
||
|
||
(_, int index) = MoveStraightController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
|
||
if (index >= MoveStraightController.Waypoints_Value.Count - 1)
|
||
{
|
||
NavState = NavigationState.Completed;
|
||
Dispose();
|
||
return;
|
||
}
|
||
|
||
double heading = Math.Atan2(y - Localization.Y, x - Localization.X);
|
||
if (straightCfg.DockToDirection == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
ResetOvershootState();
|
||
MaxLinearVelocity = NavCog.MaxLinearVelocity;
|
||
|
||
if (_hasLoad)
|
||
{
|
||
// When loaded: no rotation allowed. Check heading error.
|
||
double headingError = heading - Localization.Theta;
|
||
if (headingError > Math.PI) headingError -= 2 * Math.PI;
|
||
else if (headingError < -Math.PI) headingError += 2 * Math.PI;
|
||
|
||
double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
|
||
if (Math.Abs(headingError) > thresholdRad)
|
||
{
|
||
Logger.LogError($"MoveStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
|
||
NavState = NavigationState.Error;
|
||
Dispose();
|
||
return;
|
||
}
|
||
|
||
// Skip rotation, go directly to MovingStraight
|
||
NavState = NavigationState.MovingStraight;
|
||
HandleNavigationStart();
|
||
}
|
||
else
|
||
{
|
||
_isInitialRotation = true;
|
||
Rotate(heading);
|
||
}
|
||
}
|
||
|
||
public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
|
||
{
|
||
if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
|
||
var goal = session.Goal ?? throw new NavigationException("Dock to Goal is not existed");
|
||
_hasLoad = hasLoad;
|
||
_dockSession = session;
|
||
NavState = NavigationState.Initializing;
|
||
|
||
if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
|
||
}
|
||
VelController.LoadConfig();
|
||
VelController.SetAcceleration(NavCog.Acceleration);
|
||
VelController.SetDeceleration(NavCog.Deceleration);
|
||
|
||
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
var docktoConfig = NavigationConfig.GetDockToConfig().Clone();
|
||
if (direction.HasValue) docktoConfig.DockToDirection = direction.Value;
|
||
var currentGoal = new NavigationNode()
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
X = goal.Pose.Position.X,
|
||
Y = goal.Pose.Position.Y,
|
||
Speed = NavCog.DockToMaxSpeed,
|
||
Theta = goal.Pose.Orientation.ToYawRadian(),
|
||
};
|
||
|
||
var startNode = GetDockToStartNode(Localization.X, Localization.Y, currentGoal.X, currentGoal.Y, currentGoal.Theta ?? 0, docktoConfig.DockToLength);
|
||
DockToController = new DockToController(docktoConfig).WithPath(startNode, currentGoal);
|
||
|
||
(_, int index) = DockToController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
|
||
if (index >= DockToController.Waypoints_Value.Count - 1)
|
||
{
|
||
NavState = NavigationState.Completed;
|
||
|
||
Dispose();
|
||
return;
|
||
}
|
||
|
||
var pathAngle = Math.Atan2(currentGoal.Y - startNode.Y, currentGoal.X - startNode.X);
|
||
|
||
double frontX = DockToController.Waypoints_Value[index].X + docktoConfig.WheelBase * Math.Cos(pathAngle);
|
||
double frontY = DockToController.Waypoints_Value[index].Y + docktoConfig.WheelBase * Math.Sin(pathAngle);
|
||
|
||
double dx = frontX - Localization.X;
|
||
double dy = frontY - Localization.Y;
|
||
double heading = Math.Atan2(dy, dx);
|
||
|
||
if (docktoConfig.DockToDirection == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
ResetOvershootState();
|
||
MaxLinearVelocity = NavCog.DockToMaxSpeed;
|
||
|
||
// Load Fine Positioning config from DockToConfig
|
||
_finePositioningTimeoutMs = docktoConfig.FinePositioningTimeoutMs;
|
||
_finePositioningMaxRetries = docktoConfig.FinePositioningMaxRetries;
|
||
|
||
// Reset Fine Positioning state
|
||
_finePositioningRetryCount = 0;
|
||
_finePositioningCycleCount = 0;
|
||
_finePositioningIsAligning = true;
|
||
_finePositionRotatePID = null;
|
||
_finePositioningDirection = RobotDirection.FORWARD;
|
||
_fpWasApproaching = false;
|
||
_fpOvershootCounter = 0;
|
||
|
||
if (_hasLoad)
|
||
{
|
||
// When loaded: no rotation allowed. Check heading error.
|
||
double headingError = heading - Localization.Theta;
|
||
if (headingError > Math.PI) headingError -= 2 * Math.PI;
|
||
else if (headingError < -Math.PI) headingError += 2 * Math.PI;
|
||
|
||
double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
|
||
if (Math.Abs(headingError) > thresholdRad)
|
||
{
|
||
Logger.LogError($"DockTo hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
|
||
NavState = NavigationState.Error;
|
||
Dispose();
|
||
return;
|
||
}
|
||
|
||
// Skip rotation, go directly to Docking
|
||
NavState = NavigationState.Docking;
|
||
HandleNavigationStart();
|
||
}
|
||
else
|
||
{
|
||
_isInitialRotation = true;
|
||
Rotate(heading);
|
||
}
|
||
}
|
||
|
||
public void Pause()
|
||
{
|
||
ResumeState = NavState;
|
||
_idleVelocityZeroSent = false;
|
||
NavState = NavigationState.Paused;
|
||
}
|
||
|
||
public void SafetyStop()
|
||
{
|
||
_safetyStopPreviousState = NavState;
|
||
_idleVelocityZeroSent = false;
|
||
NavState = NavigationState.SafetyStop;
|
||
}
|
||
|
||
public void Refresh()
|
||
{
|
||
_idleVelocityZeroSent = false;
|
||
if (NavState != NavigationState.SafetyStop)
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
return;
|
||
}
|
||
|
||
var prevState = _safetyStopPreviousState;
|
||
var x = Localization.X;
|
||
var y = Localization.Y;
|
||
|
||
if (prevState is NavigationState.Moving
|
||
&& MovePurePursuit?.Waypoints_Value is { Count: > 2 })
|
||
{
|
||
MovePurePursuit.ResetTracking();
|
||
MovePurePursuit.RebuildPath();
|
||
|
||
(_, int index) = MovePurePursuit.OnNode(x, y);
|
||
if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
|
||
{ NavState = NavigationState.Completed; Dispose(); return; }
|
||
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
ResetOvershootState();
|
||
|
||
// === Local Planner (same logic as Move) ===
|
||
var lpConfig = NavigationConfig.GetLocalPlannerConfig();
|
||
var ppConfig = NavigationConfig.GetPurepursuitConfig();
|
||
double heading;
|
||
|
||
var approachResult = lpConfig.Enabled
|
||
? MovePurePursuit.GenerateAndPrependApproachPath(
|
||
x, y, index, lpConfig)
|
||
: ApproachResult.Disabled;
|
||
|
||
if (approachResult == ApproachResult.ApproachGenerated)
|
||
{
|
||
var wp0 = MovePurePursuit.Waypoints_Value[0];
|
||
var wp1 = MovePurePursuit.Waypoints_Value[1];
|
||
heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
|
||
if (wp0.Direction == RobotDirection.BACKWARD) heading += Math.PI;
|
||
}
|
||
else if (approachResult == ApproachResult.AlreadyOnPath)
|
||
{
|
||
var wp = MovePurePursuit.Waypoints_Value[index];
|
||
var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
|
||
heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
|
||
if (wp.Direction == RobotDirection.BACKWARD) heading += Math.PI;
|
||
}
|
||
else
|
||
{
|
||
double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
|
||
var target = FindLookaheadTarget(index, lookahead)
|
||
?? MovePurePursuit.Waypoints_Value[^1];
|
||
heading = Math.Atan2(target.Y - y, target.X - x);
|
||
if (target.Direction == RobotDirection.BACKWARD) heading += Math.PI;
|
||
}
|
||
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
_isInitialRotation = true;
|
||
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
||
TargetAngle = heading;
|
||
NavState = NavigationState.Rotating;
|
||
}
|
||
else if (prevState is NavigationState.Docking
|
||
&& DockToController?.Waypoints_Value is { Count: > 2 })
|
||
{
|
||
DockToController.ResetTracking();
|
||
(_, int index) = DockToController.GetClosestAheadWaypoint(x, y);
|
||
if (index >= DockToController.Waypoints_Value.Count - 1)
|
||
{ NavState = NavigationState.Completed; Dispose(); return; }
|
||
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
ResetOvershootState();
|
||
|
||
var pathAngle = Math.Atan2(
|
||
DockToController.Goal.Y - DockToController.Waypoints_Value[0].Y,
|
||
DockToController.Goal.X - DockToController.Waypoints_Value[0].X);
|
||
double frontX = DockToController.Waypoints_Value[index].X
|
||
+ DockToController.DockConfig.WheelBase * Math.Cos(pathAngle);
|
||
double frontY = DockToController.Waypoints_Value[index].Y
|
||
+ DockToController.DockConfig.WheelBase * Math.Sin(pathAngle);
|
||
double heading = Math.Atan2(frontY - y, frontX - x);
|
||
if (DockToController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
if (_hasLoad)
|
||
{
|
||
// When loaded: check heading error, resume Docking directly without rotation
|
||
double headingError = heading - Localization.Theta;
|
||
if (headingError > Math.PI) headingError -= 2 * Math.PI;
|
||
else if (headingError < -Math.PI) headingError += 2 * Math.PI;
|
||
|
||
double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
|
||
if (Math.Abs(headingError) > thresholdRad)
|
||
{
|
||
Logger.LogError($"Refresh Docking hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
|
||
NavState = NavigationState.Error;
|
||
Dispose();
|
||
return;
|
||
}
|
||
NavState = NavigationState.Docking;
|
||
}
|
||
else
|
||
{
|
||
_isInitialRotation = true;
|
||
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
||
TargetAngle = heading;
|
||
NavState = NavigationState.Rotating;
|
||
}
|
||
}
|
||
else if (prevState is NavigationState.MovingStraight
|
||
&& MoveStraightController?.Waypoints_Value is { Count: > 2 })
|
||
{
|
||
MoveStraightController.ResetTracking();
|
||
(_, int index) = MoveStraightController.GetClosestAheadWaypoint(x, y);
|
||
if (index >= MoveStraightController.Waypoints_Value.Count - 1)
|
||
{ NavState = NavigationState.Completed; Dispose(); return; }
|
||
|
||
MovePID = new PID(NavigationConfig.GetMovePidConfig());
|
||
ResetOvershootState();
|
||
|
||
double heading = Math.Atan2(
|
||
MoveStraightController.Goal.Y - y,
|
||
MoveStraightController.Goal.X - x);
|
||
if (MoveStraightController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
|
||
heading += Math.PI;
|
||
heading = SpaceCompute.NormalizeRadianAngle(heading);
|
||
|
||
if (_hasLoad)
|
||
{
|
||
// When loaded: check heading error, resume MovingStraight directly without rotation
|
||
double headingError = heading - Localization.Theta;
|
||
if (headingError > Math.PI) headingError -= 2 * Math.PI;
|
||
else if (headingError < -Math.PI) headingError += 2 * Math.PI;
|
||
|
||
double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
|
||
if (Math.Abs(headingError) > thresholdRad)
|
||
{
|
||
Logger.LogError($"Refresh MovingStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
|
||
NavState = NavigationState.Error;
|
||
Dispose();
|
||
return;
|
||
}
|
||
NavState = NavigationState.MovingStraight;
|
||
}
|
||
else
|
||
{
|
||
_isInitialRotation = true;
|
||
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
||
TargetAngle = heading;
|
||
NavState = NavigationState.Rotating;
|
||
}
|
||
}
|
||
else if (prevState is NavigationState.Rotating)
|
||
{
|
||
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
||
NavState = NavigationState.Rotating;
|
||
}
|
||
else if (prevState is NavigationState.FinePositioning)
|
||
{
|
||
_finePositioningCycleCount = 0;
|
||
_finePositioningIsAligning = true;
|
||
_finePositionRotatePID = null;
|
||
NavState = NavigationState.Docking;
|
||
}
|
||
else
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
}
|
||
}
|
||
|
||
public void RefreshOrder(Node[] nodes, Edge[] edges)
|
||
{
|
||
}
|
||
|
||
public void Resume()
|
||
{
|
||
if (ResumeState == NavigationState.FinePositioning)
|
||
{
|
||
_finePositioningCycleCount = 0;
|
||
}
|
||
_idleVelocityZeroSent = false;
|
||
NavState = ResumeState;
|
||
}
|
||
|
||
public void Rotate(double angle)
|
||
{
|
||
if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
|
||
|
||
if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
|
||
{
|
||
NavState = NavigationState.Idle;
|
||
throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
|
||
}
|
||
|
||
RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
|
||
TargetAngle = SpaceCompute.NormalizeRadianAngle(angle);
|
||
NavState = NavigationState.Rotating;
|
||
HandleNavigationStart();
|
||
}
|
||
|
||
public void SetSpeed(double speed)
|
||
{
|
||
MaxLinearVelocity = _hasLoad ? Math.Min(speed, NavCog.LoadedMaxLinearVelocity) : speed;
|
||
}
|
||
|
||
protected void UpdateGoal(string goalId)
|
||
{
|
||
MovePurePursuit?.UpdateGoal(goalId);
|
||
}
|
||
|
||
public void UpdateOrder(string newBaseNodeId)
|
||
{
|
||
var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
|
||
if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
|
||
{
|
||
CurrentBaseNode = newBaseNode;
|
||
var newGoalRotate = FindNextRotateGoal();
|
||
if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
|
||
{
|
||
GoalRotate = newGoalRotate;
|
||
UpdateGoal(newGoalRotate.NodeId);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void Clear()
|
||
{
|
||
VelController.SetVelocity(0, 0);
|
||
CurrentBaseNode = null;
|
||
MovePurePursuit = null;
|
||
MovePID = null;
|
||
RotatePID = null;
|
||
GoalRotate = null;
|
||
DockToController = null;
|
||
MoveStraightController = null;
|
||
_dockSession = null;
|
||
ProcessedRotations = [];
|
||
ResetOvershootState();
|
||
_isInitialRotation = false;
|
||
_finePositioningRetryCount = 0;
|
||
_finePositioningCycleCount = 0;
|
||
_finePositioningIsAligning = true;
|
||
_finePositionRotatePID = null;
|
||
_finePositioningDirection = RobotDirection.FORWARD;
|
||
_fpWasApproaching = false;
|
||
_fpOvershootCounter = 0;
|
||
_hasLoad = false;
|
||
_idleVelocityZeroSent = false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Tìm target point tại lookahead distance từ vị trí hiện tại trên path.
|
||
/// Nội suy giữa các waypoint để tạo điểm mượt.
|
||
/// </summary>
|
||
private NavigationNode? FindLookaheadTarget(int startIndex, double lookaheadDistance)
|
||
{
|
||
if (MovePurePursuit is null || startIndex >= MovePurePursuit.Waypoints_Value.Count - 1)
|
||
return null;
|
||
|
||
double accumulatedDistance = 0;
|
||
|
||
for (int i = startIndex; i < MovePurePursuit.Waypoints_Value.Count - 1; i++)
|
||
{
|
||
double dx = MovePurePursuit.Waypoints_Value[i + 1].X - MovePurePursuit.Waypoints_Value[i].X;
|
||
double dy = MovePurePursuit.Waypoints_Value[i + 1].Y - MovePurePursuit.Waypoints_Value[i].Y;
|
||
double segmentLength = Math.Sqrt(dx * dx + dy * dy);
|
||
|
||
if (accumulatedDistance + segmentLength >= lookaheadDistance)
|
||
{
|
||
double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
|
||
return new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
NodeId = string.Empty,
|
||
X = MovePurePursuit.Waypoints_Value[i].X + t * dx,
|
||
Y = MovePurePursuit.Waypoints_Value[i].Y + t * dy,
|
||
Direction = MovePurePursuit.Waypoints_Value[i].Direction
|
||
};
|
||
}
|
||
|
||
accumulatedDistance += segmentLength;
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
|
||
/// </summary>
|
||
protected OrderNode? FindNextRotateGoal()
|
||
{
|
||
if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;
|
||
|
||
int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
|
||
if (goalIndex == -1) return null;
|
||
|
||
int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
|
||
lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;
|
||
|
||
// Tìm từ node hiện tại đến goal
|
||
for (int i = lastNodeIdx; i <= goalIndex; i++)
|
||
{
|
||
var node = MovePurePursuit.OrderNodes[i];
|
||
|
||
// Tìm node có IsWaitingRotate và chưa xử lý
|
||
if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
|
||
{
|
||
return node;
|
||
}
|
||
}
|
||
return CurrentBaseNode;
|
||
}
|
||
|
||
private NavigationNode GetDockToStartNode(double x, double y, double goalX, double goalY, double goalTheta, double length)
|
||
{
|
||
// Hướng vuông góc với theta
|
||
double perpAngle = SpaceCompute.NormalizeRadianAngle(goalTheta + Math.PI / 2);
|
||
|
||
// Hai endpoint của đoạn thẳng, cách goal ±length theo hướng vuông góc
|
||
double ep1X = goalX + length * Math.Cos(perpAngle);
|
||
double ep1Y = goalY + length * Math.Sin(perpAngle);
|
||
|
||
double ep2X = goalX - length * Math.Cos(perpAngle);
|
||
double ep2Y = goalY - length * Math.Sin(perpAngle);
|
||
|
||
// Chọn endpoint gần (x, y) nhất
|
||
double dist1Sq = (ep1X - x) * (ep1X - x) + (ep1Y - y) * (ep1Y - y);
|
||
double dist2Sq = (ep2X - x) * (ep2X - x) + (ep2Y - y) * (ep2Y - y);
|
||
|
||
double startX = dist1Sq <= dist2Sq ? ep1X : ep2X;
|
||
double startY = dist1Sq <= dist2Sq ? ep1Y : ep2Y;
|
||
|
||
return new NavigationNode
|
||
{
|
||
Id = Guid.NewGuid(),
|
||
X = startX,
|
||
Y = startY,
|
||
};
|
||
}
|
||
}
|