393 lines
16 KiB
C#
393 lines
16 KiB
C#
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using UnityEngine;
|
|
|
|
public class ActionStateSender
|
|
{
|
|
private readonly MqttClientManager mqttClientManager;
|
|
private readonly OrderProcessor orderProcessor;
|
|
private readonly AnimationControllerAPM animationController;
|
|
private readonly string stateTopic;
|
|
private readonly string instantActionsTopic;
|
|
private readonly string serialNumber;
|
|
private readonly string currentMapId;
|
|
private readonly PathMover pathMover;
|
|
private uint headerIdCounter;
|
|
|
|
public ActionStateSender(
|
|
MqttClientManager mqttClientManager,
|
|
OrderProcessor orderProcessor,
|
|
AnimationControllerAPM animationController,
|
|
string stateTopic,
|
|
string instantActionsTopic,
|
|
string serialNumber,
|
|
string currentMapId,
|
|
PathMover pathMover)
|
|
{
|
|
this.mqttClientManager = mqttClientManager;
|
|
this.orderProcessor = orderProcessor;
|
|
this.animationController = animationController;
|
|
this.stateTopic = stateTopic;
|
|
this.instantActionsTopic = instantActionsTopic;
|
|
this.serialNumber = serialNumber;
|
|
this.currentMapId = currentMapId;
|
|
this.pathMover = pathMover;
|
|
this.headerIdCounter = 0;
|
|
}
|
|
|
|
public async void ProcessInstantActionJson(string json)
|
|
{
|
|
try
|
|
{
|
|
InstantActionData instantAction = await Task.Run(() => JsonConvert.DeserializeObject<InstantActionData>(json));
|
|
if (instantAction.Actions != null)
|
|
{
|
|
foreach (var action in instantAction.Actions)
|
|
{
|
|
if (action == null) continue;
|
|
|
|
Debug.Log($"Nhận được instant action: ActionType={action.ActionType}, ActionId={action.ActionId ?? "null"}, SerialNumber={serialNumber}");
|
|
|
|
if (instantAction.SerialNumber != serialNumber)
|
|
{
|
|
Debug.LogWarning($"Action có serialNumber ({instantAction.SerialNumber}) không khớp với serialNumber của robot ({serialNumber}). Bỏ qua.");
|
|
continue;
|
|
}
|
|
|
|
switch (action.ActionType)
|
|
{
|
|
case "cancelOrder":
|
|
Debug.Log($"Nhận được lệnh cancelOrder, ActionId: {(action.ActionId ?? "null")}, SerialNumber: {serialNumber}, HeaderId: {instantAction.HeaderId}");
|
|
//await SendActionConfirmation(instantAction, action, "FINISHED", "Hủy nhiệm vụ thành công");
|
|
UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
|
|
break;
|
|
|
|
case "pick":
|
|
if (animationController != null)
|
|
{
|
|
UnityMainThreadDispatcher.Enqueue(() =>
|
|
{
|
|
animationController.OnKey2();
|
|
Debug.Log($"Đã kích hoạt animation U cho instant action: {action.ActionType}");
|
|
});
|
|
//await SendActionConfirmation(instantAction, action, "FINISHED", "Animation U triggered successfully");
|
|
}
|
|
else
|
|
{
|
|
//await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
|
|
}
|
|
break;
|
|
|
|
case "drop":
|
|
if (animationController != null)
|
|
{
|
|
UnityMainThreadDispatcher.Enqueue(() =>
|
|
{
|
|
animationController.OnKey1();
|
|
Debug.Log($"Đã kích hoạt animation D cho instant action: {action.ActionType}");
|
|
});
|
|
//await SendActionConfirmation(instantAction, action, "FINISHED", "Animation D triggered successfully");
|
|
}
|
|
else
|
|
{
|
|
//await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
|
|
}
|
|
break;
|
|
|
|
default:
|
|
Debug.LogWarning($"Instant ActionType không được hỗ trợ: {action.ActionType}, SerialNumber: {serialNumber}");
|
|
//await SendActionConfirmation(instantAction, action, "FAILED", $"Unsupported ActionType: {action.ActionType}");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Debug.LogError($"Lỗi khi phân tích JSON instant action cho robot với serialNumber {serialNumber}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public async Task SendActionStateAsync(
|
|
ActionData action,
|
|
string referenceId,
|
|
string referenceType,
|
|
string actionStatus,
|
|
string resultDescription)
|
|
{
|
|
try
|
|
{
|
|
// Cập nhật trạng thái hành động trong ActionData
|
|
action.ActionStatus = actionStatus;
|
|
action.ActionDescription = resultDescription;
|
|
|
|
var actionState = new ActionState
|
|
{
|
|
ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
|
|
ActionType = action.ActionType,
|
|
ActionDescription = action.ActionDescription ?? "",
|
|
ActionStatus = actionStatus,
|
|
};
|
|
|
|
var stateData = new SendState
|
|
{
|
|
HeaderId = headerIdCounter++,
|
|
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
|
Version = "1.0.0",
|
|
Manufacturer = "phenikaaX",
|
|
SerialNumber = serialNumber,
|
|
Maps = new Map[]
|
|
{
|
|
new Map
|
|
{
|
|
MapId = currentMapId,
|
|
MapVersion = "1.0",
|
|
MapDescription = "Default map",
|
|
MapStatus = "ENABLED"
|
|
}
|
|
},
|
|
OrderId = orderProcessor?.OrderId ?? "",
|
|
OrderUpdateId = orderProcessor?.OrderId != null ? 1 : 0,
|
|
ZoneSetId = "",
|
|
LastNodeId = orderProcessor?.LastNodeId ?? "",
|
|
LastNodeSequenceId = orderProcessor?.LastNodeSequenceId ?? 0,
|
|
NodeStates = orderProcessor?.CurrentOrder?.Nodes?.Select(n => new NodeState
|
|
{
|
|
NodeId = n.NodeId,
|
|
SequenceId = n.SequenceId,
|
|
NodeDescription = n.NodeDescription ?? "",
|
|
Released = n.Released,
|
|
NodePosition = new NodePosition
|
|
{
|
|
X = n.NodePosition.X,
|
|
Y = n.NodePosition.Y,
|
|
Theta = n.NodePosition.Theta,
|
|
AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
|
|
AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
|
|
MapId = n.NodePosition.MapId,
|
|
MapDescription = n.NodePosition.MapDescription ?? ""
|
|
}
|
|
}).ToArray() ?? new NodeState[0],
|
|
EdgeStates = orderProcessor?.CurrentOrder?.Edges?.Select(e => new EdgeState
|
|
{
|
|
EdgeId = e.EdgeId,
|
|
SequenceId = e.SequenceId,
|
|
EdgeDescription = e.EdgeDescription ?? "",
|
|
Released = e.Released,
|
|
Trajectory = e.Trajectory != null ? new Trajectory
|
|
{
|
|
Degree = e.Trajectory.Degree,
|
|
KnotVector = e.Trajectory.KnotVector,
|
|
ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
|
|
{
|
|
X = cp.X,
|
|
Y = cp.Y,
|
|
Weight = cp.Weight
|
|
}).ToArray()
|
|
} : null
|
|
}).ToArray() ?? new EdgeState[0],
|
|
Driving = pathMover?.IsMoving ?? false,
|
|
Paused = !(pathMover?.IsMoving ?? false),
|
|
NewBaseRequest = !(pathMover?.IsMoving ?? false),
|
|
DistanceSinceLastNode = 1.531483174223427f,
|
|
AgvPosition = new AgvPosition
|
|
{
|
|
X = 0f, // Will be updated by VisualizationSender
|
|
Y = 0f,
|
|
Theta = 0f,
|
|
MapId = currentMapId,
|
|
MapDescription = "",
|
|
PositionInitialized = true,
|
|
LocalizationScore = 0.7f,
|
|
DeviationRange = 0.1f
|
|
},
|
|
Velocity = new Velocity
|
|
{
|
|
Vx = 0f,
|
|
Vy = 0f,
|
|
Omega = 0f
|
|
},
|
|
Loads = new Load[0],
|
|
ActionStates = new[] { actionState },
|
|
BatteryState = new BatteryState
|
|
{
|
|
BatteryCharge = 80f,
|
|
BatteryVoltage = 48f,
|
|
BatteryHealth = 100f,
|
|
Charging = false,
|
|
Reach = 1000f
|
|
},
|
|
OperatingMode = "AUTOMATIC",
|
|
Errors = new Error[0],
|
|
Information = new Information[0],
|
|
SafetyState = new SafetyState
|
|
{
|
|
EStop = "NONE",
|
|
FieldViolation = false
|
|
}
|
|
};
|
|
|
|
string json = JsonConvert.SerializeObject(stateData, Formatting.None);
|
|
await mqttClientManager.PublishAsync(stateTopic, json);
|
|
Debug.Log($"Đã gửi trạng thái action {action.ActionType} ({referenceType} {referenceId}): {actionStatus} - {resultDescription}");
|
|
}
|
|
catch (Exception)
|
|
{
|
|
}
|
|
}
|
|
|
|
private async Task SendActionConfirmation(InstantActionData instantAction, ActionData action, string actionStatus, string resultDescription)
|
|
{
|
|
try
|
|
{
|
|
var confirmation = new CancelConfirmationData
|
|
{
|
|
HeaderId = instantAction.HeaderId,
|
|
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
|
Version = instantAction.Version ?? "0.0.1",
|
|
Manufacturer = instantAction.Manufacturer ?? "phenikaaX",
|
|
SerialNumber = instantAction.SerialNumber ?? serialNumber,
|
|
ActionStates = new[]
|
|
{
|
|
new ActionState
|
|
{
|
|
ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
|
|
ActionType = action.ActionType,
|
|
ActionStatus = actionStatus,
|
|
ResultDescription = resultDescription
|
|
}
|
|
}
|
|
};
|
|
|
|
string confirmationJson = JsonConvert.SerializeObject(confirmation, Formatting.None);
|
|
await mqttClientManager.PublishAsync(stateTopic, confirmationJson);
|
|
Debug.Log($"Đã gửi xác nhận action {action.ActionType} lên topic {stateTopic}: {confirmationJson}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Debug.LogError($"Lỗi khi gửi xác nhận action {action.ActionType}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public IEnumerator SendCancelOrder(Node node, ActionData action)
|
|
{
|
|
if (node is null)
|
|
{
|
|
yield break;
|
|
}
|
|
|
|
if (action is null)
|
|
{
|
|
yield break;
|
|
}
|
|
|
|
// Kiểm tra orderProcessor
|
|
if (orderProcessor == null)
|
|
{
|
|
// Gửi thông báo hủy qua MQTT mà không cần orderProcessor
|
|
var cancelOrderDataWithoutProcessor = new
|
|
{
|
|
HeaderId = headerIdCounter++,
|
|
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
|
Version = "0.0.1",
|
|
Manufacturer = "phenikaaX",
|
|
SerialNumber = serialNumber ?? "unknown",
|
|
Actions = new[]
|
|
{
|
|
new
|
|
{
|
|
ActionType = "cancelOrder",
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
|
|
BlockingType = "NONE",
|
|
ActionParameters = new[]
|
|
{
|
|
new { Key = "ORDER_ID", Value = "unknown" },
|
|
new { Key = "REASON", Value = "Không có pallet" }
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithoutProcessor, Formatting.None);
|
|
|
|
if (mqttClientManager != null)
|
|
{
|
|
Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
|
|
while (!publishTask.IsCompleted)
|
|
{
|
|
yield return null;
|
|
}
|
|
if (publishTask.IsFaulted)
|
|
{
|
|
Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
|
|
}
|
|
else
|
|
{
|
|
Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
|
|
}
|
|
yield break;
|
|
}
|
|
|
|
// Hủy order ngay lập tức
|
|
UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
|
|
Debug.Log($"Đã hủy order với OrderId: {orderProcessor.OrderId ?? "N/A"} do không có pallet tại node {node.NodeDescription}");
|
|
|
|
// Tạo JSON hủy order
|
|
var cancelOrderDataWithProcessor = new
|
|
{
|
|
HeaderId = headerIdCounter++,
|
|
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
|
Version = "0.0.1",
|
|
Manufacturer = "phenikaaX",
|
|
SerialNumber = orderProcessor.SerialNumber ?? "unknown",
|
|
Actions = new[]
|
|
{
|
|
new
|
|
{
|
|
ActionType = "cancelOrder",
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
|
|
BlockingType = "NONE",
|
|
ActionParameters = new[]
|
|
{
|
|
new { Key = "ORDER_ID", Value = orderProcessor.OrderId ?? "" },
|
|
new { Key = "REASON", Value = "Không có pallet" }
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
// Gửi JSON qua MQTT
|
|
if (mqttClientManager != null)
|
|
{
|
|
string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithProcessor, Formatting.None);
|
|
|
|
Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
|
|
while (!publishTask.IsCompleted)
|
|
{
|
|
yield return null;
|
|
}
|
|
if (publishTask.IsFaulted)
|
|
{
|
|
Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
|
|
}
|
|
else
|
|
{
|
|
Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
|
|
}
|
|
}
|
|
} |