DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update connect synaos

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-11-12 14:03:04 +07:00
parent 8736bad3e7
commit 3b088a6d5d
16 changed files with 191 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
RobotApp/Interfaces/ILoad.cs
View File

@ -5,4 +5,7 @@ namespace RobotApp.Interfaces;
public interface ILoad
{
Load[] Load { get; }
void AddLoad();
void ClearLoad();
}

29
RobotApp/Services/MQTTClient.cs
View File

@ -37,12 +37,12 @@ public class MQTTClient : IAsyncDisposable
MqttClientFactory = new MqttClientFactory();
MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
.WithTcpServer(VDA5050Setting.HostServer, VDA5050Setting.Port)
.WithClientId(ClientId)
.WithClientId($"{ClientId}")
.WithCleanSession(true)
.Build();
MqttClientSubscribeOptions = MqttClientFactory.CreateSubscribeOptionsBuilder()
.WithTopicFilter(f => f.WithTopic(OrderTopic))
.WithTopicFilter(f => f.WithTopic(InstanceActionsTopic))
.WithTopicFilter(f => f.WithTopic(OrderTopic).WithAtLeastOnceQoS().WithExactlyOnceQoS().WithAtMostOnceQoS())
.WithTopicFilter(f => f.WithTopic(InstanceActionsTopic).WithAtLeastOnceQoS().WithExactlyOnceQoS().WithAtMostOnceQoS())
.Build();
}
@ -146,6 +146,10 @@ public class MQTTClient : IAsyncDisposable
await CleanupCurrentClient();
MqttClient = MqttClientFactory.CreateMqttClient();
//MqttClient.ApplicationMessageReceivedAsync -= OnMessageReceived;
MqttClient.ApplicationMessageReceivedAsync += OnMessageReceived;
//MqttClient.DisconnectedAsync -= OnDisconnected;
MqttClient.DisconnectedAsync += OnDisconnected;
while (!cancellationToken.IsCancellationRequested)
{
@ -156,7 +160,7 @@ public class MQTTClient : IAsyncDisposable
Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");
else
{
Logger.Info("Kết nối tới broker thành công");
Logger.Info($"Kết nối tới broker {VDA5050Setting.HostServer} thành công");
break;
}
}
@ -218,7 +222,7 @@ public class MQTTClient : IAsyncDisposable
.WithCleanSession(true);
if (enablePassword)
{
builder.WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password);
builder = builder.WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password);
}
if (enableTls)
@ -226,10 +230,10 @@ public class MQTTClient : IAsyncDisposable
var tlsOptions = new MqttClientTlsOptionsBuilder()
.UseTls(true)
.WithSslProtocols(System.Security.Authentication.SslProtocols.Tls12 | System.Security.Authentication.SslProtocols.Tls13)
.WithCertificateValidationHandler(ValidateCertificates)
//.WithCertificateValidationHandler(ValidateCertificates)
.WithClientCertificatesProvider(new MQTTClientCertificatesProvider(VDA5050Setting.CerFile, VDA5050Setting.KeyFile))
.Build();
builder.WithTlsOptions(tlsOptions);
builder = builder.WithTlsOptions(tlsOptions);
}
MqttClientOptions = builder.Build();
}
@ -238,6 +242,7 @@ public class MQTTClient : IAsyncDisposable
{
try
{
Logger.Info($"Has new message: {args.ApplicationMessage.Topic}");
if (IsDisposed) return Task.CompletedTask;
var stringData = Encoding.UTF8.GetString(args.ApplicationMessage.Payload);
@ -255,13 +260,11 @@ public class MQTTClient : IAsyncDisposable
public async Task SubscribeAsync(CancellationToken cancellationToken = default)
{
if (!IsDisposed)
{
//if (!IsDisposed)
//{
if (MqttClient is null) throw new Exception("Kết nối tới broker chưa được khởi tạo nhưng đã yêu cầu subscribe");
if (!MqttClient.IsConnected) throw new Exception("Kết nối tới broker chưa thành công nhưng đã yêu cầu subscribe");
MqttClient.ApplicationMessageReceivedAsync -= OnMessageReceived;
MqttClient.ApplicationMessageReceivedAsync += OnMessageReceived;
while (!cancellationToken.IsCancellationRequested)
{
@ -295,8 +298,8 @@ public class MQTTClient : IAsyncDisposable
await Task.Delay(3000, cancellationToken);
}
}
}
else throw new ObjectDisposedException(nameof(MQTTClient));
//}
//else throw new ObjectDisposedException(nameof(MQTTClient));
}
public async Task<MessageResult> PublishAsync(string topic, string data)

2
RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs
View File

@ -26,7 +26,7 @@ public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAct
}
else
{
if (string.IsNullOrEmpty(RobotOrder.OrderId) && !RobotAction.HasActionRunning)
if (RobotOrder.NodeStates.Length == 0 && RobotOrder.EdgeStates.Length == 0 && !RobotAction.HasActionRunning)
{
Status = ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;

21
RobotApp/Services/Robot/Actions/RobotDropAction.cs Normal file
View File

@ -0,0 +1,21 @@
using RobotApp.Interfaces;
namespace RobotApp.Services.Robot.Actions;
public class RobotDropAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override async Task StartAction()
{
await Task.Delay(2000);
Scope ??= ServiceProvider.CreateAsyncScope();
var LoadManager = Scope.ServiceProvider.GetRequiredService<ILoad>();
LoadManager.ClearLoad();
Status = VDA5050.State.ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

21
RobotApp/Services/Robot/Actions/RobotPickAction.cs Normal file
View File

@ -0,0 +1,21 @@
using RobotApp.Interfaces;
namespace RobotApp.Services.Robot.Actions;
public class RobotPickAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override async Task StartAction()
{
await Task.Delay(2000);
Scope ??= ServiceProvider.CreateAsyncScope();
var LoadManager = Scope.ServiceProvider.GetRequiredService<ILoad>();
LoadManager.AddLoad();
Status = VDA5050.State.ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

2
RobotApp/Services/Robot/RobotActionStorage.cs
View File

@ -18,6 +18,8 @@ public class RobotActionStorage(IServiceProvider ServiceProvider)
ActionType.cancelOrder => new RobotCancelOrderAction(ServiceProvider),
ActionType.liftUp => new RobotLiftUpAction(ServiceProvider),
ActionType.liftDown => new RobotLiftDownAction(ServiceProvider),
ActionType.drop => new RobotDropAction(ServiceProvider),
ActionType.pick => new RobotPickAction(ServiceProvider),
ActionType.liftRotate => new RobotLiftRotateAction(ServiceProvider),
ActionType.rotate => new RobotRotateAction(ServiceProvider),
ActionType.rotateKeepLift => new RobotRotateKeepLift(ServiceProvider),

14
RobotApp/Services/Robot/RobotController.cs
View File

@ -40,13 +40,13 @@ public partial class RobotController(IOrder OrderManager,
try
{
if (StateManager.RootStateName != RootStateType.Auto.ToString()) throw new OrderException(RobotErrors.Error1013(StateManager.RootStateName));
if (string.IsNullOrEmpty(OrderManager.OrderId))
{
if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
}
else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
//if (OrderManager.NodeStates.Length > 0)
//{
// if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
// if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
// if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
//}
//else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
OrderManager.UpdateOrder(order);
}
catch (RobotExeption orEx)

2
RobotApp/Services/Robot/RobotControllerInitialize.cs
View File

@ -54,6 +54,8 @@ public partial class RobotController
}
}
else StateManager.TransitionTo(RootStateType.Auto);
ErrorManager.AddError(RobotErrors.Error2001());
}
catch (Exception ex)
{

22
RobotApp/Services/Robot/RobotFactsheet.cs
View File

@ -22,6 +22,8 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
{ ActionType.cancelOrder, CancelOrder},
{ ActionType.liftUp, LiftUp},
{ ActionType.liftDown, LiftDown},
{ ActionType.pick, Pick},
{ ActionType.drop, Drop},
{ ActionType.liftRotate, LiftRotate},
{ ActionType.rotate, Rotate},
{ ActionType.rotateKeepLift, RotateKeepLift},
@ -185,6 +187,26 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction Pick = new()
{
ActionType = ActionType.pick.ToString(),
ActionDescription = "Nâng cao pallet.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã nâng cao pallet.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction Drop = new()
{
ActionType = ActionType.drop.ToString(),
ActionDescription = "Hạ thấp pallet.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã hạ thấpo pallet.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction LiftRotate = new()
{
ActionType = ActionType.liftRotate.ToString(),

2
RobotApp/Services/Robot/RobotInfomations.cs
View File

@ -1,7 +1,5 @@
using RobotApp.Interfaces;
using RobotApp.Services.State;
using RobotApp.VDA5050.State;
using RobotApp.VDA5050.Type;
namespace RobotApp.Services.Robot;

13
RobotApp/Services/Robot/RobotLoads.cs
View File

@ -5,7 +5,8 @@ namespace RobotApp.Services.Robot;
public class RobotLoads(IPeripheral PeriperalManager) : ILoad
{
public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : [];
//public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : [];
public Load[] Load { get; private set; } = [];
private static Load GetLoad()
{
return new()
@ -28,4 +29,14 @@ public class RobotLoads(IPeripheral PeriperalManager) : ILoad
Weight = 999
};
}
public void AddLoad()
{
Load = [new()];
}
public void ClearLoad()
{
Load = [];
}
}

2
RobotApp/Services/Robot/RobotLocalization.cs
View File

@ -36,7 +36,7 @@ public class RobotLocalization(RobotConfiguration RobotConfiguration, Simulation
public string SlamStateDetail => IsSimulation ? "" : XlocData.SlamStateDetail;
public string CurrentActiveMap => IsSimulation ? "" : XlocData.CurrentActiveMap;
public double Reliability => IsSimulation ? 100 : XlocData.Reliability;
public double MatchingScore => IsSimulation ? 100 : XlocData.MatchingScore;
public double MatchingScore => IsSimulation ? 1.0 : XlocData.MatchingScore;
public bool IsReady => IsSimulation || XlocData.IsReady;
private readonly XlocData XlocData = new();

96
RobotApp/Services/Robot/RobotOrderController.cs
View File

@ -1,14 +1,13 @@
using RobotApp.Common.Shares.Enums;
using MudBlazor;
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.Services.Robot.Actions;
using RobotApp.Services.State;
using RobotApp.VDA5050.InstantAction;
using RobotApp.VDA5050.Order;
using RobotApp.VDA5050.State;
using System.Collections.Concurrent;
using System.Data;
using System.Threading.Tasks;
using Action = RobotApp.VDA5050.InstantAction.Action;
namespace RobotApp.Services.Robot;
@ -34,10 +33,11 @@ public class RobotOrderController(INavigation NavigationManager,
private readonly Dictionary<string, Action[]> OrderActions = [];
private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
private List<Action> FinalAction = [];
private OrderMsg? NewOrder;
private Node[] Nodes = [];
private Edge[] Edges = [];
private VDA5050.Order.Edge[] Edges = [];
private Node? CurrentBaseNode;
private Node? LastNode;
@ -46,6 +46,8 @@ public class RobotOrderController(INavigation NavigationManager,
private bool IsCancelOrder = false;
private bool IsActionRunning = false;
private bool IsWaitingPaused = false;
private bool IsNavigationFinished = false;
private bool IsOneceNode = false;
private Action? ActionHard = null;
public void UpdateOrder(OrderMsg order)
@ -59,7 +61,7 @@ public class RobotOrderController(INavigation NavigationManager,
public void StopOrder()
{
if (!string.IsNullOrEmpty(OrderId) || OrderTimer is not null) IsCancelOrder = true;
if (NodeStates.Length > 0 || OrderTimer is not null) IsCancelOrder = true;
}
public void PauseOrder()
@ -82,6 +84,17 @@ public class RobotOrderController(INavigation NavigationManager,
{
OrderTimer?.Dispose();
OrderTimer = null;
//OrderId = string.Empty;
//OrderUpdateId = 0;
OrderActions.Clear();
ActionWaitingRunning.Clear();
CurrentBaseNode = null;
Nodes = [];
Edges = [];
UpdateState();
SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
NavigationManager.OnNavigationFinished -= NavigationFinished;
StateManager.TransitionTo(AutoStateType.Idle);
}
private Node? GetCurrentNode()
@ -91,9 +104,10 @@ public class RobotOrderController(INavigation NavigationManager,
foreach (var node in Nodes)
{
var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
if (distance <= node.NodePosition.AllowedDeviationXY)
var nodeMin = node.NodePosition.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition.AllowedDeviationXY;
if (distance <= nodeMin)
{
if (distance < minDistance)
if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
{
minDistance = distance;
inNode = node;
@ -105,17 +119,7 @@ public class RobotOrderController(INavigation NavigationManager,
private void NavigationFinished()
{
HandleOrderStop();
SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
OrderId = string.Empty;
OrderUpdateId = 0;
OrderActions.Clear();
ActionWaitingRunning.Clear();
CurrentBaseNode = null;
Nodes = [];
Edges = [];
UpdateState();
StateManager.TransitionTo(AutoStateType.Idle);
IsNavigationFinished = true;
}
private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
@ -153,10 +157,24 @@ public class RobotOrderController(INavigation NavigationManager,
private void HandleNewOrder(OrderMsg order)
{
if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
//if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
OrderActions.Clear();
LastNode = null;
FinalAction = [];
IsNavigationFinished = false;
for (int i = 0; i < order.Edges.Length; i++)
{
if (order.Edges[i].Trajectory is null)
{
order.Edges[i].Trajectory = new Trajectory()
{
Degree = 1,
ControlPoints = [new ControlPoint() { X = 0, Y = 0 }]
};
}
}
for (int i = 0; i < order.Nodes.Length; i++)
{
@ -178,28 +196,34 @@ public class RobotOrderController(INavigation NavigationManager,
{
item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
}
if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null)
{
actions = [.. actions, .. order.Edges[i].Actions];
}
else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions);
}
if (order.Nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i));
if (i < order.Nodes.Length - 1 && order.Edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(order.Edges[i].EdgeId, order.Edges[i].SequenceId, i));
//if (order.Nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i));
//if (i < order.Nodes.Length - 1 && order.Edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(order.Edges[i].EdgeId, order.Edges[i].SequenceId, i));
if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i];
}
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];
ActionManager.ClearInstantActions();
if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
NavigationManager.Move(order.Nodes, order.Edges);
NavigationManager.OnNavigationFinished += NavigationFinished;
if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
{
NavigationManager.Move(order.Nodes, order.Edges);
NavigationManager.OnNavigationFinished += NavigationFinished;
}
else IsOneceNode = true;
OrderId = order.OrderId;
OrderUpdateId = order.OrderUpdateId;
Nodes = order.Nodes;
Edges = order.Edges;
if (CurrentBaseNode is not null) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
UpdateState();
}
@ -248,6 +272,24 @@ public class RobotOrderController(INavigation NavigationManager,
return;
}
if (IsNavigationFinished || IsOneceNode)
{
if (FinalAction.Count > 0)
{
var action = FinalAction[0];
var robotAction = ActionManager[action.ActionId];
if (robotAction is null) return;
if (robotAction.IsCompleted) FinalAction.Remove(action);
if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
}
else
{
LastNode = Nodes[^1]; // ddoanj nay phai sua lai kien tra xem co toi node cuoi that khong
HandleOrderStop();
return;
}
}
var currentNode = GetCurrentNode();
if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
{
@ -319,10 +361,10 @@ public class RobotOrderController(INavigation NavigationManager,
}
if (NewOrderHandler.Nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length));
if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
//if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length));
//if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
if (string.IsNullOrEmpty(OrderId)) HandleNewOrder(NewOrderHandler);
if (NodeStates.Length == 0) HandleNewOrder(NewOrderHandler);
else HandleUpdateOrder(NewOrderHandler);
}
HandleOrder();

8
RobotApp/Services/Robot/RobotPathPlanner.cs
View File

@ -13,7 +13,7 @@ public class RobotPathPlanner(IConfiguration Configuration)
private readonly double ReverseDirectionAngleDegree = Configuration.GetValue("PathPlanning:ReverseDirectionAngleDegree", 89);
private readonly double Ratio = Configuration.GetValue("PathPlanning:Ratio", 0.1);
public RobotDirection GetDirectionInNode(double currentTheta,Node inNode, Node futureNode, Edge edge)
public RobotDirection GetDirectionInNode(double currentTheta, Node inNode, Node futureNode, Edge edge)
{
(double futurex, double futurey) = MathExtensions.Curve(0.1, new()
{
@ -26,8 +26,8 @@ public class RobotPathPlanner(IConfiguration Configuration)
ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
});
(double robotx, double roboty) =
});
(double robotx, double roboty) =
(
inNode.NodePosition.X + Math.Cos(currentTheta * Math.PI / 180),
inNode.NodePosition.Y + Math.Sin(currentTheta * Math.PI / 180)
@ -72,7 +72,7 @@ public class RobotPathPlanner(IConfiguration Configuration)
ControlPoint2X = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].X : 0,
ControlPoint2Y = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].Y : 0,
TrajectoryDegree = edges[i - 1].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i - 1].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
});
});
(double futurex, double futurey) = MathExtensions.Curve(0.1, new()
{
X1 = nodes[i].NodePosition.X,

8
RobotApp/Services/Robot/RobotStates.cs
View File

@ -47,12 +47,12 @@ public class RobotStates(RobotConfiguration RobotConfiguration,
Maps = [],
OrderId = OrderManager.OrderId,
OrderUpdateId = OrderManager.OrderUpdateId,
ZoneSetId = "",
LastNodeId = OrderManager.LastNodeId,
ZoneSetId = "9879E7A9-CF5F-4ABD-924E-08DE1C3D25FE",
LastNodeId = string.IsNullOrEmpty(OrderManager.LastNodeId) ? "EE9959EC-B670-4D22-8BD2-08DE1C3D71FC" : OrderManager.LastNodeId,
LastNodeSequenceId = OrderManager.LastNodeSequenceId,
Driving = false,
Driving = NavigationManager.VelocityX > 0 || NavigationManager.Omega > 0,
Paused = false,
NewBaseRequest = false,
NewBaseRequest = true,
DistanceSinceLastNode = 0,
OperatingMode = PeripheralManager.PeripheralMode.ToString(),
NodeStates = OrderManager.NodeStates,

9
RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
View File

@ -94,12 +94,15 @@ public class SimulationNavigation : INavigation, IDisposable
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;
(NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
if(index >= NavigationPath.Length - 1) return;
double angleFoward = Math.Atan2(NavigationPath[index + 1].Y - node.Y, NavigationPath[index + 1].X - node.X) * 180 / Math.PI;
double angleBacward = Math.Atan2(node.Y - NavigationPath[index + 1].Y, node.X - NavigationPath[index + 1].X) * 180 / Math.PI;
OrderNodes = nodes;
OrderEdges = edges;
SimulationOrderNodes = pathDirection;
Rotate(SimulationOrderNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
}
public void MoveStraight(double x, double y)