Initial commit
This commit is contained in:
@@ -0,0 +1,5 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation;
|
||||
|
||||
public class CPlusNavigation
|
||||
{
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using SysNum = System.Numerics;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Circular buffer để lưu lịch sử (fixed size)
|
||||
/// </summary>
|
||||
public class CircularBuffer<T>(int capacity) where T : SysNum.INumber<T>
|
||||
{
|
||||
private readonly T[] _buffer = new T[capacity];
|
||||
private int _head = 0;
|
||||
private int _count = 0;
|
||||
private readonly int _capacity = capacity;
|
||||
|
||||
public int Count => _count;
|
||||
public int Capacity => _capacity;
|
||||
|
||||
public void Add(T item)
|
||||
{
|
||||
_buffer[_head] = item;
|
||||
_head = (_head + 1) % _capacity;
|
||||
|
||||
if (_count < _capacity)
|
||||
_count++;
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
_head = 0;
|
||||
_count = 0;
|
||||
Array.Clear(_buffer, 0, _capacity);
|
||||
}
|
||||
|
||||
public T[] ToArray()
|
||||
{
|
||||
T[] result = new T[_count];
|
||||
for (int i = 0; i < _count; i++)
|
||||
{
|
||||
int index = (_head - _count + i + _capacity) % _capacity;
|
||||
result[i] = _buffer[index];
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public double Average()
|
||||
{
|
||||
if (_count == 0) return 0;
|
||||
|
||||
T sum = T.Zero;
|
||||
for (int i = 0; i < _count; i++)
|
||||
{
|
||||
int index = (_head - _count + i + _capacity) % _capacity;
|
||||
sum += _buffer[index];
|
||||
}
|
||||
return double.CreateChecked(sum) / _count;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,625 @@
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.Common.Models;
|
||||
using RobotNet10.RobotApp.Services.Simulation;
|
||||
using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
public class DockToConfig
|
||||
{
|
||||
#region Core Stanley Parameters
|
||||
|
||||
/// <summary>
|
||||
/// Cross-track error gain (K)
|
||||
/// Default: 2.5
|
||||
///
|
||||
/// Meaning: How aggressively to correct lateral position error
|
||||
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
|
||||
///
|
||||
/// ↑ Increase (3.0-5.0):
|
||||
/// ✓ Faster correction of cross-track error
|
||||
/// ✓ Tighter path following
|
||||
/// ✗ May cause oscillation
|
||||
/// ✗ Less smooth on noisy paths
|
||||
///
|
||||
/// ↓ Decrease (1.5-2.0):
|
||||
/// ✓ Smoother motion
|
||||
/// ✓ Less oscillation
|
||||
/// ✗ Slower error correction
|
||||
/// ✗ Larger cross-track error
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Start: 2.5 for general use
|
||||
/// - High precision: 3.0-4.0
|
||||
/// - Smooth priority: 1.5-2.0
|
||||
/// - Check stability by observing steering oscillation
|
||||
/// </summary>
|
||||
public double K { get; set; } = 2.5;
|
||||
|
||||
/// <summary>
|
||||
/// Softening constant (Ks) - meters/second
|
||||
/// Default: 0.1 m/s
|
||||
///
|
||||
/// Meaning: Added to velocity denominator to prevent division by zero at low speeds
|
||||
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
|
||||
///
|
||||
/// ↑ Increase (0.15-0.2):
|
||||
/// ✓ Less aggressive correction at low speed
|
||||
/// ✓ Smoother motion when starting
|
||||
/// ✗ Slower error correction at low speed
|
||||
///
|
||||
/// ↓ Decrease (0.05-0.08):
|
||||
/// ✓ More responsive at low speed
|
||||
/// ✗ May cause oscillation when slow
|
||||
/// ✗ Risk of instability near zero velocity
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Should be ~10% of typical operating velocity
|
||||
/// - If robot oscillates when slow: increase to 0.15-0.2
|
||||
/// - If too sluggish at startup: decrease to 0.05-0.08
|
||||
/// </summary>
|
||||
public double Ks { get; set; } = 0.1;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Vehicle Parameters
|
||||
|
||||
/// <summary>
|
||||
/// Wheelbase (L) - distance between front and rear axles (meters)
|
||||
/// Default: 0.5m
|
||||
///
|
||||
/// Meaning: Distance from rear axle (robot center) to virtual front axle
|
||||
/// Used to calculate front axle position and convert steering angle to angular velocity
|
||||
///
|
||||
/// IMPORTANT: Must match actual robot geometry
|
||||
///
|
||||
/// Formula: ω = (v × tan(δ)) / L
|
||||
/// </summary>
|
||||
public double WheelBase { get; set; } = 0.6;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum steering angle (radians)
|
||||
/// Default: 0.5 rad (≈28.6°)
|
||||
///
|
||||
/// Meaning: Physical limit of equivalent steering angle
|
||||
///
|
||||
/// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
|
||||
/// ✓ Sharper turns possible
|
||||
/// ✗ May exceed robot's turning capability
|
||||
///
|
||||
/// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
|
||||
/// ✓ Safer, gentler turns
|
||||
/// ✗ Cannot track sharp curves
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Test robot's max practical turn rate
|
||||
/// - Calculate: δ_max = arctan(L × ω_max / v_typical)
|
||||
/// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
|
||||
/// - Conservative: 0.4-0.5 rad
|
||||
/// </summary>
|
||||
public double MaxSteeringAngle { get; set; } = 0.5;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Goal Approach Parameters
|
||||
|
||||
/// <summary>
|
||||
/// Distance to start increasing K gain near goal (meters)
|
||||
/// Default: 1.0m
|
||||
///
|
||||
/// Meaning: When within this distance, K gain increases linearly
|
||||
/// to improve tracking accuracy during final approach
|
||||
///
|
||||
/// ↑ Increase (1.5-2.0m):
|
||||
/// ✓ Earlier tightening, smoother transition
|
||||
/// ✗ May be too aggressive on long approach
|
||||
///
|
||||
/// ↓ Decrease (0.5-0.8m):
|
||||
/// ✓ Only tighten very close to goal
|
||||
/// ✗ Less time to correct errors
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Should be larger than GoalTolerance × 10
|
||||
/// - Typical: 0.8-1.5m
|
||||
/// </summary>
|
||||
public double GoalApproachDistance { get; set; } = 1.0;
|
||||
|
||||
/// <summary>
|
||||
/// K gain multiplier at goal position
|
||||
/// Default: 2.0 (K doubles when at goal)
|
||||
///
|
||||
/// Meaning: At goal, effective K = K × GoalGainMultiplier
|
||||
/// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
|
||||
///
|
||||
/// ↑ Increase (2.5-3.0):
|
||||
/// ✓ Much tighter tracking near goal
|
||||
/// ✗ Risk of oscillation
|
||||
///
|
||||
/// ↓ Decrease (1.3-1.5):
|
||||
/// ✓ Gentler increase
|
||||
/// ✗ Less improvement near goal
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Start at 2.0
|
||||
/// - If oscillating near goal: decrease to 1.5
|
||||
/// - If still drifting: increase to 2.5
|
||||
/// </summary>
|
||||
public double GoalGainMultiplier { get; set; } = 2.0;
|
||||
|
||||
public double ReachedRadius { get; set; } = 0.03;
|
||||
#endregion
|
||||
|
||||
#region Angular Velocity Limit
|
||||
|
||||
/// <summary>
|
||||
/// Maximum angular velocity during dock-to approach (rad/s)
|
||||
/// Default: 0.8 rad/s
|
||||
///
|
||||
/// Meaning: Clamps the angular velocity output of Stanley controller
|
||||
/// to prevent excessive rotation during docking.
|
||||
///
|
||||
/// ↑ Increase (1.0-1.5):
|
||||
/// ✓ Faster heading correction
|
||||
/// ✗ May overshoot or oscillate during docking
|
||||
///
|
||||
/// ↓ Decrease (0.3-0.5):
|
||||
/// ✓ Smoother, more precise docking
|
||||
/// ✗ Slower heading correction
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Should be lower than PurePursuit/Stanley MaxAngularVelocity for precise docking
|
||||
/// - Start at 0.8, decrease if robot oscillates during dock
|
||||
/// </summary>
|
||||
public double MaxAngularVelocity { get; set; } = 0.05;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Path Resolution
|
||||
|
||||
/// <summary>
|
||||
/// Waypoint spacing for path sampling (meters)
|
||||
/// Default: 0.05m (5cm)
|
||||
///
|
||||
/// Meaning: Distance between interpolated path points
|
||||
/// Same as PurePursuit.ResolutionSplit for consistency
|
||||
/// </summary>
|
||||
public double ResolutionSplit { get; set; } = 0.05;
|
||||
|
||||
#endregion
|
||||
|
||||
public RobotDirection DockToDirection { get; set; } = RobotDirection.FORWARD;
|
||||
public double DockToLength { get; set; } = 3;
|
||||
|
||||
#region Fine Positioning
|
||||
|
||||
/// <summary>
|
||||
/// Timeout per FinePositioning attempt (milliseconds).
|
||||
/// Default: 6000ms (6s)
|
||||
/// </summary>
|
||||
public int FinePositioningTimeoutMs { get; set; } = 6000;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum retries for FinePositioning before transitioning to Error.
|
||||
/// Default: 3
|
||||
/// </summary>
|
||||
public int FinePositioningMaxRetries { get; set; } = 3;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Docking Overshoot & FinePositioning Tuning
|
||||
|
||||
/// <summary>
|
||||
/// Distance (meters) from goal at which overshoot detection begins during Docking.
|
||||
/// Default: 0.2m
|
||||
///
|
||||
/// Meaning: Similar to MovingOvershootDetectionRadius but for Docking phase.
|
||||
/// Smaller default because docking requires higher precision.
|
||||
///
|
||||
/// ↑ Increase (0.3-0.5):
|
||||
/// ✓ Earlier overshoot detection
|
||||
/// ✗ May false-trigger during normal approach deceleration
|
||||
///
|
||||
/// ↓ Decrease (0.1-0.15):
|
||||
/// ✓ Fewer false triggers
|
||||
/// ✗ Robot may overshoot further before detection
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Should be > ReachedRadius (default 0.03m)
|
||||
/// - If robot frequently enters FinePositioning unnecessarily: increase
|
||||
/// - If robot overshoots too far before FP kicks in: decrease
|
||||
/// </summary>
|
||||
public double DockingOvershootDetectionRadius { get; set; } = 0.2;
|
||||
|
||||
/// <summary>
|
||||
/// Distance (meters) from goal at which PID deceleration begins during Docking.
|
||||
/// Default: 3.0m
|
||||
///
|
||||
/// Meaning: When distance to goal > this value, robot runs at MaxLinearVelocity.
|
||||
/// Below this distance, PID ramps velocity down proportionally.
|
||||
///
|
||||
/// ↑ Increase (4-5):
|
||||
/// ✓ Earlier, smoother deceleration — better for heavy robots
|
||||
/// ✗ Slower docking approach
|
||||
///
|
||||
/// ↓ Decrease (1-2):
|
||||
/// ✓ Faster approach — stays at max speed longer
|
||||
/// ✗ Risk of overshoot on high-inertia robots
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Typically smaller than Moving's DecelerationDistance (docking is slower)
|
||||
/// - Should account for DockToMaxSpeed and robot braking capability
|
||||
/// </summary>
|
||||
public double DecelerationDistance { get; set; } = 3.0;
|
||||
|
||||
/// <summary>
|
||||
/// Heading alignment threshold (degrees) for FinePositioning Phase 1 (Align).
|
||||
/// Default: 0.5°
|
||||
///
|
||||
/// Meaning: Robot must align heading within this tolerance before transitioning
|
||||
/// from Align phase to Advance phase.
|
||||
///
|
||||
/// ↑ Increase (1-2°):
|
||||
/// ✓ Faster alignment — less time spent rotating
|
||||
/// ✗ Robot may drift more during advance due to initial heading error
|
||||
///
|
||||
/// ↓ Decrease (0.1-0.3°):
|
||||
/// ✓ More precise heading before advancing
|
||||
/// ✗ Longer alignment time, may oscillate on noisy heading sensor
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Must be < ReAlignThresholdDegrees
|
||||
/// - If robot drifts during advance: decrease
|
||||
/// - If alignment takes too long or oscillates: increase
|
||||
/// </summary>
|
||||
public double FineAlignThresholdDegrees { get; set; } = 0.5;
|
||||
|
||||
/// <summary>
|
||||
/// Heading drift threshold (degrees) to trigger re-alignment during FinePositioning Phase 2 (Advance).
|
||||
/// Default: 3.0°
|
||||
///
|
||||
/// Meaning: If heading error exceeds this during advance, robot stops and re-enters Align phase.
|
||||
///
|
||||
/// ↑ Increase (5-10°):
|
||||
/// ✓ Fewer re-alignment interruptions
|
||||
/// ✗ Robot may approach goal at a large angle, reducing accuracy
|
||||
///
|
||||
/// ↓ Decrease (1-2°):
|
||||
/// ✓ Tighter heading control during advance
|
||||
/// ✗ Frequent re-alignment, slower convergence
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Must be > FineAlignThresholdDegrees (to avoid immediate re-trigger)
|
||||
/// - Typical ratio: ReAlign ≈ 5-10x FineAlign
|
||||
/// - If robot keeps re-aligning: increase or tune AdvanceHeadingCorrectionGain
|
||||
/// </summary>
|
||||
public double ReAlignThresholdDegrees { get; set; } = 3.0;
|
||||
|
||||
/// <summary>
|
||||
/// P-gain for heading correction during FinePositioning Phase 2 (Advance).
|
||||
/// Default: 1.5
|
||||
///
|
||||
/// Meaning: Angular velocity correction = headingError × Gain × effectiveLinearVel
|
||||
/// Higher gain → stronger angular correction while advancing.
|
||||
///
|
||||
/// ↑ Increase (2.0-3.0):
|
||||
/// ✓ Faster heading correction during advance
|
||||
/// ✗ May cause oscillation or jerky steering
|
||||
///
|
||||
/// ↓ Decrease (0.5-1.0):
|
||||
/// ✓ Smoother advance motion
|
||||
/// ✗ Heading drift may accumulate, triggering re-alignment
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Works together with DockToAdvanceMaxAngularVelocity
|
||||
/// - If robot oscillates during advance: decrease
|
||||
/// - If robot drifts and re-aligns too often: increase
|
||||
/// </summary>
|
||||
public double AdvanceHeadingCorrectionGain { get; set; } = 1.5;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum angular velocity (rad/s) for heading correction during FinePositioning Phase 2 (Advance).
|
||||
/// Default: 0.15 rad/s
|
||||
///
|
||||
/// Meaning: Angular correction during advance is clamped to ±this value.
|
||||
/// Independent of NavigationConfig.MaxAngularVelocity (which is tuned for Moving phase).
|
||||
///
|
||||
/// ↑ Increase (0.2-0.3):
|
||||
/// ✓ Stronger heading correction during advance
|
||||
/// ✗ May cause path deviation or jerky steering
|
||||
///
|
||||
/// ↓ Decrease (0.05-0.1):
|
||||
/// ✓ Very smooth, nearly straight-line advance
|
||||
/// ✗ Cannot correct heading drift effectively, may trigger re-alignment
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - Should be small relative to DockToRetrySpeed to keep motion smooth
|
||||
/// - If advance path is too curved: decrease
|
||||
/// - If heading correction is too weak and triggers frequent re-align: increase
|
||||
/// </summary>
|
||||
public double DockToAdvanceMaxAngularVelocity { get; set; } = 0.15;
|
||||
|
||||
/// <summary>
|
||||
/// Number of consecutive distance-increasing cycles to trigger overshoot during FinePositioning Advance.
|
||||
/// Default: 3
|
||||
///
|
||||
/// Meaning: During advance, if distance to goal increases for this many consecutive cycles,
|
||||
/// overshoot is declared and a retry is initiated.
|
||||
///
|
||||
/// ↑ Increase (5-7):
|
||||
/// ✓ More tolerant of temporary distance fluctuations (sensor noise)
|
||||
/// ✗ Slower overshoot detection — robot travels further past goal
|
||||
///
|
||||
/// ↓ Decrease (1-2):
|
||||
/// ✓ Faster overshoot detection
|
||||
/// ✗ May false-trigger on sensor noise or minor jitter
|
||||
///
|
||||
/// Tuning Tips:
|
||||
/// - At 30ms cycle: 3 counts = 90ms detection delay
|
||||
/// - Noisy localization: increase to 5-7
|
||||
/// - Stable localization: 2-3 is sufficient
|
||||
/// </summary>
|
||||
public int FinePositioningOvershootCount { get; set; } = 3;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Continuous Goal Update
|
||||
|
||||
/// <summary>
|
||||
/// Maximum allowed position shift (meters) between consecutive goal updates.
|
||||
/// If new goal is farther than this from old goal, it is rejected.
|
||||
/// Default: 0.5m
|
||||
/// </summary>
|
||||
public double MaxGoalPositionShift { get; set; } = 0.2;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum allowed angle shift (degrees) between consecutive goal updates.
|
||||
/// If new goal orientation differs by more than this from old goal, it is rejected.
|
||||
/// Default: 15 degrees
|
||||
/// </summary>
|
||||
public double MaxGoalAngleShiftDegrees { get; set; } = 5.0;
|
||||
|
||||
#endregion
|
||||
|
||||
public DockToConfig Clone() => (DockToConfig)MemberwiseClone();
|
||||
}
|
||||
|
||||
public class DockToController(DockToConfig dockConfig)
|
||||
{
|
||||
public DockToConfig DockConfig { get; private set; } = dockConfig;
|
||||
public List<NavigationNode> Waypoints_Value = [];
|
||||
public NavigationNode Goal = null!;
|
||||
private int _currentWaypointAheadIndex = 0;
|
||||
public NavigationNode StartNode = null!;
|
||||
|
||||
public DockToController WithPath(NavigationNode startNode, NavigationNode currentGoal)
|
||||
{
|
||||
_currentWaypointAheadIndex = 0;
|
||||
StartNode = startNode;
|
||||
Goal = currentGoal;
|
||||
Waypoints_Value = [..PathSplit(StartNode, Goal)];
|
||||
return this;
|
||||
}
|
||||
|
||||
public void ResetTracking()
|
||||
{
|
||||
_currentWaypointAheadIndex = 0;
|
||||
}
|
||||
|
||||
public void UpdateGoal(NavigationNode currentGoal)
|
||||
{
|
||||
Goal = currentGoal;
|
||||
Waypoints_Value = [.. PathSplit(StartNode, Goal)];
|
||||
}
|
||||
|
||||
private NavigationNode[] PathSplit(NavigationNode startNode, NavigationNode currentGoal)
|
||||
{
|
||||
List<NavigationNode> navigationNode = [startNode];
|
||||
var spaceEdge = new SpaceEdge()
|
||||
{
|
||||
StartX = startNode.X,
|
||||
StartY = startNode.Y,
|
||||
EndX = currentGoal.X,
|
||||
EndY = currentGoal.Y,
|
||||
Degree = 1,
|
||||
};
|
||||
|
||||
double length = SpaceCompute.GetEdgeLength(spaceEdge, DockConfig.ResolutionSplit);
|
||||
if (length <= 0) return [];
|
||||
double step = DockConfig.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 = DockConfig.DockToDirection,
|
||||
Speed = startNode.Speed,
|
||||
});
|
||||
}
|
||||
navigationNode.Add(currentGoal);
|
||||
return [.. navigationNode];
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
|
||||
public (NavigationNode node, 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;
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// Stanley-based final approach controller
|
||||
/// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
|
||||
/// </summary>
|
||||
public (double linearVel, double angularVel) FinalApproachController(
|
||||
double robotX,
|
||||
double robotY,
|
||||
double robotTheta,
|
||||
double actualLinearVelocity,
|
||||
double maxLinearVelocity)
|
||||
{
|
||||
if (Goal is null || Waypoints_Value.Count == 0) return (0, 0);
|
||||
|
||||
if (DockConfig.DockToDirection == RobotDirection.BACKWARD) robotTheta += Math.PI;
|
||||
robotTheta = NormalizeAngle(robotTheta);
|
||||
|
||||
// Calculate front axle position
|
||||
double frontX = robotX + DockConfig.WheelBase * Math.Cos(robotTheta);
|
||||
double frontY = robotY + DockConfig.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 (DockConfig.DockToDirection == RobotDirection.BACKWARD) crossTrackError = -crossTrackError;
|
||||
|
||||
// Calculate heading error (path heading - robot heading)
|
||||
double headingError = NormalizeAngle(pathHeading - robotTheta);
|
||||
|
||||
// Adaptive K gain: increase when close to goal for tighter tracking
|
||||
double distanceToGoal = CalculateDistance(robotX, robotY, Goal.X, Goal.Y);
|
||||
double adaptiveK = DockConfig.K;
|
||||
if (distanceToGoal < DockConfig.GoalApproachDistance)
|
||||
{
|
||||
// Linearly increase K from K to K*GoalGainMultiplier as distance decreases
|
||||
double approachRatio = 1.0 - (distanceToGoal / DockConfig.GoalApproachDistance);
|
||||
adaptiveK = DockConfig.K * (1.0 + approachRatio * (DockConfig.GoalGainMultiplier - 1.0));
|
||||
}
|
||||
|
||||
// Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
|
||||
double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + DockConfig.Ks);
|
||||
|
||||
// Total steering angle
|
||||
double steeringAngle = headingError + crossTrackTerm;
|
||||
|
||||
// Clamp to maximum steering angle
|
||||
steeringAngle = Math.Clamp(steeringAngle, -DockConfig.MaxSteeringAngle, DockConfig.MaxSteeringAngle);
|
||||
|
||||
// Convert steering angle to angular velocity using bicycle model
|
||||
double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / DockConfig.WheelBase;
|
||||
|
||||
// Clamp angular velocity for dock-to approach
|
||||
angularVelocity = Math.Clamp(angularVelocity, -DockConfig.MaxAngularVelocity, DockConfig.MaxAngularVelocity);
|
||||
|
||||
// Apply direction to velocities
|
||||
double linearVel = maxLinearVelocity;
|
||||
if (DockConfig.DockToDirection == RobotDirection.BACKWARD)
|
||||
{
|
||||
linearVel = -linearVel;
|
||||
}
|
||||
|
||||
// Debug output
|
||||
Console.WriteLine($"DT-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}°, " +
|
||||
$"SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
|
||||
$"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
|
||||
$"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");
|
||||
|
||||
return (linearVel, angularVelocity);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,333 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
public class FuzzyLogic
|
||||
{
|
||||
private double Gain_P = 0.5;
|
||||
private double Gain_I = 0.01;
|
||||
private double piIntegratorState; // Trạng thái tích phân của PI controller
|
||||
|
||||
// Các tham số cho membership functions hình thang của tín hiệu góc
|
||||
// Negative Large: [-∞, -∞, -1.0, -0.5]
|
||||
private static readonly double[] NegativeLargeAngularParams = [-1.0E+10, -1.0E+10, -1.0, -0.5];
|
||||
// Positive Large: [0.5, 1.0, +∞, +∞]
|
||||
private static readonly double[] PositiveLargeAngularParams = [0.5, 1.0, 1.0E+10, 1.0E+10];
|
||||
|
||||
// Các tham số cho membership functions hình thang của vận tốc
|
||||
// High Velocity: [0.75, 1.0, +∞, +∞]
|
||||
private static readonly double[] HighVelocityParams = [0.75, 1.0, 1.0E+9, 1.0E+9];
|
||||
// Low Velocity: [-∞, -∞, 0.0, 0.25]
|
||||
private static readonly double[] LowVelocityParams = [-1.0E+9, -1.0E+9, 0.0, 0.25];
|
||||
|
||||
// Mảng quy tắc cho bộ điều khiển bánh phải (wr)
|
||||
// 25 phần tử đầu: chỉ số membership function cho tín hiệu góc (input 1)
|
||||
// 25 phần tử sau: chỉ số membership function cho vận tốc (input 2)
|
||||
private static readonly byte[] RightWheelRuleInput1Indices = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
|
||||
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
|
||||
4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
|
||||
// Mảng quy tắc output cho bánh phải (25 quy tắc)
|
||||
private static readonly byte[] RightWheelRuleOutputIndices = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
|
||||
|
||||
// Mảng quy tắc cho bộ điều khiển bánh trái (wl)
|
||||
private static readonly byte[] LeftWheelRuleInput1Indices = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
|
||||
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
|
||||
2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
|
||||
private static readonly byte[] LeftWheelRuleOutputIndices = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
|
||||
|
||||
public void SetGainP(double gainP)
|
||||
{
|
||||
Gain_P = gainP;
|
||||
}
|
||||
|
||||
public void SetGainI(double gainI)
|
||||
{
|
||||
Gain_I = gainI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính toán giá trị membership cho hàm hình thang (trapezoidal membership function).
|
||||
/// </summary>
|
||||
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
|
||||
/// <param name="parameters">Mảng 4 phần tử: [a, b, c, d] trong đó:
|
||||
/// - a: điểm bắt đầu của cạnh tăng (left foot)
|
||||
/// - b: điểm bắt đầu của phần phẳng (left shoulder)
|
||||
/// - c: điểm kết thúc của phần phẳng (right shoulder)
|
||||
/// - d: điểm kết thúc của cạnh giảm (right foot)</param>
|
||||
/// <returns>Giá trị membership trong khoảng [0, 1]</returns>
|
||||
private static double Fuzzy_trapmf(double inputValue, double[] parameters)
|
||||
{
|
||||
// Extract các tham số để dễ đọc
|
||||
double leftFoot = parameters[0]; // a: điểm bắt đầu tăng
|
||||
double leftShoulder = parameters[1]; // b: điểm bắt đầu phẳng
|
||||
double rightShoulder = parameters[2]; // c: điểm kết thúc phẳng
|
||||
double rightFoot = parameters[3]; // d: điểm kết thúc giảm
|
||||
|
||||
// Tính giá trị membership từ cạnh trái (từ a đến b)
|
||||
double leftMembership = 0.0;
|
||||
if (inputValue < leftFoot)
|
||||
{
|
||||
// Ngoài vùng hình thang bên trái
|
||||
leftMembership = 0.0;
|
||||
}
|
||||
else if (inputValue >= leftShoulder)
|
||||
{
|
||||
// Trong vùng phẳng bên trái
|
||||
leftMembership = 1.0;
|
||||
}
|
||||
else if (leftFoot != leftShoulder)
|
||||
{
|
||||
// Trên cạnh tăng (tính toán tuyến tính từ a đến b)
|
||||
leftMembership = (inputValue - leftFoot) / (leftShoulder - leftFoot);
|
||||
}
|
||||
|
||||
// Tính giá trị membership từ cạnh phải (từ c đến d)
|
||||
double rightMembership = 0.0;
|
||||
if (inputValue <= rightShoulder)
|
||||
{
|
||||
// Trong vùng phẳng bên phải
|
||||
rightMembership = 1.0;
|
||||
}
|
||||
else if (inputValue > rightFoot)
|
||||
{
|
||||
// Ngoài vùng hình thang bên phải
|
||||
rightMembership = 0.0;
|
||||
}
|
||||
else if (rightShoulder != rightFoot)
|
||||
{
|
||||
// Trên cạnh giảm (tính toán tuyến tính từ c đến d)
|
||||
rightMembership = (rightFoot - inputValue) / (rightFoot - rightShoulder);
|
||||
}
|
||||
|
||||
// Kết quả là giá trị nhỏ hơn để đảm bảo không vượt quá 1.0
|
||||
return leftMembership < rightMembership ? leftMembership : rightMembership;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính toán giá trị membership cho hàm tam giác (triangular membership function).
|
||||
/// </summary>
|
||||
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
|
||||
/// <param name="parameters">Mảng 3 phần tử: [a, b, c] trong đó:
|
||||
/// - a: điểm bắt đầu (left foot)
|
||||
/// - b: điểm đỉnh (peak) - giá trị membership = 1.0
|
||||
/// - c: điểm kết thúc (right foot)</param>
|
||||
/// <returns>Giá trị membership trong khoảng [0, 1]</returns>
|
||||
private static double Fuzzy_trimf(double inputValue, double[] parameters)
|
||||
{
|
||||
// Extract các tham số để dễ đọc
|
||||
double leftFoot = parameters[0]; // a: điểm bắt đầu
|
||||
double peak = parameters[1]; // b: điểm đỉnh
|
||||
double rightFoot = parameters[2]; // c: điểm kết thúc
|
||||
|
||||
// Kiểm tra nếu giá trị nằm ngoài vùng tam giác
|
||||
if (inputValue < leftFoot || inputValue > rightFoot)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Nếu giá trị tại đỉnh, membership = 1.0
|
||||
if (inputValue == peak)
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
// Tính toán membership trên cạnh tăng (từ a đến b)
|
||||
if (leftFoot < inputValue && inputValue < peak && leftFoot != peak)
|
||||
{
|
||||
return (inputValue - leftFoot) / (peak - leftFoot);
|
||||
}
|
||||
|
||||
// Tính toán membership trên cạnh giảm (từ b đến c)
|
||||
if (peak < inputValue && inputValue < rightFoot && peak != rightFoot)
|
||||
{
|
||||
return (rightFoot - inputValue) / (rightFoot - peak);
|
||||
}
|
||||
|
||||
// Trường hợp đặc biệt: nếu không khớp với điều kiện nào
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính toán vận tốc bánh trái và bánh phải dựa trên fuzzy logic controller.
|
||||
/// </summary>
|
||||
/// <param name="v">Vận tốc tuyến tính (linear velocity) - giá trị chuẩn hóa [0, 1]</param>
|
||||
/// <param name="w">Vận tốc góc (angular velocity)</param>
|
||||
/// <param name="timeSample">Thời gian mẫu (sampling time) cho tích phân - phải > 0</param>
|
||||
/// <returns>Tuple chứa (wl: vận tốc bánh trái, wr: vận tốc bánh phải) - giá trị chuẩn hóa [0, 1]</returns>
|
||||
/// <exception cref="ArgumentException">Thrown khi timeSample <= 0 hoặc các tham số không hợp lệ</exception>
|
||||
public (double wl, double wr) Fuzzy_step(double v, double w, double timeSample)
|
||||
{
|
||||
// Validation đầu vào
|
||||
if (timeSample <= 0.0 || double.IsNaN(timeSample) || double.IsInfinity(timeSample))
|
||||
{
|
||||
throw new ArgumentException("timeSample must be a positive finite number", nameof(timeSample));
|
||||
}
|
||||
if (double.IsNaN(v) || double.IsNaN(w) || double.IsInfinity(v) || double.IsInfinity(w))
|
||||
{
|
||||
throw new ArgumentException("Input parameters v and w must be finite numbers", nameof(v));
|
||||
}
|
||||
|
||||
(double wl, double wr) result = new();
|
||||
|
||||
// Cache cho các giá trị membership của đầu vào (10 membership functions)
|
||||
// [0-4]: membership functions cho tín hiệu góc đã xử lý
|
||||
// [5-9]: membership functions cho vận tốc tuyến tính
|
||||
double[] inputMembershipValues = new double[10];
|
||||
|
||||
// Cache cho các giá trị membership của đầu ra (5 levels: 0.0, 0.25, 0.5, 0.75, 1.0)
|
||||
double[] outputMembershipValuesRight = new double[5]; // Cho bánh phải (wr)
|
||||
double[] outputMembershipValuesLeft = new double[5]; // Cho bánh trái (wl)
|
||||
|
||||
// Mảng tạm để chứa tham số cho hàm tam giác (3 phần tử: [a, b, c])
|
||||
double[] triangularParams = new double[3];
|
||||
|
||||
// Các biến tạm để cache kết quả fuzzification (tránh tính toán lại)
|
||||
double negativeLargeMembership;
|
||||
double positiveLargeMembership;
|
||||
double highVelocityMembership;
|
||||
double lowVelocityMembership;
|
||||
// ========== BƯỚC 1: Xử lý tín hiệu đầu vào bằng PI Controller ==========
|
||||
// Tích phân vận tốc góc để loại bỏ sai số ổn định
|
||||
piIntegratorState += Gain_I * w * timeSample;
|
||||
// Kết hợp thành phần tỷ lệ và tích phân
|
||||
double piControllerOutput = Gain_P * w + piIntegratorState;
|
||||
|
||||
// ========== BƯỚC 2: Fuzzification - Chuyển đổi đầu vào thành độ thuộc ==========
|
||||
// Tính toán membership values cho tín hiệu góc đã xử lý (5 membership functions)
|
||||
|
||||
// MF1: Negative Large (hình thang)
|
||||
negativeLargeMembership = Fuzzy_trapmf(piControllerOutput, NegativeLargeAngularParams);
|
||||
inputMembershipValues[0] = negativeLargeMembership;
|
||||
|
||||
// MF2: Negative (tam giác: -0.5, 0.0, 0.5)
|
||||
triangularParams[0] = -0.5;
|
||||
triangularParams[1] = 0.0;
|
||||
triangularParams[2] = 0.5;
|
||||
inputMembershipValues[1] = Fuzzy_trimf(piControllerOutput, triangularParams);
|
||||
|
||||
// MF3: Positive Large (hình thang)
|
||||
positiveLargeMembership = Fuzzy_trapmf(piControllerOutput, PositiveLargeAngularParams);
|
||||
inputMembershipValues[2] = positiveLargeMembership;
|
||||
|
||||
// MF4: Very Negative (tam giác: -1.0, -0.5, 0.0)
|
||||
triangularParams[0] = -1.0;
|
||||
triangularParams[1] = -0.5;
|
||||
triangularParams[2] = 0.0;
|
||||
inputMembershipValues[3] = Fuzzy_trimf(piControllerOutput, triangularParams);
|
||||
|
||||
// MF5: Positive (tam giác: 0.0, 0.5, 1.0)
|
||||
triangularParams[0] = 0.0;
|
||||
triangularParams[1] = 0.5;
|
||||
triangularParams[2] = 1.0;
|
||||
inputMembershipValues[4] = Fuzzy_trimf(piControllerOutput, triangularParams);
|
||||
|
||||
// Tính toán membership values cho vận tốc tuyến tính (5 membership functions)
|
||||
|
||||
// MF6: Low (tam giác: 0.0, 0.25, 0.5)
|
||||
triangularParams[0] = 0.0;
|
||||
triangularParams[1] = 0.25;
|
||||
triangularParams[2] = 0.5;
|
||||
inputMembershipValues[5] = Fuzzy_trimf(v, triangularParams);
|
||||
|
||||
// MF7: Medium (tam giác: 0.25, 0.5, 0.75)
|
||||
triangularParams[0] = 0.25;
|
||||
triangularParams[1] = 0.5;
|
||||
triangularParams[2] = 0.75;
|
||||
inputMembershipValues[6] = Fuzzy_trimf(v, triangularParams);
|
||||
|
||||
// MF8: High (hình thang)
|
||||
highVelocityMembership = Fuzzy_trapmf(v, HighVelocityParams);
|
||||
inputMembershipValues[7] = highVelocityMembership;
|
||||
|
||||
// MF9: Very Low (hình thang)
|
||||
lowVelocityMembership = Fuzzy_trapmf(v, LowVelocityParams);
|
||||
inputMembershipValues[8] = lowVelocityMembership;
|
||||
|
||||
// MF10: High-Medium (tam giác: 0.5, 0.75, 1.0)
|
||||
triangularParams[0] = 0.5;
|
||||
triangularParams[1] = 0.75;
|
||||
triangularParams[2] = 1.0;
|
||||
inputMembershipValues[9] = Fuzzy_trimf(v, triangularParams);
|
||||
// ========== BƯỚC 3: Tính toán vận tốc bánh phải (wr) ==========
|
||||
// Khởi tạo giá trị membership cho đầu ra (5 mức: 0.0, 0.25, 0.5, 0.75, 1.0)
|
||||
outputMembershipValuesRight[0] = 0.0;
|
||||
outputMembershipValuesRight[1] = 0.25;
|
||||
outputMembershipValuesRight[2] = 0.5;
|
||||
outputMembershipValuesRight[3] = 0.75;
|
||||
outputMembershipValuesRight[4] = 1.0;
|
||||
|
||||
// Đánh giá 25 quy tắc fuzzy và tính toán defuzzification
|
||||
double totalRuleActivation = 0.0;
|
||||
double weightedOutputSum = 0.0;
|
||||
const int numberOfRules = 25;
|
||||
|
||||
for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
|
||||
{
|
||||
// Tính độ kích hoạt của quy tắc: product(input1_membership, input2_membership)
|
||||
// Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
|
||||
// input1: tín hiệu góc (index từ RightWheelRuleInput1Indices[ruleIndex] - 1, vì mảng bắt đầu từ 0)
|
||||
// input2: vận tốc (index từ RightWheelRuleInput1Indices[ruleIndex + 25] + 4, offset 4 vì vận tốc bắt đầu từ index 5)
|
||||
int angularSignalIndex = RightWheelRuleInput1Indices[ruleIndex] - 1;
|
||||
int velocityIndex = RightWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
|
||||
double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
|
||||
|
||||
totalRuleActivation += ruleActivation;
|
||||
|
||||
// Tính tổng có trọng số cho defuzzification (Center of Gravity)
|
||||
int outputIndex = RightWheelRuleOutputIndices[ruleIndex] - 1;
|
||||
weightedOutputSum += outputMembershipValuesRight[outputIndex] * ruleActivation;
|
||||
}
|
||||
|
||||
// Defuzzification: Center of Gravity method
|
||||
if (totalRuleActivation == 0.0)
|
||||
{
|
||||
// Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
|
||||
result.wr = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
result.wr = weightedOutputSum / totalRuleActivation;
|
||||
}
|
||||
// ========== BƯỚC 4: Tính toán vận tốc bánh trái (wl) ==========
|
||||
// Sử dụng lại các giá trị membership đã tính ở BƯỚC 2 (không cần tính lại vì không thay đổi)
|
||||
// Các giá trị trong inputMembershipValues[0-9] đã được tính toán và lưu trữ ở BƯỚC 2
|
||||
|
||||
// Khởi tạo giá trị membership cho đầu ra bánh trái
|
||||
outputMembershipValuesLeft[0] = 0.0;
|
||||
outputMembershipValuesLeft[1] = 0.25;
|
||||
outputMembershipValuesLeft[2] = 0.5;
|
||||
outputMembershipValuesLeft[3] = 0.75;
|
||||
outputMembershipValuesLeft[4] = 1.0;
|
||||
|
||||
// Đánh giá 25 quy tắc fuzzy cho bánh trái và tính toán defuzzification
|
||||
totalRuleActivation = 0.0;
|
||||
weightedOutputSum = 0.0;
|
||||
|
||||
for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
|
||||
{
|
||||
// Tính độ kích hoạt của quy tắc cho bánh trái: product(input1_membership, input2_membership)
|
||||
// Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
|
||||
// input1: tín hiệu góc (index từ LeftWheelRuleInput1Indices[ruleIndex] - 1)
|
||||
// input2: vận tốc (index từ LeftWheelRuleInput1Indices[ruleIndex + 25] + 4)
|
||||
int angularSignalIndex = LeftWheelRuleInput1Indices[ruleIndex] - 1;
|
||||
int velocityIndex = LeftWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
|
||||
double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
|
||||
|
||||
totalRuleActivation += ruleActivation;
|
||||
|
||||
// Tính tổng có trọng số cho defuzzification
|
||||
int outputIndex = LeftWheelRuleOutputIndices[ruleIndex] - 1;
|
||||
weightedOutputSum += outputMembershipValuesLeft[outputIndex] * ruleActivation;
|
||||
}
|
||||
|
||||
// Defuzzification: Center of Gravity method cho bánh trái
|
||||
if (totalRuleActivation == 0.0)
|
||||
{
|
||||
// Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
|
||||
result.wl = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
result.wl = weightedOutputSum / totalRuleActivation;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
public enum ApproachResult
|
||||
{
|
||||
/// <summary>
|
||||
/// Approach waypoints đã sinh và prepend thành công
|
||||
/// </summary>
|
||||
ApproachGenerated,
|
||||
|
||||
/// <summary>
|
||||
/// Robot đã trên path, không cần approach
|
||||
/// </summary>
|
||||
AlreadyOnPath,
|
||||
|
||||
/// <summary>
|
||||
/// Robot quá xa path, fallback Rotate cũ
|
||||
/// </summary>
|
||||
TooFarFromPath,
|
||||
|
||||
/// <summary>
|
||||
/// LocalPlanner tắt hoặc path quá ngắn
|
||||
/// </summary>
|
||||
Disabled
|
||||
}
|
||||
|
||||
public class LocalPlannerConfig
|
||||
{
|
||||
/// <summary>
|
||||
/// Bật/tắt Local Planner
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Khoảng cách dưới mức này coi robot đã trên path, không cần approach (m)
|
||||
/// </summary>
|
||||
public double OnPathThreshold { get; set; } = 0.3;
|
||||
|
||||
/// <summary>
|
||||
/// Khoảng cách trên mức này thì fallback Rotate cũ (m)
|
||||
/// </summary>
|
||||
public double MaxApproachDistance { get; set; } = 3.0;
|
||||
|
||||
/// <summary>
|
||||
/// Hệ số tính merge distance: mergeDistance = MergeDistanceGain × distToPath
|
||||
/// </summary>
|
||||
public double MergeDistanceGain { get; set; } = 2.0;
|
||||
|
||||
/// <summary>
|
||||
/// Merge distance tối thiểu (m)
|
||||
/// </summary>
|
||||
public double MergeDistanceMin { get; set; } = 0.3;
|
||||
|
||||
/// <summary>
|
||||
/// Merge distance tối đa (m)
|
||||
/// </summary>
|
||||
public double MergeDistanceMax { get; set; } = 2.0;
|
||||
|
||||
/// <summary>
|
||||
/// Tỷ lệ control arm phía robot (P1): d1 = ratio × dist(P0, P3)
|
||||
/// </summary>
|
||||
public double ControlArmRatioStart { get; set; } = 0.4;
|
||||
|
||||
/// <summary>
|
||||
/// Tỷ lệ control arm phía path (P2): d2 = ratio × dist(P0, P3)
|
||||
/// </summary>
|
||||
public double ControlArmRatioEnd { get; set; } = 0.4;
|
||||
|
||||
/// <summary>
|
||||
/// Bước sample approach curve (m), nên giống PurePursuitConfig.ResolutionSplit
|
||||
/// </summary>
|
||||
public double ResolutionSplit { get; set; } = 0.05;
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
public class MotorDynamicsConfig
|
||||
{
|
||||
/// <summary>
|
||||
/// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
|
||||
/// Đơn vị: giây (s)
|
||||
/// Typical: 0.1 - 0.5s cho DC motor với driver PID
|
||||
/// </summary>
|
||||
public double Tau { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
|
||||
/// Đơn vị: giây (s)
|
||||
/// Bao gồm: communication delay + driver processing
|
||||
/// Typical: 0.02 - 0.1s
|
||||
/// </summary>
|
||||
public double Delta { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Mô hình động học của motor driver
|
||||
/// First-order system: v(t) = v_cmd × (1 - e^(-(t-δ)/τ))
|
||||
/// </summary>
|
||||
public class MotorDynamicsModel
|
||||
{
|
||||
/// <summary>
|
||||
/// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
|
||||
/// Đơn vị: giây (s)3
|
||||
/// Typical: 0.1 - 0.5s cho DC motor với driver PID
|
||||
/// </summary>
|
||||
public double Tau { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
|
||||
/// Đơn vị: giây (s)
|
||||
/// Bao gồm: communication delay + driver processing
|
||||
/// Typical: 0.02 - 0.1s
|
||||
/// </summary>
|
||||
public double Delta { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Constructor với giá trị mặc định
|
||||
/// </summary>
|
||||
public MotorDynamicsModel()
|
||||
{
|
||||
Tau = 0.3; // 300ms time constant
|
||||
Delta = 0.05f; // 50ms delay
|
||||
}
|
||||
|
||||
public MotorDynamicsModel(MotorDynamicsConfig cog)
|
||||
{
|
||||
Tau = cog.Tau;
|
||||
Delta = cog.Delta;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Predict vận tốc tại thời điểm tương lai
|
||||
/// </summary>
|
||||
/// <param name="vCmd">Velocity command đã gửi</param>
|
||||
/// <param name="vActual">Velocity thực tế hiện tại</param>
|
||||
/// <param name="timeAhead">Thời gian dự đoán về tương lai (s)</param>
|
||||
/// <returns>Vận tốc dự đoán</returns>
|
||||
public double PredictVelocity(double vCmd, double vActual, double timeAhead)
|
||||
{
|
||||
// Nếu thời gian dự đoán < delay
|
||||
// → Motor chưa bắt đầu phản ứng
|
||||
if (timeAhead < Delta)
|
||||
{
|
||||
return vActual;
|
||||
}
|
||||
|
||||
// Thời gian hiệu dụng (sau khi trừ delay)
|
||||
double effectiveTime = timeAhead - Delta;
|
||||
|
||||
// First-order system response
|
||||
// response = 1 - e^(-t/τ)
|
||||
double response = 1.0 - Math.Exp(-effectiveTime / Tau);
|
||||
|
||||
// Velocity prediction
|
||||
// v_future = v_actual + (v_cmd - v_actual) × response
|
||||
double vPredicted = vActual + (vCmd - vActual) * response;
|
||||
|
||||
return vPredicted;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính settling time (thời gian để đạt 95% target)
|
||||
/// </summary>
|
||||
public double GetSettlingTime()
|
||||
{
|
||||
// 95% response: t = -τ × ln(0.05) ≈ 3τ
|
||||
return Delta + 3.0 * Tau;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính rise time (thời gian để đạt từ 10% đến 90%)
|
||||
/// </summary>
|
||||
public double GetRiseTime()
|
||||
{
|
||||
// Rise time ≈ 2.2τ
|
||||
return 2.2 * Tau;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return $"MotorModel(τ={Tau:F3}s, δ={Delta:F3}s, settling={GetSettlingTime():F3}s)";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
public class PIDConfig
|
||||
{
|
||||
public double Kp { get; set; }
|
||||
public double Ki { get; set; }
|
||||
public double Kd { get; set; }
|
||||
/// <summary>
|
||||
/// Integral chỉ tích lũy khi |error| <= IntegralZone.
|
||||
/// Giá trị 0 = không giới hạn (integral luôn tích lũy).
|
||||
/// </summary>
|
||||
public double IntegralZone { get; set; }
|
||||
}
|
||||
|
||||
public class PID(PIDConfig config)
|
||||
{
|
||||
private double Kp = config.Kp;
|
||||
private double Ki = config.Ki;
|
||||
private double Kd = config.Kd;
|
||||
private double IntegralZone = config.IntegralZone;
|
||||
|
||||
private double _prevError;
|
||||
private double _integral;
|
||||
|
||||
public PID WithKp(double kp)
|
||||
{
|
||||
Kp = kp;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKi(double ki)
|
||||
{
|
||||
Ki = ki;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKd(double kd)
|
||||
{
|
||||
Kd = kd;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithIntegralZone(double integralZone)
|
||||
{
|
||||
IntegralZone = integralZone;
|
||||
return this;
|
||||
}
|
||||
|
||||
public double PID_step(double error, double max, double min, double timeSample)
|
||||
{
|
||||
double integralStep = 0.5 * (error + _prevError) * timeSample;
|
||||
|
||||
// Integral Zone: chỉ tích lũy khi |error| nằm trong vùng cho phép
|
||||
bool inIntegralZone = IntegralZone <= 0 || Math.Abs(error) <= IntegralZone;
|
||||
if (inIntegralZone)
|
||||
_integral += integralStep;
|
||||
else
|
||||
_integral = 0;
|
||||
|
||||
double derivative = (error - _prevError) / timeSample;
|
||||
_prevError = error;
|
||||
|
||||
double Out = Kp * error
|
||||
+ Ki * _integral
|
||||
+ Kd * derivative;
|
||||
|
||||
// Anti-windup: hoàn tác integralStep khi output bị bão hòa
|
||||
double clamped = Math.Clamp(Out, min, max);
|
||||
if (clamped != Out && inIntegralZone)
|
||||
_integral -= integralStep;
|
||||
|
||||
return clamped;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_prevError = 0;
|
||||
_integral = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,929 @@
|
||||
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
|
||||
|
||||
}
|
||||
@@ -0,0 +1,457 @@
|
||||
using RobotNet10.CANOpen.CiA402.Enums;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.RobotApp.Services.ConfigManager;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Configuration cho signal processing
|
||||
/// </summary>
|
||||
public class VelocitySignalProcessingConfig
|
||||
{
|
||||
/// <summary>
|
||||
/// Hệ số lọc cho encoder velocity
|
||||
/// Giá trị nhỏ (0.1-0.2): Smooth nhưng lag
|
||||
/// Giá trị lớn (0.3-0.4): Responsive nhưng nhiễu
|
||||
/// </summary>
|
||||
public double AlphaFilter { get; set; } = 0.3;
|
||||
|
||||
/// <summary>
|
||||
/// Ngưỡng phát hiện encoder nhiễu (m/s)
|
||||
/// Nếu thay đổi vận tốc > threshold trong 1 cycle → có thể nhiễu
|
||||
/// </summary>
|
||||
public double NoiseThreshold { get; set; } = 0.5;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configuration cho velocity estimator
|
||||
/// </summary>
|
||||
public class VelocityEstimatorConfig
|
||||
{
|
||||
// Blend ratio limits
|
||||
public double MinBlendRatio { get; set; } = 0.15f;
|
||||
public double MaxBlendRatio { get; set; } = 0.8f;
|
||||
public double DefaultBlendRatio { get; set; } = 0.6;
|
||||
|
||||
// Adaptive blending thresholds
|
||||
public double GoodTrackingThreshold { get; set; } = 0.12f; // < 10% error
|
||||
public double ModerateTrackingThreshold { get; set; } = 0.3; // < 30% error
|
||||
|
||||
// Blend ratios for different tracking qualities
|
||||
public double GoodTrackingBlend { get; set; } = 0.7;
|
||||
public double ModerateTrackingBlend { get; set; } = 0.5;
|
||||
public double PoorTrackingBlend { get; set; } = 0.25f;
|
||||
|
||||
// Model confidence decay
|
||||
public double ConfidenceDecayRate { get; set; } = 0.95f;
|
||||
public double MinConfidence { get; set; } = 0.3;
|
||||
}
|
||||
|
||||
public class VelocityController(IInverseKinematics InverseKinematic,
|
||||
OdometryService odometryService,
|
||||
IRobotConfiguration RobtoConfiguration,
|
||||
INavigationConfig NavigationConfig,
|
||||
ILogger<VelocityController> _logger) : IVelocityController
|
||||
{
|
||||
public (double Linear, double Angular) ActualVelocity => GetCurrentVel();
|
||||
public (double Linear, double Angular) RawVelocity => GetRawCurrentVel();
|
||||
|
||||
// vận tốc tính toán m/s và rad/s đối với vận tốc góc
|
||||
private double _rightVelCmd = 0;
|
||||
private double _leftVelCmd = 0;
|
||||
private double _oldRightVel = 0;
|
||||
private double _oldLeftVel = 0;
|
||||
|
||||
private MotorDynamicsConfig _motorDynamicsConifg = new();
|
||||
private MotorDynamicsModel _motorDynamicsModel = new();
|
||||
private VelocityEstimatorConfig _estimatorConfig = new();
|
||||
public PurePursuitConfig _purePursuitConfig = new();
|
||||
private VelocitySignalProcessingConfig _signalConfig = new();
|
||||
private readonly CircularBuffer<double> _predictionErrors = new(20);
|
||||
|
||||
private double _currentConfidence = 1.0;
|
||||
private readonly double wheelBase = RobtoConfiguration.GetRobotPhysicalConfig().WheelBase;
|
||||
private int _ensureIKReadyCounter = 0; // Counter for logging throttling
|
||||
|
||||
public void SetVelocity(double linearVel, double angularVel)
|
||||
{
|
||||
// InverseKinematic.SetVelocity not available - commented out
|
||||
// InverseKinematic.SetVelocity(new()
|
||||
// {
|
||||
// Linear = new(){
|
||||
// X = linearVel,
|
||||
// Y = 0,
|
||||
// },
|
||||
// Angular = new(){
|
||||
// Z = angularVel,
|
||||
// },
|
||||
// });
|
||||
_leftVelCmd = linearVel - (wheelBase / 2) * angularVel;
|
||||
_rightVelCmd = linearVel + (wheelBase / 2) * angularVel;
|
||||
}
|
||||
public (double linearVel, double angularVel) GetRawCurrentVel()
|
||||
{
|
||||
try
|
||||
{
|
||||
var odom = odometryService.CurrentOdometry;
|
||||
double vActual = odom.Twist.Twist.Linear.X;
|
||||
double omegaActual = odom.Twist.Twist.Angular.Z;
|
||||
return (vActual, omegaActual);
|
||||
}
|
||||
catch { return (0, 0); }
|
||||
}
|
||||
|
||||
public (double linearVel, double angularVel) GetCurrentVel()
|
||||
{
|
||||
try
|
||||
{
|
||||
var odom = odometryService.CurrentOdometry;
|
||||
var vActual = odom.Twist.Twist.Linear.X;
|
||||
var omegaActual = odom.Twist.Twist.Angular.Z;
|
||||
|
||||
// Convert linear/angular back to left/right wheel velocities for the estimator
|
||||
_oldLeftVel = vActual - (wheelBase / 2) * omegaActual;
|
||||
_oldRightVel = vActual + (wheelBase / 2) * omegaActual;
|
||||
|
||||
return Estimate(_oldLeftVel, _oldRightVel, _leftVelCmd, _rightVelCmd, wheelBase);
|
||||
}
|
||||
catch { return (0, 0); }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exponential Moving Average (EMA) Low-Pass Filter
|
||||
/// </summary>
|
||||
/// <param name="newValue">Giá trị mới từ sensor</param>
|
||||
/// <param name="oldValue">Giá trị đã lọc trước đó</param>
|
||||
/// <param name="alpha">Hệ số lọc (0-1). Càng nhỏ càng smooth, càng lớn càng responsive</param>
|
||||
/// <returns>Giá trị sau khi lọc</returns>
|
||||
private static double LowPassFilter(double newValue, double oldValue, double alpha)
|
||||
{
|
||||
// Validate alpha
|
||||
if (alpha < 0 || alpha > 1)
|
||||
{
|
||||
throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
|
||||
}
|
||||
|
||||
return alpha * newValue + (1.0 - alpha) * oldValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MAIN FUNCTION: Estimate velocity
|
||||
/// </summary>
|
||||
private (double linearVel, double angularVel) Estimate(
|
||||
double vLeftActual, // Từ encoder (filtered)
|
||||
double vRightActual, // Từ encoder (filtered)
|
||||
double vLeftCmdPrev, // Command từ cycle trước
|
||||
double vRightCmdPrev, // Command từ cycle trước
|
||||
double wheelbase)
|
||||
{
|
||||
// 1. Tính vận tốc actual (linear & angular)
|
||||
double vActual = (vLeftActual + vRightActual) / 2.0;
|
||||
double omegaActual = (vRightActual - vLeftActual) / wheelbase;
|
||||
|
||||
// 2. Tính vận tốc command từ cycle trước
|
||||
double vCmdPrev = (vLeftCmdPrev + vRightCmdPrev) / 2.0;
|
||||
double omegaCmdPrev = (vRightCmdPrev - vLeftCmdPrev) / wheelbase;
|
||||
|
||||
// 3. Tính prediction horizon
|
||||
double predictionHorizon = CalculatePredictionHorizon(vActual);
|
||||
|
||||
// 4. Predict velocity cho từng bánh
|
||||
double vLeftPredicted = _motorDynamicsModel.PredictVelocity(
|
||||
vLeftCmdPrev,
|
||||
vLeftActual,
|
||||
predictionHorizon
|
||||
);
|
||||
|
||||
double vRightPredicted = _motorDynamicsModel.PredictVelocity(
|
||||
vRightCmdPrev,
|
||||
vRightActual,
|
||||
predictionHorizon
|
||||
);
|
||||
|
||||
// 5. Tính linear & angular predicted
|
||||
double vPredicted = (vLeftPredicted + vRightPredicted) / 2.0;
|
||||
double omegaPredicted = (vRightPredicted - vLeftPredicted) / wheelbase;
|
||||
|
||||
// 6. Update model confidence
|
||||
UpdateModelConfidence(vPredicted, vActual);
|
||||
|
||||
// 7. Tính tracking error
|
||||
double linearErr = CalculateLinearTrackingError(vCmdPrev, vActual);
|
||||
double angularErr = CalculateAngularTrackingError(omegaCmdPrev, omegaActual);
|
||||
double combinedTrackingError = 0.65f * linearErr + 0.35f * angularErr;
|
||||
|
||||
// 8. Calculate adaptive blend ratio
|
||||
double blendRatio = CalculateAdaptiveBlendRatio(
|
||||
combinedTrackingError,
|
||||
_currentConfidence
|
||||
);
|
||||
|
||||
// 9. Blend predicted và actual
|
||||
var vHybrid = (blendRatio * vPredicted) + ((1.0 - blendRatio) * vActual);
|
||||
double omegaHybrid = blendRatio * omegaPredicted + (1.0 - blendRatio) * omegaActual;
|
||||
|
||||
// 10. Return result
|
||||
return (vHybrid, omegaHybrid);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính prediction horizon dựa vào lookahead distance
|
||||
/// </summary>
|
||||
private double CalculatePredictionHorizon(double vActual)
|
||||
{
|
||||
// Lookahead distance
|
||||
double lookahead = _purePursuitConfig.LookaheadMin + _purePursuitConfig.Kdd * Math.Abs(vActual);
|
||||
|
||||
lookahead = Math.Clamp(lookahead, _purePursuitConfig.LookaheadMin, _purePursuitConfig.LookaheadMax);
|
||||
|
||||
// Prediction time = lookahead / velocity
|
||||
// Nếu vận tốc quá nhỏ, dùng một giá trị minimum
|
||||
double predictionTime = lookahead / Math.Max(Math.Abs(vActual), 0.1);
|
||||
|
||||
// Giới hạn prediction time (không nên quá xa)
|
||||
predictionTime = Math.Clamp(predictionTime, 0.1, 2.0);
|
||||
|
||||
return predictionTime;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính linear velocity tracking error (normalized)
|
||||
/// </summary>
|
||||
private static double CalculateLinearTrackingError(double vCmd, double vActual)
|
||||
{
|
||||
double error = Math.Abs(vCmd - vActual);
|
||||
double normalizedError = error / Math.Max(Math.Abs(vCmd), 0.1);
|
||||
return normalizedError;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tính angular velocity tracking error (normalized)
|
||||
/// </summary>
|
||||
private static double CalculateAngularTrackingError(double oCmd, double oActual)
|
||||
{
|
||||
double error = Math.Abs(oCmd - oActual);
|
||||
double normalizedError = error / Math.Max(Math.Abs(oCmd), 0.05f);
|
||||
return normalizedError;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update model confidence dựa trên prediction accuracy
|
||||
/// </summary>
|
||||
private void UpdateModelConfidence(double vPredictedPrev, double vActualNow)
|
||||
{
|
||||
// Prediction error từ cycle trước
|
||||
double predError = Math.Abs(vPredictedPrev - vActualNow) / Math.Max(Math.Abs(vActualNow), 0.1);
|
||||
|
||||
_predictionErrors.Add(predError);
|
||||
|
||||
// Tính confidence dựa trên average error
|
||||
if (_predictionErrors.Count > 0)
|
||||
{
|
||||
double avgError = _predictionErrors.Average();
|
||||
|
||||
// Confidence = 1 - avgError (capped)
|
||||
double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);
|
||||
|
||||
// Smooth update với decay
|
||||
_currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence + (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;
|
||||
|
||||
_currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate adaptive blend ratio
|
||||
/// </summary>
|
||||
private double CalculateAdaptiveBlendRatio(
|
||||
double trackingError,
|
||||
double modelConfidence)
|
||||
{
|
||||
double alpha;
|
||||
|
||||
// Factor 1: Tracking error
|
||||
if (trackingError < _estimatorConfig.GoodTrackingThreshold)
|
||||
{
|
||||
// Motor tracking tốt → tin prediction nhiều
|
||||
alpha = _estimatorConfig.GoodTrackingBlend;
|
||||
}
|
||||
else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
|
||||
{
|
||||
// Moderate error → balanced
|
||||
alpha = _estimatorConfig.ModerateTrackingBlend;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Poor tracking (slip/overload) → tin actual nhiều
|
||||
alpha = _estimatorConfig.PoorTrackingBlend;
|
||||
}
|
||||
|
||||
// Factor 2: Model confidence
|
||||
// Nếu model không chính xác, giảm blend ratio
|
||||
alpha *= modelConfidence;
|
||||
|
||||
// Clamp trong khoảng cho phép
|
||||
alpha = Math.Clamp(alpha, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);
|
||||
|
||||
return alpha;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reset estimator state
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_predictionErrors.Clear();
|
||||
_currentConfidence = 1.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get current model confidence
|
||||
/// </summary>
|
||||
public double GetModelConfidence()
|
||||
{
|
||||
return _currentConfidence;
|
||||
}
|
||||
|
||||
public void LoadConfig()
|
||||
{
|
||||
_motorDynamicsConifg = NavigationConfig.GetMotorDynamicsConfig();
|
||||
_motorDynamicsModel = new(_motorDynamicsConifg);
|
||||
_estimatorConfig = NavigationConfig.GetVelocityEstimatorConfig();
|
||||
_purePursuitConfig = NavigationConfig.GetPurepursuitConfig();
|
||||
_signalConfig = NavigationConfig.GetVelocitySignalProcessingConfig();
|
||||
}
|
||||
|
||||
public void SetAcceleration(double acc)
|
||||
{
|
||||
// SetAcceleration not available - commented out
|
||||
// InverseKinematic.SetAcceleration(acc);
|
||||
}
|
||||
|
||||
public void SetDeceleration(double dec)
|
||||
{
|
||||
// SetDeceleration not available - commented out
|
||||
// InverseKinematic.SetDeceleration(dec);
|
||||
}
|
||||
|
||||
public bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Increment counter for logging throttling
|
||||
_ensureIKReadyCounter++;
|
||||
|
||||
// Check if need to reset fault first
|
||||
// Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
|
||||
if (!InverseKinematic.IsOperationEnabled)
|
||||
{
|
||||
// Try fault reset first (in case it's in fault state)
|
||||
InverseKinematic.FaultReset();
|
||||
PreciseDelay(200, cancellationToken);
|
||||
}
|
||||
|
||||
// Check if IInverseKinematics is in OperationEnabled state
|
||||
if (!InverseKinematic.IsOperationEnabled)
|
||||
{
|
||||
// Auto-enable IInverseKinematics through state transitions
|
||||
// Enable() is a convenience method that automatically transitions through all states
|
||||
int maxAttempts = 3;
|
||||
int attemptDelay = 300; // ms
|
||||
|
||||
for (int i = 0; i < maxAttempts && !InverseKinematic.IsOperationEnabled; i++)
|
||||
{
|
||||
InverseKinematic.Enable();
|
||||
PreciseDelay(attemptDelay, cancellationToken);
|
||||
}
|
||||
|
||||
// Check if enabled successfully
|
||||
if (!InverseKinematic.IsOperationEnabled)
|
||||
{
|
||||
// Log only once every 10 times to avoid spam
|
||||
if (_ensureIKReadyCounter % 10 == 0)
|
||||
{
|
||||
_logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Check and set operation mode to ProfileVelocity (synchronous)
|
||||
try
|
||||
{
|
||||
// GetOperationMode/SetOperationMode not available - commented out
|
||||
// OperationMode currentMode = InverseKinematic.GetOperationMode();
|
||||
// if (currentMode != OperationMode.ProfileVelocity)
|
||||
// {
|
||||
// InverseKinematic.SetOperationMode(OperationMode.ProfileVelocity);
|
||||
// }
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Log only once every 10 times to avoid spam
|
||||
if (_ensureIKReadyCounter % 10 == 0)
|
||||
{
|
||||
_logger.LogWarning(ex, "Error checking/setting operation mode");
|
||||
}
|
||||
// Continue anyway to prevent blocking
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Log only once every 10 times to avoid spam
|
||||
if (_ensureIKReadyCounter % 10 == 0)
|
||||
{
|
||||
_logger.LogError(ex, "Error ensuring IInverseKinematics ready");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
|
||||
/// This ensures accurate timing for the update loop without async overhead
|
||||
/// </summary>
|
||||
private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
|
||||
{
|
||||
if (milliseconds <= 0)
|
||||
return;
|
||||
|
||||
if (milliseconds > 1)
|
||||
{
|
||||
// Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
|
||||
// Check cancellation periodically during sleep
|
||||
var sleepStart = DateTime.UtcNow;
|
||||
while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return;
|
||||
|
||||
var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
|
||||
if (remaining > 0)
|
||||
{
|
||||
Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Use SpinWait for very short delays to maintain precise timing
|
||||
var spinWait = new SpinWait();
|
||||
for (int i = 0; i < 10; i++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
break;
|
||||
spinWait.SpinOnce();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,874 @@
|
||||
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,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
namespace RobotNet10.RobotApp.Services.Navigation;
|
||||
|
||||
public interface IVelocityController
|
||||
{
|
||||
(double Linear, double Angular) ActualVelocity { get; }
|
||||
(double Linear, double Angular) RawVelocity { get; }
|
||||
void SetVelocity(double linearVel, double angularVel);
|
||||
void LoadConfig();
|
||||
double GetModelConfidence();
|
||||
void SetAcceleration(double acc);
|
||||
void SetDeceleration(double dec);
|
||||
bool EnsureInverseKinematicsReady(CancellationToken cancellationToken);
|
||||
}
|
||||
Reference in New Issue
Block a user