zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/HkAgvDataService.java
@@ -11,9 +11,9 @@ import com.zy.acs.framework.common.DateUtils; import com.zy.acs.manager.common.config.HikOrderProperties; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.constant.HikAngleConstant; import com.zy.acs.manager.core.domain.BackpackDto; import com.zy.acs.manager.core.service.astart.MapDataDispatcher; import com.zy.acs.manager.core.utils.AgvAngleUtils; import com.zy.acs.manager.manager.entity.AgvDetail; import com.zy.acs.manager.manager.entity.Code; import com.zy.acs.manager.manager.service.AgvDetailService; @@ -133,7 +133,7 @@ HkStateVelocity velocity = state.getVelocity(); if (agvPosition != null && agvPosition.getTheta() != null) { double angle = normalizeAngle(HikAngleConstant.toRcsAngleDegrees(Math.toDegrees(agvPosition.getTheta()))); double angle = normalizeAngle(AgvAngleUtils.fromRadians(agvPosition.getTheta())); detail.setAgvAngle(angle); detail.setGyroAngle(angle); detail.setEncoderAngle(angle); zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MainService.java
@@ -23,6 +23,7 @@ import com.zy.acs.manager.core.integrate.dto.OpenBusSubmitParam; import com.zy.acs.manager.core.service.hik.HikOrderPublishService; import com.zy.acs.manager.core.service.astart.MapDataDispatcher; import com.zy.acs.manager.core.utils.AgvAngleUtils; import com.zy.acs.manager.manager.entity.*; import com.zy.acs.manager.manager.enums.*; import com.zy.acs.manager.manager.service.*; @@ -901,8 +902,6 @@ if (Cools.isEmpty(agvId, segmentList)) { return pathTrace; } Date now = new Date(); long actionPrepareSts = ActionStsType.PREPARE.val(); // JSONObject storeDirection = configService.getVal("storeDirection", JSONObject.class); int angleOffsetVal = configService.getVal("mapAngleOffsetVal", Integer.class); // Double defaultShelfDepth = configService.getVal("defaultShelfDepth", Double.class); // defaultShelfDepth = Optional.ofNullable(defaultShelfDepth).orElse((double) 0); String agvNo = agvService.getAgvNo(agvId); @@ -951,7 +950,7 @@ Code nextCode = codeService.getCacheByData(next); // 车头朝前的下一个行走方向 Double nextDirection = mapService.calculateDirection(lastCode, nextCode, angleOffsetVal); Double nextDirection = AgvAngleUtils.calculateDirection(lastCode, nextCode, agvModel); // 反向角 final double oppLastDir = (lastDirection + 180) % 360; zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/hik/HikOrderPublishService.java
@@ -15,8 +15,8 @@ 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.core.constant.HikAngleConstant; import com.zy.acs.manager.core.service.MapService; import com.zy.acs.manager.core.utils.AgvAngleUtils; import com.zy.acs.manager.manager.entity.Action; import com.zy.acs.manager.manager.entity.AgvDetail; import com.zy.acs.manager.manager.entity.AgvModel; @@ -24,7 +24,6 @@ import com.zy.acs.manager.manager.enums.ActionTypeType; import com.zy.acs.manager.manager.service.AgvDetailService; import com.zy.acs.manager.manager.service.CodeService; import com.zy.acs.manager.system.service.ConfigService; import lombok.AllArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.beans.factory.annotation.Autowired; @@ -49,8 +48,6 @@ private CodeService codeService; @Autowired private AgvDetailService agvDetailService; @Autowired private ConfigService configService; private final RedisSupport redis = RedisSupport.defaultRedisSupport; @@ -134,7 +131,7 @@ case StraightBackTurnable: case StraightBackUnturnable: String endCode = findNextMoveEndCode(actionList, i, currentNode.code); Double travelDirection = calculateTravelDirection(currentNode.code, endCode); Double travelDirection = calculateTravelDirection(currentNode.code, endCode, agvModel); Double moveDirection = resolveMoveDirection(currentDirection, travelDirection, actionType, actionGroupId, currentNode.code, endCode); Double edgeOrientation = resolveEdgeOrientation(moveDirection, travelDirection); HkOrderEdge edge = createEdge(actionGroupId, edgeIndex++, sequenceId++, currentNode, endCode, edgeOrientation, action, agvModel); @@ -379,10 +376,7 @@ } private Double toHikThetaRadians(Double thetaDegrees) { if (thetaDegrees == null) { return null; } return HikAngleConstant.toHikAngleRadians(Math.toRadians(thetaDegrees)); return AgvAngleUtils.toRadians(thetaDegrees); } private void applyNodeTheta(HkOrderNode node, Double directionDegrees) { @@ -392,21 +386,13 @@ node.getNodePosition().setTheta(toHikThetaRadians(directionDegrees)); } private Double calculateTravelDirection(String startCode, String endCode) { private Double calculateTravelDirection(String startCode, String endCode, AgvModel agvModel) { 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"); } Integer angleOffsetVal = configService.getVal("mapAngleOffsetVal", Integer.class); if (angleOffsetVal == null) { angleOffsetVal = 0; } double deltaX = end.getX() - start.getX(); double deltaY = end.getY() - start.getY(); double angle = Math.atan2(deltaX, deltaY); angle = Math.toDegrees(angle) + angleOffsetVal; return normalizeDegrees(angle); return AgvAngleUtils.calculateDirection(start, end, agvModel); } private Double resolveMoveDirection(Double currentDirection, zy-acs-manager/src/main/java/com/zy/acs/manager/core/utils/AgvAngleUtils.java
New file @@ -0,0 +1,75 @@ package com.zy.acs.manager.core.utils; import com.zy.acs.common.enums.ClockwiseType; import com.zy.acs.manager.manager.entity.AgvModel; import com.zy.acs.manager.manager.entity.Code; /** * 车型角度工具： * 将地图物理方向（默认顺时针）转换为车型自身角度体系。 */ public final class AgvAngleUtils { private AgvAngleUtils() { } public static Double calculateDirection(Code startCode, Code endCode, AgvModel agvModel) { if (startCode == null || endCode == null) { return null; } double deltaX = endCode.getX() - startCode.getX(); double deltaY = endCode.getY() - startCode.getY(); double mapAngle = Math.toDegrees(Math.atan2(deltaX, deltaY)); return toModelAngle(normalizeAngle(mapAngle), agvModel); } public static Double toModelAngle(Double mapAngleDegrees, AgvModel agvModel) { if (mapAngleDegrees == null) { return null; } double factor = isClockwise(agvModel) ? 1D : -1D; double offset = getAngleOffsetDeg(agvModel); return normalizeAngle(factor * normalizeAngle(mapAngleDegrees) + offset); } public static Double fromModelAngle(Double modelAngleDegrees, AgvModel agvModel) { if (modelAngleDegrees == null) { return null; } double factor = isClockwise(agvModel) ? 1D : -1D; double offset = getAngleOffsetDeg(agvModel); return normalizeAngle(factor * (normalizeAngle(modelAngleDegrees) - offset)); } public static Double toRadians(Double angleDegrees) { if (angleDegrees == null) { return null; } return Math.toRadians(normalizeAngle(angleDegrees)); } public static Double fromRadians(Double angleRadians) { if (angleRadians == null) { return null; } return normalizeAngle(Math.toDegrees(angleRadians)); } public static Double normalizeAngle(Double angleDegrees) { if (angleDegrees == null) { return null; } double normalized = angleDegrees % 360D; return normalized >= 0 ? normalized : normalized + 360D; } private static Double getAngleOffsetDeg(AgvModel agvModel) { return agvModel == null || agvModel.getAngleOffsetDeg() == null ? 0D : agvModel.getAngleOffsetDeg(); } private static boolean isClockwise(AgvModel agvModel) { return agvModel == null || agvModel.getAngleClockwise() == null || agvModel.getAngleClockwise().equals(ClockwiseType.CW.value); } } zy-acs-manager/src/main/java/com/zy/acs/manager/fake/AgvSimulatorTask.java
@@ -13,12 +13,12 @@ import com.zy.acs.common.utils.RedisSupport; import com.zy.acs.manager.core.cache.CoreCache; import com.zy.acs.manager.common.config.HikOrderProperties; import com.zy.acs.manager.core.constant.HikAngleConstant; import com.zy.acs.manager.core.domain.CodeStepDto; import com.zy.acs.manager.core.domain.type.JobType; import com.zy.acs.manager.core.scheduler.MapDataWsScheduler; import com.zy.acs.manager.core.service.MainService; import com.zy.acs.manager.core.service.MapService; import com.zy.acs.manager.core.utils.AgvAngleUtils; import com.zy.acs.manager.manager.entity.Action; import com.zy.acs.manager.manager.entity.Agv; import com.zy.acs.manager.manager.entity.AgvDetail; @@ -278,7 +278,7 @@ AgvDetail agvDetail = agvDetailService.selectByAgvId(agv.getId()); if (agvDetail != null && agvDetail.getAgvAngle() != null) { position.setTheta(HikAngleConstant.toHikAngleRadians(Math.toRadians(agvDetail.getAgvAngle()))); position.setTheta(AgvAngleUtils.toRadians(agvDetail.getAgvAngle())); } return position; }
