package com.zy.common.utils;
|
|
import com.baomidou.mybatisplus.mapper.EntityWrapper;
|
import com.zy.asrs.entity.BasShuttle;
|
import com.zy.asrs.service.BasShuttleService;
|
import com.zy.asrs.utils.Utils;
|
import com.zy.common.model.NavigateNode;
|
import com.zy.core.News;
|
import com.zy.core.enums.ShuttleRunDirection;
|
import com.zy.core.model.command.ShuttleAssignCommand;
|
import com.zy.core.model.command.ShuttleCommand;
|
import com.zy.core.model.protocol.ShuttleProtocol;
|
import com.zy.core.thread.ShuttleThread;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Component;
|
|
import java.util.ArrayList;
|
import java.util.List;
|
import java.util.Optional;
|
|
/**
|
* 四向车操作工具类
|
* 此工具类只要作用于获取小车的各类命令List，移动命令、搬运货物命令、复位命令等。如命令为移动命令，还将返回行走路径List
|
*/
|
@Component
|
public class ShuttleOperaUtils {
|
|
@Autowired
|
private BasShuttleService basShuttleService;
|
@Autowired
|
private NavigateUtils navigateUtils;
|
@Autowired
|
private NavigateMapUtils navigateMapUtils;
|
|
public synchronized List<ShuttleCommand> getStartToTargetCommands(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
|
return getStartToTargetCommands(startLocNo, endLocNo, mapType, null, assignCommand, shuttleThread);
|
}
|
|
public synchronized List<ShuttleCommand> getStartToTargetCommands(String startLocNo, String endLocNo, Integer mapType, List<int[]> whites, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
|
ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
|
if (shuttleProtocol == null) {
|
return null;
|
}
|
Integer shuttleNo = shuttleProtocol.getShuttleNo();
|
//获取小车移动速度
|
Integer runSpeed = Optional.ofNullable(basShuttleService.selectOne(new EntityWrapper<BasShuttle>().eq("shuttle_no", shuttleNo)).getRunSpeed()).orElse(1000);
|
List<NavigateNode> nodeList = navigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startLocNo)), null);
|
if (nodeList == null) {
|
News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
|
return null;
|
}
|
|
List<NavigateNode> allNode = new ArrayList<>();
|
for (NavigateNode node : nodeList) {
|
allNode.add(node.clone());
|
}
|
|
List<ShuttleCommand> commands = new ArrayList<>();
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = navigateUtils.getSectionPath(nodeList);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = navigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
//通过xy坐标小车二维码
|
String startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
|
//通过xy坐标小车二维码
|
String distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
|
//获取移动命令
|
ShuttleCommand command = shuttleThread.getMoveCommand(assignCommand.getDeviceTaskNo(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed, nodes);
|
command.setNodes(nodes);//将行走节点添加到每一步命令中
|
commands.add(command);
|
}
|
|
assignCommand.setNodes(allNode);//当前任务所占用的节点list
|
|
return commands;
|
}
|
|
public synchronized List<ShuttleCommand> shuttleInOutLiftCommand(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
|
NavigateNode startNode = NavigatePositionConvert.locNoToNode(startLocNo);
|
NavigateNode endNode = NavigatePositionConvert.locNoToNode(endLocNo);
|
List<NavigateNode> unlockPath = new ArrayList<>();
|
unlockPath.add(startNode);
|
unlockPath.add(endNode);
|
|
ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
|
if (shuttleProtocol == null) {
|
return null;
|
}
|
|
Integer shuttleNo = shuttleProtocol.getShuttleNo();
|
//所使用的路径进行锁定/解锁
|
boolean lockResult = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(endLocNo), shuttleProtocol.getShuttleNo(), unlockPath, false);//所使用的路径进行解锁
|
if (!lockResult) {
|
News.error("{} dash {} can't find unlock path!", startLocNo, endLocNo);
|
return null;//解锁失败
|
}
|
|
//获取小车移动速度
|
Integer runSpeed = Optional.ofNullable(basShuttleService.selectOne(new EntityWrapper<BasShuttle>().eq("shuttle_no", shuttleNo)).getRunSpeed()).orElse(1000);
|
List<NavigateNode> nodeList = navigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startLocNo)), null);
|
if (nodeList == null) {
|
News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
|
return null;
|
}
|
|
List<NavigateNode> allNode = new ArrayList<>();
|
for (NavigateNode node : nodeList) {
|
allNode.add(node.clone());
|
}
|
|
List<ShuttleCommand> commands = new ArrayList<>();
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = navigateUtils.getSectionPath(nodeList);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = navigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
//通过xy坐标小车二维码
|
String startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
|
//通过xy坐标小车二维码
|
String distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
|
//获取移动命令
|
ShuttleCommand command = shuttleThread.getMoveCommand(assignCommand.getDeviceTaskNo(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed, nodes);
|
command.setNodes(nodes);//将行走节点添加到每一步命令中
|
commands.add(command);
|
}
|
|
assignCommand.setNodes(allNode);//当前任务所占用的节点list
|
|
return commands;
|
}
|
|
/**
|
* 获取充电命令
|
*/
|
public synchronized List<ShuttleCommand> getShuttleChargeCommand(ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread, Boolean openCharge) {
|
List<ShuttleCommand> commands = new ArrayList<>();
|
|
//获取充电命令
|
ShuttleCommand command = shuttleThread.getChargeCommand(assignCommand.getDeviceTaskNo(), openCharge);
|
commands.add(command);
|
return commands;
|
}
|
|
/**
|
* 获取顶升命令
|
*/
|
public synchronized List<ShuttleCommand> getShuttleLiftCommand(ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread, Boolean lift) {
|
List<ShuttleCommand> commands = new ArrayList<>();
|
|
//获取顶升命令
|
ShuttleCommand command = shuttleThread.getLiftCommand(assignCommand.getDeviceTaskNo(), lift);
|
commands.add(command);
|
return commands;
|
}
|
|
// private boolean checkSimilarityPath(Motion motion, ShuttleAssignCommand assignCommand) {
|
// String movePath = motion.getMovePath();
|
// if (Cools.isEmpty(movePath)) {
|
// return false;
|
// }
|
//
|
// Double similarityRef = 0.9D;
|
// Dict similarityRefDict = dictService.getOne(new LambdaQueryWrapper<Dict>()
|
// .eq(Dict::getFlag, "similarityRef")
|
// .eq(Dict::getStatus, 1));
|
// if (similarityRefDict != null) {
|
// similarityRef = Double.parseDouble(similarityRefDict.getValue());
|
// }
|
//
|
// List<NavigateNode> originPath = JSON.parseArray(movePath, NavigateNode.class);
|
// List<NavigateNode> finalPath = assignCommand.getNodes();
|
//
|
// if (finalPath == null) {
|
// return false;
|
// }
|
//
|
// Double similarity = navigateUtils.similarityPath(originPath, finalPath);
|
// if (similarity <= similarityRef) {
|
// Object object = redisUtil.get(DeviceRedisConstant.SIMILARITY_TIMES + motion.getTaskNo());
|
// if (object == null) {
|
// redisUtil.set(DeviceRedisConstant.SIMILARITY_TIMES + motion.getTaskNo(), System.currentTimeMillis(), 60 * 60 * 24);
|
// }else {
|
// long similarityTimeoutRef = 20L;//默认超时20s
|
// Dict similarityTimeoutDict = dictService.getOne(new LambdaQueryWrapper<Dict>()
|
// .eq(Dict::getFlag, "similarityTimeout")
|
// .eq(Dict::getStatus, 1));
|
// if (similarityTimeoutDict != null) {
|
// similarityTimeoutRef = Long.parseLong(similarityTimeoutDict.getValue());
|
// }
|
//
|
// long recordTime = Long.parseLong(object.toString());
|
// if (System.currentTimeMillis() - recordTime > (60 * similarityTimeoutRef)) {
|
// //超时，直接放行
|
// return true;
|
// }
|
// }
|
// News.error("{} dash {} path similarity mismatch!", motion.getOrigin(), motion.getTarget());
|
// return false;
|
// }
|
//
|
// return true;
|
// }
|
|
// /**
|
// * 计算并获取小车从起点——中间点——目标点之间搬运货物动作命令
|
// * @param shuttleNo 小车号
|
// * @param wrkNo 工作号
|
// * @param startPoint 起点（小车当前位置）
|
// * @param targetPoint 目标点（货物目标位置）
|
// */
|
// public static synchronized ShuttleOperaResult getShuttleTransportCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint) {
|
// //行走路径
|
// ArrayList<NavigateNode> nodes = new ArrayList<>();
|
// //命令集合
|
// ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
|
//
|
// //计算起点到目标点命令
|
// ShuttleOperaResult result = getStartToTargetCommands(shuttleNo, wrkNo, startPoint, targetPoint, NavigationMapType.DFX.id);
|
// if (result == null) {
|
// //计算结果必须不为空，否则计算失败
|
// return null;
|
// }
|
// nodes.addAll(result.getNodes());
|
// //起点位置下发一条顶升命令将货物进行搬运
|
// commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, true));
|
// commands.addAll(result.getCommands());//起点到目标点移动命令
|
// //当小车行走到目标点后，需要下发一条下降命令将货物放置
|
// commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, false));
|
// return result(commands, nodes);
|
// }
|
//
|
// /**
|
// * 计算并获取小车从起点——中间点——目标点之间搬运货物动作命令
|
// * @param shuttleNo 小车号
|
// * @param wrkNo 工作号
|
// * @param startPoint 起点（小车当前位置）
|
// * @param middlePoint 中间点（货物位置）
|
// * @param targetPoint 目标点（货物目标位置）
|
// */
|
// public static synchronized ShuttleOperaResult getShuttleTransportCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String middlePoint, String targetPoint) {
|
// //行走路径
|
// ArrayList<NavigateNode> nodes = new ArrayList<>();
|
// //命令集合
|
// ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
|
//
|
// if (!startPoint.equals(middlePoint)) {//起点和中间点不一致，需要计算起点到中间点路径
|
// //计算起点到中间点命令
|
// ShuttleOperaResult result1 = getStartToTargetCommands(shuttleNo, wrkNo, startPoint, middlePoint, NavigationMapType.NORMAL.id);
|
// if (result1 == null) {
|
// //计算结果必须不为空，否则计算失败
|
// return null;
|
// }
|
// nodes.addAll(result1.getNodes());
|
// commands.addAll(result1.getCommands());
|
// }
|
//
|
// //计算中间点到目标点命令
|
// ShuttleOperaResult result2 = getStartToTargetCommands(shuttleNo, wrkNo, middlePoint, targetPoint, NavigationMapType.DFX.id);
|
// if (result2 == null) {
|
// //计算结果必须不为空，否则计算失败
|
// return null;
|
// }
|
// nodes.addAll(result2.getNodes());
|
// //当小车行走到中间点后，需要下发一条顶升命令将货物进行搬运
|
// commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, true));
|
// commands.addAll(result2.getCommands());//中间点到目标点移动命令
|
// //当小车行走到目标点后，需要下发一条下降命令将货物放置
|
// commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, false));
|
// return result(commands, nodes);
|
// }
|
//
|
// /**
|
// * 获取起点到目标点行走命令
|
// */
|
// public synchronized ShuttleOperaResult getStartToTargetCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint, Integer mapType) {
|
// NavigateMapUtils navigateMapUtils = SpringUtils.getBean(NavigateMapUtils.class);
|
// //计算起点到目标点行走节点
|
// List<NavigateNode> calc = NavigateUtils.calc(startPoint, targetPoint, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startPoint)), null);
|
// if (calc == null) {
|
// return null;
|
// }
|
//
|
// //命令集合
|
// ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
|
// List<NavigateNode> allNode = new ArrayList<>();
|
//
|
// //获取分段路径
|
// ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
|
// //将每一段路径分成command指令
|
// for (ArrayList<NavigateNode> nodes : data) {
|
// //开始路径
|
// NavigateNode startPath = nodes.get(0);
|
// //目标路径
|
// NavigateNode targetPath = nodes.get(nodes.size() - 1);
|
// //获取移动命令
|
// NyShuttleHttpCommand moveCommand = NyHttpUtils.getMoveCommand(shuttleNo, wrkNo, startPath, targetPath);
|
// moveCommand.setNodes(nodes);//将行走节点添加到每一步命令中
|
// commands.add(moveCommand);
|
//
|
// allNode.addAll(nodes);
|
// }
|
//
|
// boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(startPoint), shuttleNo, allNode, true);//锁定路径
|
// if (!result) {
|
// return null;//路径锁定失败
|
// }
|
// return result(commands, calc);
|
// }
|
//
|
// /**
|
// * 获取起点到目标点行走命令(可传白名单)
|
// */
|
// public static synchronized ShuttleOperaResult getStartToTargetCommandsByWhites(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint, Integer mapType, List<int[]> whites) {
|
// NavigateMapUtils navigateMapUtils = SpringUtils.getBean(NavigateMapUtils.class);
|
// //计算起点到目标点行走节点
|
// List<NavigateNode> calc = NavigateUtils.calc(startPoint, targetPoint, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startPoint)), whites);
|
// if (calc == null) {
|
// return null;
|
// }
|
//
|
// //命令集合
|
// ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
|
// List<NavigateNode> allNode = new ArrayList<>();
|
//
|
// //获取分段路径
|
// ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
|
// //将每一段路径分成command指令
|
// for (ArrayList<NavigateNode> nodes : data) {
|
// //开始路径
|
// NavigateNode startPath = nodes.get(0);
|
// //目标路径
|
// NavigateNode targetPath = nodes.get(nodes.size() - 1);
|
// //获取移动命令
|
// NyShuttleHttpCommand moveCommand = NyHttpUtils.getMoveCommand(shuttleNo, wrkNo, startPath, targetPath);
|
// moveCommand.setNodes(nodes);//将行走节点添加到每一步命令中
|
// commands.add(moveCommand);
|
//
|
// allNode.addAll(nodes);
|
// }
|
//
|
// //锁定路径时剔除白名单节点
|
// ArrayList<NavigateNode> nodes = new ArrayList<>();
|
// for (NavigateNode node : allNode) {
|
// boolean flag = false;
|
// for (int[] white : whites) {
|
// if (node.getX() == white[0] && node.getY() == white[1]) {
|
// flag = true;//存在白名单节点
|
// break;//跳过白名单节点
|
// }
|
// }
|
//
|
// if (!flag) {
|
// nodes.add(node);
|
// }
|
// }
|
// boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(startPoint), shuttleNo, nodes, true);//锁定路径
|
// if (!result) {
|
// return null;//路径锁定失败
|
// }
|
// return result(commands, calc);
|
// }
|
//
|
// //返回结果集
|
// public static ShuttleOperaResult result(List<NyShuttleHttpCommand> commands, List<NavigateNode> nodes) {
|
// return new ShuttleOperaResult(commands, nodes);
|
// }
|
|
}
|
