| New file |
| | |
| | | package com.zy.acs.manager.core.service.hik; |
| | | |
| | | import com.alibaba.fastjson.JSON; |
| | | import com.zy.acs.common.constant.RedisConstant; |
| | | import com.zy.acs.common.hk.action.HkAction; |
| | | import com.zy.acs.common.hk.action.HkActionParameter; |
| | | import com.zy.acs.common.hk.action.type.HkActionType; |
| | | import com.zy.acs.common.hk.action.type.HkBlockingType; |
| | | import com.zy.acs.common.hk.order.HkNodePosition; |
| | | import com.zy.acs.common.hk.order.HkOrderDown; |
| | | import com.zy.acs.common.hk.order.HkOrderEdge; |
| | | import com.zy.acs.common.hk.order.HkOrderMessage; |
| | | import com.zy.acs.common.hk.order.HkOrderNode; |
| | | import com.zy.acs.common.utils.RedisSupport; |
| | | import com.zy.acs.framework.common.SnowflakeIdWorker; |
| | | import com.zy.acs.framework.exception.CoolException; |
| | | import com.zy.acs.manager.common.config.HikOrderProperties; |
| | | import com.zy.acs.manager.manager.entity.Action; |
| | | import com.zy.acs.manager.manager.entity.AgvModel; |
| | | import com.zy.acs.manager.manager.entity.Code; |
| | | import com.zy.acs.manager.manager.enums.ActionTypeType; |
| | | import com.zy.acs.manager.manager.service.CodeService; |
| | | import lombok.AllArgsConstructor; |
| | | import lombok.extern.slf4j.Slf4j; |
| | | import org.springframework.beans.factory.annotation.Autowired; |
| | | import org.springframework.stereotype.Service; |
| | | import org.springframework.util.StringUtils; |
| | | |
| | | import java.time.Instant; |
| | | import java.util.ArrayList; |
| | | import java.util.List; |
| | | import java.util.Locale; |
| | | import java.util.Objects; |
| | | |
| | | @Slf4j |
| | | @Service |
| | | public class HikOrderPublishService { |
| | | |
| | | @Autowired |
| | | private HikOrderProperties hikOrderProperties; |
| | | @Autowired |
| | | private SnowflakeIdWorker snowflakeIdWorker; |
| | | @Autowired |
| | | private CodeService codeService; |
| | | |
| | | private final RedisSupport redis = RedisSupport.defaultRedisSupport; |
| | | |
| | | public boolean support(AgvModel agvModel) { |
| | | if (agvModel == null || !StringUtils.hasText(agvModel.getProtocol())) { |
| | | return false; |
| | | } |
| | | return agvModel.getProtocol().toLowerCase(Locale.ROOT).contains("hik"); |
| | | } |
| | | |
| | | public void publish(String actionGroupId, String agvNo, AgvModel agvModel, List<Action> actionList) { |
| | | HkOrderMessage orderMessage = buildOrderMessage(actionGroupId, agvNo, agvModel, actionList); |
| | | HkOrderDown orderDown = new HkOrderDown(); |
| | | orderDown.setAgvNo(agvNo); |
| | | orderDown.setActionGroupId(actionGroupId); |
| | | orderDown.setOrderMessage(orderMessage); |
| | | redis.push(RedisConstant.HK_AGV_PATH_DOWN_FLAG, orderDown); |
| | | log.info("push hik order to redis, agvNo={}, actionGroupId={}, nodeCount={}, edgeCount={}, redisKey={}", |
| | | agvNo, |
| | | actionGroupId, |
| | | orderMessage.getNodes().size(), |
| | | orderMessage.getEdges().size(), |
| | | RedisConstant.HK_AGV_PATH_DOWN_FLAG); |
| | | } |
| | | |
| | | private HkOrderMessage buildOrderMessage(String actionGroupId, String agvNo, AgvModel agvModel, List<Action> actionList) { |
| | | if (!StringUtils.hasText(actionGroupId)) { |
| | | throw new CoolException("actionGroupId can not be blank"); |
| | | } |
| | | if (!StringUtils.hasText(agvNo)) { |
| | | throw new CoolException("agvNo can not be blank"); |
| | | } |
| | | if (actionList == null || actionList.isEmpty()) { |
| | | throw new CoolException("actionList can not be empty"); |
| | | } |
| | | |
| | | Action firstAction = findFirstActionWithCode(actionList); |
| | | if (firstAction == null) { |
| | | throw new CoolException("hik order adaptation failed: no start code found"); |
| | | } |
| | | |
| | | HkOrderMessage orderMessage = new HkOrderMessage(); |
| | | orderMessage.setHeaderId(snowflakeIdWorker.nextId()); |
| | | orderMessage.setTimestamp(Instant.now().toString()); |
| | | orderMessage.setVersion(hikOrderProperties.getMajorVersion()); |
| | | orderMessage.setManufacturer(hikOrderProperties.getManufacturer()); |
| | | orderMessage.setSerialNumber(agvNo); |
| | | orderMessage.setOrderId(actionGroupId); |
| | | orderMessage.setOrderUpdateId(0L); |
| | | orderMessage.setZoneSetId(hikOrderProperties.getZoneSetId()); |
| | | |
| | | List<HkOrderNode> nodes = new ArrayList<>(); |
| | | List<HkOrderEdge> edges = new ArrayList<>(); |
| | | |
| | | long sequenceId = 0L; |
| | | int nodeIndex = 0; |
| | | int edgeIndex = 0; |
| | | |
| | | NodeCursor currentNode = createNode(actionGroupId, firstAction.getCode(), nodeIndex++, sequenceId++); |
| | | nodes.add(currentNode.node); |
| | | |
| | | Double currentTheta = currentNode.node.getNodePosition().getTheta(); |
| | | for (int i = 0; i < actionList.size(); i++) { |
| | | Action action = actionList.get(i); |
| | | ActionTypeType actionType = resolveActionType(action); |
| | | String actionCode = action.getCode(); |
| | | |
| | | if (StringUtils.hasText(actionCode) && !Objects.equals(actionCode, currentNode.code)) { |
| | | throw new CoolException("hik order adaptation failed: action code jump without move, groupId=" |
| | | + actionGroupId + ", currentCode=" + currentNode.code + ", actionCode=" + actionCode); |
| | | } |
| | | |
| | | switch (actionType) { |
| | | case TurnCorner: |
| | | currentTheta = parseThetaRadians(action.getParams()); |
| | | currentNode.node.getNodePosition().setTheta(currentTheta); |
| | | break; |
| | | case StraightAheadTurnable: |
| | | case StraightAheadUnturnable: |
| | | case StraightBackTurnable: |
| | | case StraightBackUnturnable: |
| | | String endCode = findNextMoveEndCode(actionList, i, currentNode.code); |
| | | Double edgeTheta = currentTheta != null ? currentTheta : calculateTheta(currentNode.code, endCode); |
| | | HkOrderEdge edge = createEdge(actionGroupId, edgeIndex++, sequenceId++, currentNode, endCode, edgeTheta, action, agvModel); |
| | | edges.add(edge); |
| | | |
| | | NodeCursor nextNode = createNode(actionGroupId, endCode, nodeIndex++, sequenceId++); |
| | | nextNode.node.getNodePosition().setTheta(edgeTheta); |
| | | nodes.add(nextNode.node); |
| | | |
| | | currentNode = nextNode; |
| | | currentTheta = edgeTheta; |
| | | break; |
| | | case FinishPath: |
| | | break; |
| | | default: |
| | | currentNode.node.getActions().add(buildNodeAction(action, actionType, currentTheta)); |
| | | break; |
| | | } |
| | | } |
| | | |
| | | orderMessage.setNodes(nodes); |
| | | orderMessage.setEdges(edges); |
| | | return orderMessage; |
| | | } |
| | | |
| | | private Action findFirstActionWithCode(List<Action> actionList) { |
| | | for (Action action : actionList) { |
| | | if (StringUtils.hasText(action.getCode())) { |
| | | return action; |
| | | } |
| | | } |
| | | return null; |
| | | } |
| | | |
| | | private ActionTypeType resolveActionType(Action action) { |
| | | ActionTypeType actionType = ActionTypeType.get(action.getActionTypeEl()); |
| | | if (actionType == null) { |
| | | throw new CoolException("unsupported action type, actionId=" + action.getId()); |
| | | } |
| | | return actionType; |
| | | } |
| | | |
| | | private NodeCursor createNode(String actionGroupId, String code, int nodeIndex, long sequenceId) { |
| | | Code codeEntity = codeService.getCacheByData(code); |
| | | if (codeEntity == null) { |
| | | throw new CoolException("hik order adaptation failed: code not found, code=" + code); |
| | | } |
| | | |
| | | HkOrderNode node = new HkOrderNode(); |
| | | node.setNodeId(buildId(actionGroupId, "N", nodeIndex, code)); |
| | | node.setSequenceId(sequenceId); |
| | | node.setNodeDescription(code); |
| | | node.setReleased(Boolean.TRUE); |
| | | node.setAllowedDeviationXY(hikOrderProperties.getDefaultAllowedDeviationXY()); |
| | | node.setAllowedDeviationTheta(hikOrderProperties.getDefaultAllowedDeviationTheta()); |
| | | node.setMapId(hikOrderProperties.getMapId()); |
| | | node.setMapDescription(hikOrderProperties.getMapDescription()); |
| | | node.setActions(new ArrayList<>()); |
| | | node.setNodePosition(buildNodePosition(codeEntity)); |
| | | return new NodeCursor(code, node); |
| | | } |
| | | |
| | | private HkNodePosition buildNodePosition(Code codeEntity) { |
| | | HkNodePosition nodePosition = new HkNodePosition(); |
| | | nodePosition.setX(scaleCoordinate(codeEntity.getX())); |
| | | nodePosition.setY(scaleCoordinate(codeEntity.getY())); |
| | | nodePosition.setTheta(null); |
| | | return nodePosition; |
| | | } |
| | | |
| | | private HkOrderEdge createEdge(String actionGroupId, |
| | | int edgeIndex, |
| | | long sequenceId, |
| | | NodeCursor startNode, |
| | | String endCode, |
| | | Double edgeTheta, |
| | | Action action, |
| | | AgvModel agvModel) { |
| | | HkOrderEdge edge = new HkOrderEdge(); |
| | | edge.setEdgeId(buildId(actionGroupId, "E", edgeIndex, startNode.code + "_" + endCode)); |
| | | edge.setSequenceId(sequenceId); |
| | | edge.setEdgeDescription(startNode.code + "->" + endCode); |
| | | edge.setReleased(Boolean.TRUE); |
| | | edge.setStartNodeId(startNode.node.getNodeId()); |
| | | edge.setEndNodeId(buildId(actionGroupId, "N", edgeIndex + 1, endCode)); |
| | | edge.setMaxSpeed(resolveMaxSpeed(action, agvModel)); |
| | | edge.setOrientation(edgeTheta); |
| | | edge.setOrientationType("GLOBAL"); |
| | | edge.setRotationAllowed(Boolean.FALSE); |
| | | edge.setActions(new ArrayList<>()); |
| | | return edge; |
| | | } |
| | | |
| | | private String findNextMoveEndCode(List<Action> actionList, int currentIndex, String currentCode) { |
| | | for (int i = currentIndex + 1; i < actionList.size(); i++) { |
| | | String code = actionList.get(i).getCode(); |
| | | if (!StringUtils.hasText(code)) { |
| | | continue; |
| | | } |
| | | if (!Objects.equals(code, currentCode)) { |
| | | return code; |
| | | } |
| | | } |
| | | throw new CoolException("hik order adaptation failed: move action missing end code, startCode=" + currentCode); |
| | | } |
| | | |
| | | private HkAction buildNodeAction(Action action, ActionTypeType actionType, Double currentTheta) { |
| | | HkAction hkAction = new HkAction(); |
| | | hkAction.setActionId("A" + action.getId()); |
| | | hkAction.setActionDescription(actionType.desc); |
| | | hkAction.setBlockingType(HkBlockingType.HARD); |
| | | hkAction.setActionType(resolveHkActionType(actionType)); |
| | | |
| | | List<HkActionParameter> parameters = new ArrayList<>(); |
| | | addParameter(parameters, "sourceActionType", actionType.name()); |
| | | addParameter(parameters, "sourceActionId", action.getId()); |
| | | |
| | | switch (actionType) { |
| | | case ReadyTakeFromShelvesLoc: |
| | | case ReadyTakeFromConveyorSta: |
| | | case ReadyReleaseToShelvesLoc: |
| | | case ReadyReleaseToConveyorSta: |
| | | addParameter(parameters, "loadType", resolveLoadType(actionType)); |
| | | addParameter(parameters, "actuatorDirection", resolveDirection(action.getVal())); |
| | | HeightDepthParam heightDepthParam = parseHeightDepth(action.getParams()); |
| | | addMetricParameter(parameters, "height", heightDepthParam.height); |
| | | addMetricParameter(parameters, "depth", heightDepthParam.depth); |
| | | break; |
| | | case ReadyTakeFromAgvSite: |
| | | case ReadyReleaseToAgvSite: |
| | | addParameter(parameters, "loadType", "AGV_SITE"); |
| | | addParameter(parameters, "level", toInteger(action.getVal())); |
| | | addMetricParameter(parameters, "height", parseDouble(action.getParams())); |
| | | break; |
| | | case DockingCharge: |
| | | case UndockingCharge: |
| | | break; |
| | | case LoadPlatformLift: |
| | | addMetricParameter(parameters, "height", parseDouble(action.getParams())); |
| | | if (currentTheta != null) { |
| | | addParameter(parameters, "angle", currentTheta); |
| | | } |
| | | break; |
| | | default: |
| | | throw new CoolException("hik order adaptation failed: unsupported node action type=" + actionType); |
| | | } |
| | | |
| | | hkAction.setActionParameters(parameters); |
| | | return hkAction; |
| | | } |
| | | |
| | | private HkActionType resolveHkActionType(ActionTypeType actionType) { |
| | | switch (actionType) { |
| | | case ReadyTakeFromShelvesLoc: |
| | | case ReadyTakeFromConveyorSta: |
| | | case ReadyTakeFromAgvSite: |
| | | return HkActionType.PICK; |
| | | case ReadyReleaseToShelvesLoc: |
| | | case ReadyReleaseToConveyorSta: |
| | | case ReadyReleaseToAgvSite: |
| | | return HkActionType.DROP; |
| | | case DockingCharge: |
| | | return HkActionType.START_CHARGING; |
| | | case UndockingCharge: |
| | | return HkActionType.STOP_CHARGING; |
| | | case LoadPlatformLift: |
| | | return HkActionType.ROTATE_LOAD_LIFT; |
| | | default: |
| | | throw new CoolException("hik action mapping not found: " + actionType); |
| | | } |
| | | } |
| | | |
| | | private String resolveLoadType(ActionTypeType actionType) { |
| | | switch (actionType) { |
| | | case ReadyTakeFromShelvesLoc: |
| | | case ReadyReleaseToShelvesLoc: |
| | | return "SHELF"; |
| | | case ReadyTakeFromConveyorSta: |
| | | case ReadyReleaseToConveyorSta: |
| | | return "CONVEYOR"; |
| | | default: |
| | | return "UNKNOWN"; |
| | | } |
| | | } |
| | | |
| | | private String resolveDirection(Double val) { |
| | | if (val == null) { |
| | | return null; |
| | | } |
| | | int direct = val.intValue(); |
| | | if (direct == 1) { |
| | | return "LEFT"; |
| | | } |
| | | if (direct == 2) { |
| | | return "RIGHT"; |
| | | } |
| | | if (direct == 3) { |
| | | return "FORWARD"; |
| | | } |
| | | return String.valueOf(direct); |
| | | } |
| | | |
| | | private HeightDepthParam parseHeightDepth(String params) { |
| | | com.zy.acs.common.domain.HeightDepthDto heightDepthDto = JSON.parseObject(params, com.zy.acs.common.domain.HeightDepthDto.class); |
| | | if (heightDepthDto == null) { |
| | | throw new CoolException("hik order adaptation failed: invalid height/depth params"); |
| | | } |
| | | return new HeightDepthParam(heightDepthDto.getHeight(), heightDepthDto.getDepth()); |
| | | } |
| | | |
| | | private Double parseThetaRadians(String params) { |
| | | return Math.toRadians(parseDouble(params)); |
| | | } |
| | | |
| | | private Double calculateTheta(String startCode, String endCode) { |
| | | Code start = codeService.getCacheByData(startCode); |
| | | Code end = codeService.getCacheByData(endCode); |
| | | if (start == null || end == null) { |
| | | throw new CoolException("hik order adaptation failed: code position missing"); |
| | | } |
| | | return Math.atan2(end.getY() - start.getY(), end.getX() - start.getX()); |
| | | } |
| | | |
| | | private Double resolveMaxSpeed(Action action, AgvModel agvModel) { |
| | | if (agvModel != null && agvModel.getTravelSpeed() != null) { |
| | | return agvModel.getTravelSpeed() * hikOrderProperties.getMetricScale(); |
| | | } |
| | | if (action.getVal() != null) { |
| | | return action.getVal(); |
| | | } |
| | | return null; |
| | | } |
| | | |
| | | private Double scaleCoordinate(Double value) { |
| | | if (value == null) { |
| | | return null; |
| | | } |
| | | return value * hikOrderProperties.getCoordinateScale(); |
| | | } |
| | | |
| | | private void addMetricParameter(List<HkActionParameter> parameters, String key, Number value) { |
| | | if (value == null) { |
| | | return; |
| | | } |
| | | addParameter(parameters, key, value.doubleValue() * hikOrderProperties.getMetricScale()); |
| | | } |
| | | |
| | | private void addParameter(List<HkActionParameter> parameters, String key, Object value) { |
| | | if (value == null) { |
| | | return; |
| | | } |
| | | HkActionParameter parameter = new HkActionParameter(); |
| | | parameter.setKey(key); |
| | | parameter.setValue(value); |
| | | parameters.add(parameter); |
| | | } |
| | | |
| | | private Double parseDouble(String value) { |
| | | if (!StringUtils.hasText(value)) { |
| | | return null; |
| | | } |
| | | return Double.parseDouble(value); |
| | | } |
| | | |
| | | private Integer toInteger(Double value) { |
| | | return value == null ? null : value.intValue(); |
| | | } |
| | | |
| | | private String buildId(String actionGroupId, String prefix, int index, String suffix) { |
| | | return actionGroupId + "-" + prefix + String.format("%03d", index) + "-" + sanitize(suffix); |
| | | } |
| | | |
| | | private String sanitize(String value) { |
| | | return value == null ? "NA" : value.replaceAll("[^0-9A-Za-z_-]", "_"); |
| | | } |
| | | |
| | | @AllArgsConstructor |
| | | private static class NodeCursor { |
| | | private final String code; |
| | | private final HkOrderNode node; |
| | | } |
| | | |
| | | @AllArgsConstructor |
| | | private static class HeightDepthParam { |
| | | private final Number height; |
| | | private final Number depth; |
| | | } |
| | | } |