656 lines
28 KiB
C#
656 lines
28 KiB
C#
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.RobotManager.Services.OpenACS;
|
|
using RobotNet.RobotManager.Services.Planner.Space;
|
|
using RobotNet.RobotManager.Services.Simulation.Algorithm;
|
|
using RobotNet.RobotManager.Services.Simulation.Models;
|
|
using RobotNet.RobotManager.Services.Traffic;
|
|
using RobotNet.RobotShares.Dtos;
|
|
using RobotNet.RobotShares.Enums;
|
|
using RobotNet.Shares;
|
|
|
|
namespace RobotNet.RobotManager.Services.Simulation;
|
|
|
|
public abstract class NavigationService : INavigationService, IDisposable
|
|
{
|
|
protected readonly VisualizationService Visualization;
|
|
protected readonly VelocityController VelocityController;
|
|
protected readonly RobotSimulationModel RobotModel;
|
|
protected readonly TrafficManager TrafficManager;
|
|
protected readonly TrafficACS TrafficACS;
|
|
protected readonly PathPlanner PathPlanner;
|
|
protected readonly MapManager MapManager;
|
|
protected readonly LoggerController<NavigationService> Logger;
|
|
|
|
private WatchTimer<NavigationService>? navTimer;
|
|
protected const int SampleTimeMilliseconds = 50;
|
|
|
|
private WatchTimerAsync<NavigationService>? checkingTimer;
|
|
private const int CheckingTimeMilliseconds = 1000;
|
|
|
|
protected double TargetAngle = 0;
|
|
protected PID? RotatePID;
|
|
protected readonly double AngularVelocity;
|
|
|
|
protected PID? MovePID;
|
|
protected FuzzyLogic? MoveFuzzy;
|
|
protected PurePursuit? MovePurePursuit;
|
|
|
|
private Guid TrafficManagerGoalId = Guid.Empty;
|
|
private Guid TrafficACSGoalId = Guid.Empty;
|
|
protected NavigationNode? NavigationGoal;
|
|
protected NavigationNode? FinalGoal;
|
|
protected List<NavigationNode>? NavigationPath;
|
|
protected List<NavigationNode> CheckingNodes = [];
|
|
private List<NavigationNode> RefreshPath = [];
|
|
private List<NavigationNode> BaseNodes = [];
|
|
private bool IsWaittingStop;
|
|
private double RotateAngle;
|
|
protected Dictionary<Guid, ZoneDto[]> Zones = [];
|
|
|
|
protected NavigationAction Action = NavigationAction.Stop;
|
|
|
|
public NavigationNode? InNode { get; set; } = null;
|
|
public NavigationStateType NavigationState { get; set; } = NavigationStateType.Ready;
|
|
public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None;
|
|
public bool IsError { get; private set; }
|
|
public bool IsCompleted { get; private set; } = true;
|
|
public bool IsProcessing { get; private set; }
|
|
public bool IsCanceled { get; private set; }
|
|
public string[] Errors => [];
|
|
public NavigationPathEdge[] FullPath => GetFullPath();
|
|
public NavigationPathEdge[] BasePath => GetBasePath();
|
|
public Guid MapId { get; set; } = Guid.Empty;
|
|
public List<ZoneDto> CurrentZones { get; set; } = [];
|
|
|
|
public NavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider)
|
|
{
|
|
Visualization = visualization;
|
|
VelocityController = new VelocityController(Visualization).WithDeceleration(model.Deceleration).WithAcceleration(model.Acceleration).WithSampleTime(SampleTimeMilliseconds / 1000.0);
|
|
RobotModel = model;
|
|
AngularVelocity = MathExtension.CheckLimit(RobotModel.MaxAngularVelocity * RobotModel.Width / RobotModel.RadiusWheel, 2 * Math.PI, 0);
|
|
TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
|
|
TrafficACS = ServiceProvider.GetRequiredService<TrafficACS>();
|
|
PathPlanner = ServiceProvider.GetRequiredService<PathPlanner>();
|
|
MapManager = ServiceProvider.GetRequiredService<MapManager>();
|
|
Logger = ServiceProvider.GetRequiredService<LoggerController<NavigationService>>();
|
|
}
|
|
|
|
public virtual MessageResult Rotate(double angle)
|
|
{
|
|
RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
|
|
TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
|
|
Action = NavigationAction.Rotate;
|
|
NavStart();
|
|
CheckingStart();
|
|
return new(true);
|
|
}
|
|
|
|
public virtual MessageResult MoveStraight(NavigationNode[] path)
|
|
{
|
|
if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
|
|
NavigationPath = [.. path];
|
|
CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
|
|
InNode = CheckingNodes[0];
|
|
|
|
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
|
MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
|
|
MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
|
|
double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
|
|
Rotate(Angle * 180 / Math.PI);
|
|
return new(true);
|
|
}
|
|
|
|
public virtual MessageResult Move(NavigationNode[] path)
|
|
{
|
|
if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
|
|
NavigationPath = [.. path];
|
|
CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
|
|
if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
|
|
InNode = CheckingNodes[0];
|
|
|
|
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
|
MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
|
|
MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
|
|
double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
|
|
Rotate(Angle * 180 / Math.PI);
|
|
return new(true);
|
|
}
|
|
|
|
public virtual MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
|
|
var pathNodes = PathPlanner.CalculatorDirection(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
|
|
var pathSplit = PathPlanner.PathSplit(pathNodes, edges);
|
|
if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
|
|
if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");
|
|
|
|
var getZones = PathPlanner.GetZones(MapId, nodes);
|
|
getZones.Wait();
|
|
Zones = getZones.Result.Data ?? [];
|
|
|
|
NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Theta = n.Theta,
|
|
Direction = MapCompute.GetRobotDirection(n.Direction),
|
|
Actions = n.Actions,
|
|
})];
|
|
CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
|
|
InNode = CheckingNodes[0];
|
|
FinalGoal = CheckingNodes[^1];
|
|
|
|
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
|
MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
|
|
MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
|
|
|
|
double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
|
|
double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
|
|
Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
|
|
return new(true);
|
|
}
|
|
|
|
public MessageResult Cancel()
|
|
{
|
|
Dispose();
|
|
VelocityController.Stop();
|
|
NavigationState = NavigationStateType.Ready;
|
|
IsCanceled = true;
|
|
return new(true);
|
|
}
|
|
|
|
protected virtual void NavigationHandler()
|
|
{
|
|
}
|
|
|
|
private void HandleRefreshPath(TrafficSolution trafficSolution)
|
|
{
|
|
var edgesDto = trafficSolution.Edges.Select(e => new EdgeDto
|
|
{
|
|
Id = e.Id,
|
|
StartNodeId = e.StartNodeId,
|
|
EndNodeId = e.EndNodeId,
|
|
TrajectoryDegree = e.TrajectoryDegree,
|
|
ControlPoint1X = e.ControlPoint1X,
|
|
ControlPoint1Y = e.ControlPoint1Y,
|
|
ControlPoint2X = e.ControlPoint2X,
|
|
ControlPoint2Y = e.ControlPoint2Y,
|
|
});
|
|
List<NodeDto> nodesDto = [..trafficSolution.Nodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Name = n.Name,
|
|
Direction = MapCompute.GetNodeDirection(n.Direction),
|
|
})];
|
|
var splitPath = PathPlanner.PathSplit([.. nodesDto], [.. edgesDto]);
|
|
|
|
if (splitPath.IsSuccess)
|
|
{
|
|
if (splitPath.Data != null)
|
|
{
|
|
RefreshPath = [..splitPath.Data.Select(n => new NavigationNode()
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Theta = n.Theta,
|
|
Direction = MapCompute.GetRobotDirection(n.Direction),
|
|
Actions = n.Actions,
|
|
})];
|
|
}
|
|
}
|
|
}
|
|
|
|
private void HandleCompleteState(TrafficSolution trafficSolution)
|
|
{
|
|
if (trafficSolution.State == TrafficSolutionState.Complete && RefreshPath.Count > 0)
|
|
{
|
|
NavigationPath = RefreshPath;
|
|
CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
|
|
if (MovePurePursuit is not null)
|
|
{
|
|
MovePurePursuit.UpdatePath([.. NavigationPath]);
|
|
MovePurePursuit.OnNodeIndex = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y).index;
|
|
}
|
|
RefreshPath = [];
|
|
|
|
double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
|
|
double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
|
|
TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
|
|
Action = NavigationAction.Rotate;
|
|
}
|
|
}
|
|
|
|
private void HandleGivewayState(TrafficSolution trafficSolution)
|
|
{
|
|
if (trafficSolution.GivewayNodes[^1].Id == trafficSolution.ReleaseNode.Id && NavigationGoal?.Id != trafficSolution.ReleaseNode.Id)
|
|
{
|
|
var splitPath = PathPlanner.PathSplit([..trafficSolution.GivewayNodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Name = n.Name,
|
|
Direction = MapCompute.GetNodeDirection(n.Direction),
|
|
})], [.. trafficSolution.GivewayEdges.Select(e => new EdgeDto
|
|
{
|
|
Id = e.Id,
|
|
StartNodeId = e.StartNodeId,
|
|
EndNodeId = e.EndNodeId,
|
|
TrajectoryDegree = e.TrajectoryDegree,
|
|
ControlPoint1X = e.ControlPoint1X,
|
|
ControlPoint1Y = e.ControlPoint1Y,
|
|
ControlPoint2X = e.ControlPoint2X,
|
|
ControlPoint2Y = e.ControlPoint2Y,
|
|
})]);
|
|
if (splitPath.IsSuccess && splitPath.Data != null && MovePurePursuit != null)
|
|
{
|
|
NavigationPath = [..splitPath.Data.Select(n => new NavigationNode()
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Theta = n.Theta,
|
|
Direction = MapCompute.GetRobotDirection(n.Direction),
|
|
Actions = n.Actions,
|
|
})];
|
|
CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
|
|
MovePurePursuit.UpdatePath([.. NavigationPath]);
|
|
MovePurePursuit.OnNodeIndex = 0;
|
|
|
|
double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
|
|
double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
|
|
TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
|
|
Action = NavigationAction.Rotate;
|
|
}
|
|
}
|
|
}
|
|
|
|
private async Task<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> newZones)
|
|
{
|
|
Guid trafficACSrelaseNodeId = Guid.Empty;
|
|
foreach (var zone in newZones)
|
|
{
|
|
if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key;
|
|
else
|
|
{
|
|
bool requestSuccess = true;
|
|
foreach (var zoneACS in zone.Value)
|
|
{
|
|
if (string.IsNullOrEmpty(zoneACS.Name)) continue;
|
|
if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue;
|
|
|
|
var getInTrafficACS = await TrafficACS.RequestIn(RobotModel.RobotId, zoneACS.Name);
|
|
if (getInTrafficACS.IsSuccess && getInTrafficACS.Data) CurrentZones.Add(zoneACS);
|
|
else
|
|
{
|
|
requestSuccess = false;
|
|
break;
|
|
}
|
|
}
|
|
if (requestSuccess) trafficACSrelaseNodeId = zone.Key;
|
|
else break;
|
|
}
|
|
}
|
|
return trafficACSrelaseNodeId;
|
|
}
|
|
|
|
private void UpdateTraffic()
|
|
{
|
|
var trafficManagerGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficManagerGoalId);
|
|
var trafficACSGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficACSGoalId);
|
|
if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1)
|
|
{
|
|
var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex);
|
|
NavigationNode goal = CheckingNodes[goalIndex];
|
|
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == (InNode is null ? Guid.Empty : InNode.Id));
|
|
inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
|
|
if (BaseNodes.Count == 0 || BaseNodes[^1].Id != goal.Id) BaseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
|
|
|
|
if (IsWaittingStop)
|
|
{
|
|
if (NavigationState == NavigationStateType.Waitting)
|
|
{
|
|
TargetAngle = MathExtension.CheckLimit(RotateAngle, 180, -180);
|
|
Action = NavigationAction.Rotate;
|
|
IsWaittingStop = false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int i = inNodeIndex + 1; i <= goalIndex; i++)
|
|
{
|
|
if (Math.Abs(CheckingNodes[i].Theta - CheckingNodes[i - 1].Theta) > 30)
|
|
{
|
|
goal = CheckingNodes[i];
|
|
IsWaittingStop = true;
|
|
RotateAngle = CheckingNodes[i].Theta;
|
|
break;
|
|
}
|
|
}
|
|
UpdateGoal(goal);
|
|
}
|
|
}
|
|
}
|
|
|
|
private async Task GoOutTrafficACS()
|
|
{
|
|
if (BaseNodes is null || BaseNodes.Count == 0 || InNode is null) return;
|
|
var goalIndex = CheckingNodes.FindIndex(n => n.Id == BaseNodes[^1].Id);
|
|
if (goalIndex == -1) return;
|
|
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
|
|
inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
|
|
if (goalIndex <= inNodeIndex) return;
|
|
var baseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
|
|
var baseZones = PathPlanner.GetZones([..baseNodes.Select(n => n.ToNodeDto())], Zones);
|
|
var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList();
|
|
foreach (var zoneACS in outZones)
|
|
{
|
|
if (string.IsNullOrEmpty(zoneACS.Name)) continue;
|
|
var outTrafficACS = await TrafficACS.RequestOut(RobotModel.RobotId, zoneACS.Name);
|
|
if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id);
|
|
}
|
|
}
|
|
|
|
protected virtual async Task CheckingHandler()
|
|
{
|
|
try
|
|
{
|
|
if (NavigationPath is null || InNode is null) throw new Exception("Đường đi không tồn tại");
|
|
|
|
await GoOutTrafficACS();
|
|
|
|
if (!TrafficManager.Enable)
|
|
{
|
|
if (TrafficManagerGoalId != CheckingNodes[^1].Id) TrafficManagerGoalId = CheckingNodes[^1].Id;
|
|
}
|
|
else
|
|
{
|
|
TrafficManager.UpdateInNode(RobotModel.RobotId, InNode.Id);
|
|
var trafficSolution = TrafficManager.GetTrafficNode(RobotModel.RobotId);
|
|
if (trafficSolution.State != TrafficSolutionState.RefreshPath && TrafficSolutionState == TrafficSolutionState.RefreshPath && trafficSolution.Edges.Length > 0 && trafficSolution.Nodes.Length > 1)
|
|
{
|
|
HandleRefreshPath(trafficSolution);
|
|
}
|
|
if (trafficSolution.State == TrafficSolutionState.Complete || trafficSolution.State == TrafficSolutionState.Waitting)
|
|
{
|
|
HandleCompleteState(trafficSolution);
|
|
}
|
|
else if (trafficSolution.State == TrafficSolutionState.GiveWay && trafficSolution.GivewayNodes.Count > 1 && trafficSolution.GivewayEdges.Count > 0)
|
|
{
|
|
HandleGivewayState(trafficSolution);
|
|
}
|
|
var goal = CheckingNodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id);
|
|
if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id;
|
|
TrafficSolutionState = trafficSolution.State;
|
|
}
|
|
|
|
if (!TrafficACS.Enable)
|
|
{
|
|
if (TrafficACSGoalId != CheckingNodes[^1].Id) TrafficACSGoalId = CheckingNodes[^1].Id;
|
|
}
|
|
else
|
|
{
|
|
var newZones = TrafficACS.GetZones(InNode?.Id ?? Guid.Empty, [.. CheckingNodes.Select(n => n.ToNodeDto())], Zones);
|
|
var trafficACSrelaseNodeId = await RequestInACS(newZones);
|
|
if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId;
|
|
}
|
|
UpdateTraffic();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"{RobotModel.RobotId} Checking ex: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private NavigationPathEdge[] GetBasePath()
|
|
{
|
|
if (InNode is not null && BaseNodes is not null && BaseNodes.Count > 1)
|
|
{
|
|
var inNodeIndex = BaseNodes.FindIndex(n => n.Id == InNode.Id);
|
|
if (inNodeIndex != -1 && inNodeIndex < BaseNodes.Count - 1)
|
|
{
|
|
var nodes = BaseNodes.GetRange(inNodeIndex, BaseNodes.Count - inNodeIndex);
|
|
var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
})]);
|
|
if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
|
|
{
|
|
List<NavigationPathEdge> pathEdges = [];
|
|
var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
|
|
if (edge is null)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = Visualization.X,
|
|
StartY = Visualization.Y,
|
|
EndX = nodes[1].X,
|
|
EndY = nodes[1].Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
else
|
|
{
|
|
var splitStartPath = PathPlanner.PathSplit([new NodeDto
|
|
{
|
|
Id = nodes[0].Id,
|
|
X = nodes[0].X,
|
|
Y = nodes[0].Y,
|
|
}, new NodeDto
|
|
{
|
|
Id = nodes[1].Id,
|
|
X = nodes[1].X,
|
|
Y = nodes[1].Y,
|
|
Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
|
|
}], [edge]);
|
|
if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
|
|
{
|
|
int index = 0;
|
|
double minDistance = double.MaxValue;
|
|
for (int i = 0; i < splitStartPath.Data.Length; i++)
|
|
{
|
|
var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
index = i;
|
|
}
|
|
}
|
|
for (int i = index; i < splitStartPath.Data.Length - 1; i++)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = splitStartPath.Data[i].X,
|
|
StartY = splitStartPath.Data[i].Y,
|
|
EndX = splitStartPath.Data[i + 1].X,
|
|
EndY = splitStartPath.Data[i + 1].Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
for (int i = 1; i < nodes.Count - 1; i++)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = nodes[i].X,
|
|
StartY = nodes[i].Y,
|
|
EndX = nodes[i + 1].X,
|
|
EndY = nodes[i + 1].Y,
|
|
ControlPoint1X = edges[i].ControlPoint1X,
|
|
ControlPoint1Y = edges[i].ControlPoint1Y,
|
|
ControlPoint2X = edges[i].ControlPoint2X,
|
|
ControlPoint2Y = edges[i].ControlPoint2Y,
|
|
Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
|
|
});
|
|
}
|
|
return [.. pathEdges];
|
|
}
|
|
}
|
|
}
|
|
return [];
|
|
}
|
|
|
|
private NavigationPathEdge[] GetFullPath()
|
|
{
|
|
if (InNode is not null && CheckingNodes is not null && CheckingNodes.Count > 1)
|
|
{
|
|
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
|
|
if (inNodeIndex != -1 && inNodeIndex < CheckingNodes.Count - 1)
|
|
{
|
|
var nodes = CheckingNodes.GetRange(inNodeIndex, CheckingNodes.Count - inNodeIndex);
|
|
var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
})]);
|
|
if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
|
|
{
|
|
List<NavigationPathEdge> pathEdges = [];
|
|
var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
|
|
if (edge is null)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = Visualization.X,
|
|
StartY = Visualization.Y,
|
|
EndX = nodes[1].X,
|
|
EndY = nodes[1].Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
else
|
|
{
|
|
var splitStartPath = PathPlanner.PathSplit([new NodeDto
|
|
{
|
|
Id = nodes[0].Id,
|
|
X = nodes[0].X,
|
|
Y = nodes[0].Y,
|
|
}, new NodeDto
|
|
{
|
|
Id = nodes[1].Id,
|
|
X = nodes[1].X,
|
|
Y = nodes[1].Y,
|
|
Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
|
|
}], [edge]);
|
|
if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
|
|
{
|
|
int index = 0;
|
|
double minDistance = double.MaxValue;
|
|
for (int i = 0; i < splitStartPath.Data.Length; i++)
|
|
{
|
|
var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
index = i;
|
|
}
|
|
}
|
|
for (int i = index; i < splitStartPath.Data.Length - 1; i++)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = splitStartPath.Data[i].X,
|
|
StartY = splitStartPath.Data[i].Y,
|
|
EndX = splitStartPath.Data[i + 1].X,
|
|
EndY = splitStartPath.Data[i + 1].Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
for (int i = 1; i < nodes.Count - 1; i++)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = nodes[i].X,
|
|
StartY = nodes[i].Y,
|
|
EndX = nodes[i + 1].X,
|
|
EndY = nodes[i + 1].Y,
|
|
ControlPoint1X = edges[i].ControlPoint1X,
|
|
ControlPoint1Y = edges[i].ControlPoint1Y,
|
|
ControlPoint2X = edges[i].ControlPoint2X,
|
|
ControlPoint2Y = edges[i].ControlPoint2Y,
|
|
Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
|
|
});
|
|
}
|
|
return [.. pathEdges];
|
|
}
|
|
}
|
|
}
|
|
return [];
|
|
}
|
|
|
|
public bool UpdateGoal(NavigationNode goal)
|
|
{
|
|
if (NavigationGoal is null || NavigationGoal.Id != goal.Id)
|
|
{
|
|
if (NavigationPath is not null && NavigationPath.Any(node => node.Id == goal.Id))
|
|
{
|
|
NavigationGoal = goal;
|
|
MovePurePursuit?.UpdateGoal(goal);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private void ClearPath()
|
|
{
|
|
NavigationPath = null;
|
|
NavigationGoal = null;
|
|
InNode = null;
|
|
CheckingNodes.Clear();
|
|
TrafficManager.DeleteAgent(RobotModel.RobotId);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
CheckingStop();
|
|
NavStop();
|
|
ClearPath();
|
|
NavigationState = NavigationStateType.Ready;
|
|
VelocityController.Stop();
|
|
IsCompleted = true;
|
|
IsProcessing = false;
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
protected void NavStart()
|
|
{
|
|
navTimer = new(SampleTimeMilliseconds, NavigationHandler, Logger);
|
|
navTimer.Start();
|
|
IsCompleted = false;
|
|
IsCanceled = false;
|
|
IsError = false;
|
|
IsProcessing = true;
|
|
}
|
|
|
|
protected void NavStop()
|
|
{
|
|
navTimer?.Dispose();
|
|
}
|
|
|
|
protected void CheckingStart()
|
|
{
|
|
checkingTimer = new(CheckingTimeMilliseconds, CheckingHandler, Logger);
|
|
checkingTimer.Start();
|
|
}
|
|
|
|
public void CreateComledted()
|
|
{
|
|
IsCompleted = true;
|
|
IsCanceled = false;
|
|
IsError = false;
|
|
}
|
|
|
|
protected void CheckingStop()
|
|
{
|
|
checkingTimer?.Dispose();
|
|
}
|
|
}
|