using Microsoft.Extensions.Logging;
|
using WIDESEA_Core;
|
using WIDESEAWCS_Common.TaskEnum;
|
using WIDESEAWCS_Core.Helper;
|
using WIDESEAWCS_Core.LogHelper;
|
using WIDESEAWCS_ITaskInfoService;
|
using WIDESEAWCS_Model.Models;
|
using WIDESEAWCS_Tasks.SocketServer;
|
using WIDESEAWCS_Tasks.Workflow.Abstractions;
|
|
namespace WIDESEAWCS_Tasks.Workflow
|
{
|
/// <summary>
|
/// 机器人任务编排器 - 负责 RobotJob 中的状态机流转和执行步骤编排
|
/// </summary>
|
/// <remarks>
|
/// 迁移原 RobotJob 状态机流转支持，降低 Job 层的复杂度。
|
///
|
/// 核心职责：
|
/// 1. 根据机器人当前状态和任务状态，决定下一步动作
|
/// 2. 处理取货完成后的放货指令下发
|
/// 3. 处理放货完成后的取货指令下发（组盘场景）
|
///
|
/// 状态机流转规则：
|
/// - 条件：RobotRunMode == 2（自动模式）且 RobotControlMode == 1（客户端控制）且 OperStatus != "Running"
|
/// - 取货完成 -> 放货：PickFinished + RobotArmObject == 1 + RobotPickFinish -> 发送 Putbattery
|
/// - 放货完成 -> 取货：PutFinished + Homed + RobotArmObject == 0 -> 发送 Pickbattery（组盘/换盘场景）
|
/// </remarks>
|
public class RobotWorkflowOrchestrator : IRobotWorkflowOrchestrator
|
{
|
/// <summary>
|
/// 机械手状态管理器
|
/// </summary>
|
/// <remarks>
|
/// 用于读取和更新机器人的状态。
|
/// </remarks>
|
private readonly RobotStateManager _stateManager;
|
|
/// <summary>
|
/// 机械手客户端管理器
|
/// </summary>
|
/// <remarks>
|
/// 用于向机器人客户端发送指令。
|
/// </remarks>
|
private readonly RobotClientManager _clientManager;
|
|
/// <summary>
|
/// 任务处理器
|
/// </summary>
|
/// <remarks>
|
/// 用于执行任务相关的业务逻辑，如发送取货指令。
|
/// </remarks>
|
private readonly RobotTaskProcessor _taskProcessor;
|
|
/// <summary>
|
/// 机器人任务服务
|
/// </summary>
|
/// <remarks>
|
/// 用于更新任务状态。
|
/// </remarks>
|
private readonly IRobotTaskService _robotTaskService;
|
|
/// <summary>
|
/// 日志记录器
|
/// </summary>
|
private readonly ILogger _logger;
|
|
/// <summary>
|
/// 构造函数
|
/// </summary>
|
/// <param name="stateManager">状态管理器</param>
|
/// <param name="clientManager">客户端管理器</param>
|
/// <param name="taskProcessor">任务处理器</param>
|
/// <param name="robotTaskService">任务服务</param>
|
/// <param name="logger">日志记录器</param>
|
public RobotWorkflowOrchestrator(
|
RobotStateManager stateManager,
|
RobotClientManager clientManager,
|
RobotTaskProcessor taskProcessor,
|
IRobotTaskService robotTaskService,
|
ILogger logger)
|
{
|
_stateManager = stateManager;
|
_clientManager = clientManager;
|
_taskProcessor = taskProcessor;
|
_robotTaskService = robotTaskService;
|
_logger = logger;
|
}
|
|
/// <summary>
|
/// 执行任务编排流程
|
/// </summary>
|
/// <remarks>
|
/// 根据机器人当前状态和任务状态，决定是否下发指令。
|
///
|
/// 执行条件：
|
/// 1. 机器人处于自动模式（RobotRunMode == 2）
|
/// 2. 机器人处于客户端控制模式（RobotControlMode == 1）
|
/// 3. 机器人不在运行中（OperStatus != "Running"）
|
///
|
/// 流转逻辑：
|
/// - 取货完成且手臂有货 -> 发送放货指令（Putbattery）
|
/// - 放货完成且手臂无货 -> 发送取货指令（Pickbattery）
|
/// </remarks>
|
/// <param name="latestState">机器人最新状态</param>
|
/// <param name="task">待执行的机器人任务</param>
|
/// <param name="ipAddress">机器人 IP 地址</param>
|
public async Task ExecuteAsync(RobotSocketState latestState, Dt_RobotTask task, string ipAddress)
|
{
|
// 按原方案分支判断，确保逻辑一致
|
// 检查是否满足自动控制条件：
|
// 1. 运行模式为自动（2）
|
// 2. 控制模式为客户端控制（1）
|
// 3. 运行状态是 Running
|
if (latestState.RobotRunMode == 2 /*&& latestState.RobotControlMode == 1*/ && latestState.OperStatus == "Running" && (latestState.Homed == "Homed" || latestState.Homed.IsNullOrEmpty()))
|
{
|
// ========== 取货完成后的放货处理 ==========
|
// 条件：
|
// - 当前动作是 PickFinished 或 AllPickFinished（取货完成）
|
// - 手臂上有物料（RobotArmObject == 1）
|
// - 任务状态为 RobotPickFinish（已记录取货完成）
|
if ((latestState.CurrentAction == "PickFinished" || latestState.CurrentAction == "AllPickFinished")
|
&& (latestState.RobotArmObject.IsNullOrEmpty() || latestState.RobotArmObject == 1)
|
&& task.RobotTaskState == TaskRobotStatusEnum.RobotPickFinish.GetHashCode())
|
{
|
_logger.LogInformation("ExecuteAsync：满足放货条件，开始下发放货任务，任务号: {TaskNum}", task.RobotTaskNum);
|
QuartzLogger.Info($"ExecuteAsync：满足放货条件，开始下发放货任务", latestState.RobotCrane?.DeviceName ?? ipAddress);
|
// 发送放货指令
|
await HandlePickFinishedStateAsync(task, ipAddress);
|
}
|
|
// ========== 初始化或者放货完成后的取货处理 ==========
|
// 条件：
|
// - 当前动作是 PutFinished、AllPutFinished 或 null（放货完成）
|
// - 运行状态为 Homed（已归位）
|
// - 手臂上无物料（RobotArmObject == 0）
|
// - 任务状态为 RobotPutFinish 或不是 RobotExecuting
|
else if ((latestState.CurrentAction == "PutFinished" || latestState.CurrentAction == "AllPutFinished" || latestState.CurrentAction.IsNullOrEmpty())
|
&& (latestState.RobotArmObject.IsNullOrEmpty() || latestState.RobotArmObject == 0)
|
&& (task.RobotTaskState == TaskRobotStatusEnum.RobotPutFinish.GetHashCode()
|
|| task.RobotTaskState != TaskRobotStatusEnum.RobotExecuting.GetHashCode()))
|
{
|
_logger.LogInformation("ExecuteAsync：满足取货条件，开始下发取货任务，任务号: {TaskNum}", task.RobotTaskNum);
|
QuartzLogger.Info($"ExecuteAsync：满足取货条件，开始下发取货任务", latestState.RobotCrane?.DeviceName ?? ipAddress);
|
// 发送取货指令
|
await HandlePutFinishedStateAsync(task, ipAddress);
|
}
|
}
|
}
|
|
/// <summary>
|
/// 处理取货完成后的放货指令
|
/// </summary>
|
/// <remarks>
|
/// 当取货完成后，向机器人发送放货指令（Putbattery）。
|
/// 换盘任务使用批次格式 SendPutWithBatchAsync。
|
///
|
/// 指令格式：Putbattery,{目标地址}
|
/// 例如：Putbattery,B01 表示将货物放置到 B01 位置
|
/// </remarks>
|
/// <param name="task">当前任务</param>
|
/// <param name="ipAddress">机器人 IP 地址</param>
|
private async Task HandlePickFinishedStateAsync(Dt_RobotTask task, string ipAddress)
|
{
|
string taskString;
|
|
var state = _stateManager.GetState(ipAddress);
|
|
// 换盘任务批次模式
|
if (task.RobotTaskType == RobotTaskTypeEnum.ChangePallet.GetHashCode())
|
{
|
int targetNormalCount = task.RobotTaskTotalNum;
|
int currentCompletedCount = state?.RobotTaskTotalNum ?? 0;
|
|
bool isFlowA = task.RobotSourceAddressLineCode is "11001" or "11010";
|
|
// ==================== Phase 2: 放正常电芯到目标托盘（两流向相同）====================
|
// PickFinished 到达：Phase 1 的 Pick 命令完成，现在下发 Put 命令放正常电芯
|
if (state?.ChangePalletPhase == 2)
|
{
|
int remainingNormal = targetNormalCount - currentCompletedCount;
|
if (remainingNormal <= 0)
|
{
|
// 正常电芯全部放完，等待 HandlePutFinishedAsync 切换到 Phase 3
|
return;
|
}
|
|
int batchStart = (currentCompletedCount / 4) * 4 + 1;
|
int putCount = Math.Min(4, remainingNormal);
|
var (start, end) = _taskProcessor.BuildBatchRange(batchStart, putCount);
|
|
await _taskProcessor.SendPutWithBatchAsync(task, state, task.RobotTargetAddress, start, end);
|
|
// Phase 保持为 2，等 HandlePutFinishedAsync 处理完放货计数后再切回 Phase 1
|
_stateManager.TryUpdateStateSafely(ipAddress, state);
|
return;
|
}
|
|
// ==================== Phase 4: 放假电芯（两流向分叉）====================
|
// PickFinished 到达：Phase 3 的 Pick 命令完成，现在下发 Put 命令放假电芯
|
if (state?.ChangePalletPhase == 4)
|
{
|
int fakeCount = 48 - targetNormalCount;
|
int completedFake = Math.Max(0, currentCompletedCount - targetNormalCount);
|
int remainingFake = fakeCount - completedFake;
|
|
if (remainingFake <= 0)
|
{
|
// 假电芯全部放完，等待 allputfinished 触发 Phase 5 入库
|
return;
|
}
|
|
if (isFlowA)
|
{
|
// 流向A：放假电芯到目标托盘
|
int batchStart = targetNormalCount + 1 + (state.CurrentBatchIndex - 1);
|
int putCount = Math.Min(4, remainingFake);
|
var (start, end) = _taskProcessor.BuildBatchRange(batchStart, putCount);
|
|
await _taskProcessor.SendPutWithBatchAsync(task, state, task.RobotTargetAddress, start, end);
|
|
state.CurrentBatchIndex += putCount;
|
// Phase 保持为 4，等 HandlePutFinishedAsync 处理完后再切回 Phase 3
|
_stateManager.TryUpdateStateSafely(ipAddress, state);
|
}
|
else
|
{
|
// 流向B：放假电芯到5号位
|
var positions = _taskProcessor.GetNextAvailableFakeBatteryPositions(Math.Min(4, remainingFake));
|
if (positions.Count == 0)
|
{
|
_logger.LogError("HandlePickFinishedStateAsync：无可用假电芯点位，任务号: {TaskNum}", task.RobotTaskNum);
|
return;
|
}
|
|
int start = positions.Min();
|
int end = positions.Max();
|
|
await _taskProcessor.SendPutWithBatchAsync(task, state, "5", start, end);
|
|
// Phase 保持为 4，等 HandlePutFinishedAsync 处理完后再切回 Phase 3
|
_stateManager.TryUpdateStateSafely(ipAddress, state);
|
}
|
return;
|
}
|
|
// 非批次模式或其他阶段不下发指令
|
return;
|
}
|
|
// 非换盘任务：使用原有格式
|
if (state != null && state.IsGroupPallet && task.RobotTaskType == RobotTaskTypeEnum.GroupPallet.GetHashCode())
|
{
|
if (state.IsScanNG)
|
{
|
taskString = $"Putbattery,1";
|
}
|
else
|
{
|
taskString = $"Putbattery,{task.RobotTargetAddress}";
|
}
|
}
|
else
|
taskString = $"Putbattery,{task.RobotTargetAddress}";
|
|
bool result = await _clientManager.SendToClientAsync(ipAddress, taskString);
|
|
if (result)
|
{
|
_logger.LogInformation("HandlePickFinishedStateAsync：下发放货指令成功，指令: {TaskString}，任务号: {TaskNum}", taskString, task.RobotTaskNum);
|
QuartzLogger.Info($"下发放货指令成功，指令: {taskString}", task.RobotRoadway);
|
|
task.RobotTaskState = TaskRobotStatusEnum.RobotExecuting.GetHashCode();
|
|
var stateToUpdate = _stateManager.GetState(ipAddress);
|
if (stateToUpdate != null)
|
{
|
stateToUpdate.CurrentTask = task;
|
|
if (_stateManager.TryUpdateStateSafely(ipAddress, stateToUpdate))
|
{
|
await _robotTaskService.UpdateRobotTaskAsync(task);
|
}
|
}
|
}
|
else
|
{
|
_logger.LogError("HandlePickFinishedStateAsync：下发放货指令失败，指令: {TaskString}，任务号: {TaskNum}", taskString, task.RobotTaskNum);
|
QuartzLogger.Error($"下发放货指令失败，指令: {taskString}", task.RobotRoadway);
|
}
|
}
|
|
/// <summary>
|
/// 处理放货完成后的取货指令
|
/// </summary>
|
/// <remarks>
|
/// 当放货完成后，根据任务类型决定下一步：
|
///
|
/// 1. 如果不是拆盘也不是组盘（普通任务）：
|
/// - 直接发送取货指令
|
///
|
/// 2. 如果是组盘或换盘任务：
|
/// - 生成新的托盘条码
|
/// - 将条码添加到状态中
|
/// - 发送取货指令
|
///
|
/// 组盘任务的条码用于标识新生成的托盘，
|
/// 后续放货时会用到这些条码信息。
|
/// </remarks>
|
/// <param name="task">当前任务</param>
|
/// <param name="ipAddress">机器人 IP 地址</param>
|
private async Task HandlePutFinishedStateAsync(Dt_RobotTask task, string ipAddress)
|
{
|
// 获取最新状态
|
var stateForUpdate = _stateManager.GetState(ipAddress);
|
if (stateForUpdate == null)
|
{
|
_logger.LogWarning("HandlePutFinishedStateAsync：获取状态失败，IP: {IpAddress}", ipAddress);
|
QuartzLogger.Warn($"HandlePutFinishedStateAsync：获取状态失败，IP: {ipAddress}", ipAddress);
|
return;
|
}
|
|
// 如果状态中还没有设置任务类型标志，根据任务类型设置
|
if (!stateForUpdate.IsSplitPallet && !stateForUpdate.IsGroupPallet)
|
{
|
// 判断任务类型并设置相应的标志
|
stateForUpdate.IsSplitPallet = task.RobotTaskType == RobotTaskTypeEnum.SplitPallet.GetHashCode();
|
stateForUpdate.IsGroupPallet = task.RobotTaskType == RobotTaskTypeEnum.GroupPallet.GetHashCode()
|
|| task.RobotTaskType == RobotTaskTypeEnum.ChangePallet.GetHashCode();
|
}
|
|
// 如果是组盘任务
|
if (task.RobotTaskType == RobotTaskTypeEnum.GroupPallet.GetHashCode())
|
{
|
// 检查电池是否已到位
|
if (!stateForUpdate.BatteryArrived)
|
{
|
return;
|
}
|
// 读取线体电芯条码
|
string trayBarcode1 = RobotBarcodeGenerator.GenerateTrayBarcode("DB40.990");
|
string trayBarcode2 = RobotBarcodeGenerator.GenerateTrayBarcode("DB40.1020");
|
|
// 如果条码生成成功
|
if (!string.IsNullOrEmpty(trayBarcode1) && !string.IsNullOrEmpty(trayBarcode2))
|
{
|
if (stateForUpdate.CellBarcode.Contains(trayBarcode1) || stateForUpdate.CellBarcode.Contains(trayBarcode2))
|
{
|
_logger.LogError("HandlePutFinishedStateAsync：读取的托盘条码已存在，可能存在重复，任务号: {TaskNum}", task.RobotTaskNum);
|
QuartzLogger.Error($"读取的托盘条码已存在，可能存在重复", stateForUpdate.RobotCrane.DeviceName);
|
|
// 条码重复，记录错误日志并停止后续操作(后续放货时会用到这些条码信息，供后续放货时使用，调试后可能会取消此逻辑)
|
|
// 发送取货指令 标记扫码NG，放货时不使用这些条码，并放入NG口
|
await _taskProcessor.SendSocketRobotPickAsync(task, stateForUpdate, true);
|
return;
|
}
|
else
|
{
|
_logger.LogInformation("HandlePutFinishedStateAsync：读取的托盘条码唯一，继续执行，任务号: {TaskNum}", task.RobotTaskNum);
|
QuartzLogger.Info($"读取的托盘条码唯一，继续执行", stateForUpdate.RobotCrane.DeviceName);
|
|
// 将条码添加到状态中，供后续放货时使用
|
stateForUpdate.CellBarcode = new List<string>()
|
{
|
trayBarcode1,trayBarcode2
|
};
|
}
|
|
|
// 记录日志：读取托盘条码成功
|
_logger.LogInformation("HandlePutFinishedStateAsync：读取托盘条码成功: {Barcode1}+{Barcode2}，任务号: {TaskNum}", trayBarcode1, trayBarcode2, task.RobotTaskNum);
|
QuartzLogger.Info($"读取托盘条码成功: {trayBarcode1}+{trayBarcode2}", stateForUpdate.RobotCrane.DeviceName);
|
|
// 发送取货指令
|
await _taskProcessor.SendSocketRobotPickAsync(task, stateForUpdate);
|
}
|
else
|
{
|
// 条码读取失败，记录错误日志
|
_logger.LogError("HandlePutFinishedStateAsync：读取托盘条码失败，任务号: {TaskNum}", task.RobotTaskNum);
|
QuartzLogger.Error($"读取托盘条码失败", stateForUpdate.RobotCrane.DeviceName);
|
|
|
// 发送取货指令 标记扫码NG，放货时不使用这些条码，并放入NG口
|
await _taskProcessor.SendSocketRobotPickAsync(task, stateForUpdate, true);
|
return;
|
}
|
}
|
else if (task.RobotTaskType == RobotTaskTypeEnum.ChangePallet.GetHashCode())
|
{
|
const int targetTotal = 48;
|
// 换盘任务进入批次模式，分阶段处理正常电芯和假电芯 （机器人任务总数）
|
int targetNormalCount = task.RobotTaskTotalNum;
|
// 当前已完成数量（取货完成的数量），初始为状态中的 RobotTaskTotalNum，后续根据取货完成的数量动态更新
|
int currentCompletedCount = stateForUpdate.RobotTaskTotalNum;
|
|
// 判断流向（null-safe）
|
bool isFlowA = task.RobotSourceAddressLineCode is "11001" or "11010";
|
|
// 目标数量为48：直接走原有逻辑，不进入批次模式
|
if (targetNormalCount == targetTotal)
|
{
|
await _taskProcessor.SendSocketRobotPickAsync(task, stateForUpdate);
|
return;
|
}
|
|
// 初始化批次模式
|
if (stateForUpdate.ChangePalletPhase == 0)
|
{
|
stateForUpdate.ChangePalletPhase = 1;
|
stateForUpdate.CurrentBatchIndex = 1;
|
_logger.LogInformation("HandlePutFinishedStateAsync：换盘任务进入批次模式，任务号: {TaskNum}，流向: {Flow}",
|
task.RobotTaskNum, isFlowA ? "A" : "B");
|
_stateManager.TryUpdateStateSafely(ipAddress, stateForUpdate);
|
}
|
|
// ==================== Phase 1: 取正常电芯（两流向相同）====================
|
if (stateForUpdate.ChangePalletPhase == 1)
|
{
|
int remainingNormal = targetNormalCount - currentCompletedCount;
|
if (remainingNormal <= 0)
|
{
|
// 正常电芯取完，切换到 Phase 3 取假电芯
|
stateForUpdate.ChangePalletPhase = 3;
|
stateForUpdate.CurrentBatchIndex = 1; // 假电芯批次从头开始
|
_logger.LogInformation("HandlePutFinishedStateAsync：正常电芯取完，切换到Phase 3取假电芯，任务号: {TaskNum}", task.RobotTaskNum);
|
_stateManager.TryUpdateStateSafely(ipAddress, stateForUpdate);
|
return;
|
}
|
|
int pickCount = Math.Min(4, remainingNormal);
|
var (start, end) = _taskProcessor.BuildBatchRange(stateForUpdate.CurrentBatchIndex, pickCount);
|
|
await _taskProcessor.SendPickWithBatchAsync(task, stateForUpdate, task.RobotSourceAddress, start, end);
|
|
stateForUpdate.CurrentBatchIndex += pickCount;
|
// 发完 Pick 后切换到 Phase=2，等 PickFinished 触发 HandlePickFinishedStateAsync 下发放货指令
|
stateForUpdate.ChangePalletPhase = 2;
|
_stateManager.TryUpdateStateSafely(ipAddress, stateForUpdate);
|
return;
|
}
|
|
// ==================== Phase 3: 处理假电芯（流向A/B 分叉），自己循环 ====================
|
if (stateForUpdate.ChangePalletPhase == 3)
|
{
|
int fakeCount = targetTotal - targetNormalCount;
|
int completedFake = Math.Max(0, currentCompletedCount - targetNormalCount);
|
int remainingFake = fakeCount - completedFake;
|
|
if (remainingFake <= 0)
|
{
|
// 假电芯全部处理完，切换到 Phase 5 等待入库
|
stateForUpdate.ChangePalletPhase = 5;
|
_logger.LogInformation("HandlePutFinishedStateAsync：假电芯处理完毕，切换到Phase 5等待入库，任务号: {TaskNum}", task.RobotTaskNum);
|
_stateManager.TryUpdateStateSafely(ipAddress, stateForUpdate);
|
return;
|
}
|
|
if (isFlowA)
|
{
|
// 流向A：从5号位取假电芯
|
var positions = _taskProcessor.GetNextAvailableFakeBatteryPositions(Math.Min(4, remainingFake));
|
if (positions.Count == 0)
|
{
|
_logger.LogError("HandlePutFinishedStateAsync：无可用假电芯点位，任务号: {TaskNum}", task.RobotTaskNum);
|
return;
|
}
|
await _taskProcessor.SendSocketRobotFakeBatteryPickAsync(task, stateForUpdate, positions);
|
}
|
else
|
{
|
// 流向B：从源地址取假电芯
|
int pickCount = Math.Min(4, remainingFake);
|
var (start, end) = _taskProcessor.BuildBatchRange(stateForUpdate.CurrentBatchIndex, pickCount);
|
await _taskProcessor.SendPickWithBatchAsync(task, stateForUpdate, task.RobotSourceAddress, start, end);
|
stateForUpdate.CurrentBatchIndex += pickCount;
|
}
|
|
// 发完 Pick 后切换到 Phase=4，等 PickFinished 触发 HandlePickFinishedStateAsync 下发放货指令
|
stateForUpdate.ChangePalletPhase = 4;
|
_stateManager.TryUpdateStateSafely(ipAddress, stateForUpdate);
|
return;
|
}
|
|
// ==================== Phase 5: 完成入库（allputfinished / allpickfinished 触发）====================
|
if (stateForUpdate.ChangePalletPhase == 5)
|
{
|
// Phase 5 由 allpickfinished/allputfinished 触发入库，本方法不再下发指令
|
return;
|
}
|
}
|
else
|
{
|
// 非组盘任务，直接发送取货指令
|
await _taskProcessor.SendSocketRobotPickAsync(task, stateForUpdate);
|
}
|
}
|
}
|
}
|
