package com.zy.acs.manager.core.service;
|
|
import com.alibaba.fastjson.JSON;
|
import com.zy.acs.common.enums.AgvStatusType;
|
import com.zy.acs.common.hk.action.type.HkActionType;
|
import com.zy.acs.common.hk.state.*;
|
import com.zy.acs.common.hk.state.type.HkActionStatusType;
|
import com.zy.acs.common.hk.state.type.HkEStopType;
|
import com.zy.acs.common.utils.News;
|
import com.zy.acs.framework.common.Cools;
|
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.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;
|
import com.zy.acs.manager.manager.service.AgvService;
|
import com.zy.acs.manager.manager.service.CodeService;
|
import com.zy.acs.manager.manager.service.JamService;
|
import com.zy.acs.manager.manager.service.impl.CodeServiceImpl;
|
import lombok.extern.slf4j.Slf4j;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.scheduling.annotation.Async;
|
import org.springframework.stereotype.Service;
|
|
import java.util.*;
|
|
/**
|
* 海康 state 主题数据适配为内部 AgvDetail。
|
*/
|
@Slf4j
|
@Service
|
public class HkAgvDataService {
|
|
private static final boolean PRINT_LOG = false;
|
|
private static final double DEFAULT_POSITION_MATCH_TOLERANCE = 0.05D;
|
|
@Autowired
|
private AgvService agvService;
|
@Autowired
|
private AgvDetailService agvDetailService;
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private MapService mapService;
|
@Autowired
|
private ThreadPoolRegulator threadPoolRegulator;
|
@Autowired
|
private JamService jamService;
|
@Autowired
|
private HkOrderStateClosureService hkOrderStateClosureService;
|
@Autowired
|
private HikOrderProperties hikOrderProperties;
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
|
@Async
|
public void dataProcess(HkState state) {
|
if (state == null || Cools.isEmpty(state.getSerialNumber())) {
|
log.warn("ignore empty hk state message: {}", JSON.toJSONString(state));
|
return;
|
}
|
|
String agvNo = state.getSerialNumber();
|
Long agvId = agvService.getAgvId(agvNo);
|
if (agvId == null) {
|
News.warn("Hk Agv [{}] 尚未鉴权 ！！！", agvNo);
|
return;
|
}
|
|
AgvDetail detail = agvDetailService.selectByAgvId(agvId);
|
if (detail == null) {
|
detail = new AgvDetail();
|
detail.setAgvId(agvId);
|
if (!agvDetailService.save(detail)) {
|
News.error("Hk Agv [{}] 详情初始化失败 ！！！", agvNo);
|
return;
|
}
|
}
|
|
Date now = new Date();
|
detail.setUpdateTime(now);
|
|
PositionResolution positionResolution = resolvePosition(state);
|
syncPosition(detail, positionResolution);
|
syncPose(detail, state);
|
syncBattery(detail, state);
|
syncBackpack(detail, agvId, state.getLoads());
|
detail.setStatus(resolveStatus(state));
|
syncError(detail, state, now);
|
|
if (!Cools.isEmpty(positionResolution.recentCodeData)) {
|
mapService.unlockPath(agvNo, positionResolution.recentCodeData);
|
threadPoolRegulator.getInstance()
|
.execute(() -> jamService.checkIfFinish(agvId, positionResolution.recentCodeData));
|
}
|
|
if (!agvDetailService.updateById(detail)) {
|
News.error("Hk Agv [{}] 详情更新失败 ！！！", agvNo);
|
return;
|
}
|
|
hkOrderStateClosureService.process(state);
|
|
if (PRINT_LOG) {
|
News.info("Hk Agv [{}] state ===>> {}", agvNo, JSON.toJSONString(state));
|
}
|
}
|
|
private void syncPosition(AgvDetail detail, PositionResolution positionResolution) {
|
if (positionResolution.code == null) {
|
return;
|
}
|
|
if (positionResolution.onNode) {
|
detail.setCode(positionResolution.code.getId());
|
detail.setLastCode(null);
|
detail.setPos(1);
|
return;
|
}
|
|
detail.setCode(null);
|
detail.setLastCode(positionResolution.code.getId());
|
detail.setPos(0);
|
}
|
|
private void syncPose(AgvDetail detail, HkState state) {
|
HkStateAgvPosition agvPosition = state.getAgvPosition();
|
HkStateVelocity velocity = state.getVelocity();
|
|
if (agvPosition != null && agvPosition.getTheta() != null) {
|
double angle = normalizeAngle(AgvAngleUtils.fromRadians(agvPosition.getTheta()));
|
detail.setAgvAngle(angle);
|
detail.setGyroAngle(angle);
|
detail.setEncoderAngle(angle);
|
}
|
|
// if (velocity != null) {
|
// detail.setStraightVal(Math.hypot(defaultDouble(velocity.getVx()), defaultDouble(velocity.getVy())));
|
// }
|
//
|
// if (agvPosition != null || velocity != null) {
|
// Map<String, Object> snapshot = new LinkedHashMap<>();
|
// if (agvPosition != null) {
|
// snapshot.put("x", agvPosition.getX());
|
// snapshot.put("y", agvPosition.getY());
|
// snapshot.put("theta", agvPosition.getTheta());
|
// snapshot.put("mapId", agvPosition.getMapId());
|
// snapshot.put("positionInitialized", agvPosition.getPositionInitialized());
|
// snapshot.put("localizationScore", agvPosition.getLocalizationScore());
|
// }
|
// if (velocity != null) {
|
// snapshot.put("vx", velocity.getVx());
|
// snapshot.put("vy", velocity.getVy());
|
// snapshot.put("omega", velocity.getOmega());
|
// }
|
// detail.setCodeOffsert(JSON.toJSONString(snapshot));
|
// }
|
}
|
|
private void syncBattery(AgvDetail detail, HkState state) {
|
HkStateBatteryState batteryState = state.getBatteryState();
|
if (batteryState == null) {
|
return;
|
}
|
|
if (batteryState.getBatteryCharge() != null) {
|
detail.setSoc((int) Math.round(batteryState.getBatteryCharge()));
|
}
|
if (batteryState.getBatteryVoltage() != null) {
|
detail.setVol((int) Math.round(batteryState.getBatteryVoltage()));
|
}
|
}
|
|
private void syncBackpack(AgvDetail detail, Long agvId, List<HkStateLoad> loads) {
|
int loadCount = loads == null ? 0 : loads.size();
|
Integer backpackCap = agvService.getBackpack(agvId);
|
int slotCount = Math.max(loadCount, backpackCap == null ? 0 : backpackCap);
|
|
List<BackpackDto> backpackDtoList = new ArrayList<>();
|
for (int i = 0; i < slotCount; i++) {
|
backpackDtoList.add(new BackpackDto(i + 1, i < loadCount));
|
}
|
detail.setBackpack(JSON.toJSONString(backpackDtoList));
|
}
|
|
private void syncError(AgvDetail detail, HkState state, Date now) {
|
String errorMessage = buildErrorMessage(state);
|
if (!Cools.isEmpty(errorMessage)) {
|
detail.setError(errorMessage);
|
detail.setErrorTime(now);
|
return;
|
}
|
|
if (!Cools.isEmpty(detail.realError()) && detail.getErrorTime() != null
|
&& DateUtils.diffToSeconds(detail.getErrorTime(), now) > 10) {
|
detail.setError(MapDataConstant.EMPTY_OF_ERROR);
|
}
|
}
|
|
private Integer resolveStatus(HkState state) {
|
// todo
|
// if (hasError(state)) {
|
// return AgvStatusType.ERROR.val;
|
// }
|
|
HkStateBatteryState batteryState = state.getBatteryState();
|
if (batteryState != null && Boolean.TRUE.equals(batteryState.getCharging())) {
|
return AgvStatusType.CHARGE.val;
|
}
|
|
if (Boolean.TRUE.equals(state.getPaused())) {
|
return AgvStatusType.PAUSE.val;
|
}
|
|
if (hasRunningAction(state, HkActionType.ROTATE_LOAD_LIFT.getCode())) {
|
return AgvStatusType.TEMP.val;
|
}
|
|
if (hasRunningAction(state, HkActionType.PICK.getCode())
|
|| hasRunningAction(state, HkActionType.DROP.getCode())) {
|
return AgvStatusType.MOTION.val;
|
}
|
|
if (Boolean.TRUE.equals(state.getDriving())) {
|
HkStateVelocity velocity = state.getVelocity();
|
double omega = velocity == null ? 0D : Math.abs(defaultDouble(velocity.getOmega()));
|
return omega > 1e-4 ? AgvStatusType.TURN.val : AgvStatusType.STRAIGHT.val;
|
}
|
|
return AgvStatusType.IDLE.val;
|
}
|
|
private boolean hasRunningAction(HkState state, String actionType) {
|
if (Cools.isEmpty(actionType) || Cools.isEmpty(state.getActionStates())) {
|
return false;
|
}
|
for (HkStateActionState actionState : state.getActionStates()) {
|
if (actionState == null || Cools.isEmpty(actionState.getActionType())) {
|
continue;
|
}
|
if (!actionType.equalsIgnoreCase(actionState.getActionType())) {
|
continue;
|
}
|
HkActionStatusType status = actionState.getActionStatus();
|
if (status == HkActionStatusType.RUNNING
|
|| status == HkActionStatusType.INITIALIZING
|
|| status == HkActionStatusType.PAUSED) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
private boolean hasError(HkState state) {
|
if (!Cools.isEmpty(state.getErrors())) {
|
return true;
|
}
|
HkStateSafetyState safetyState = state.getSafetyState();
|
if (safetyState == null) {
|
return false;
|
}
|
if (Boolean.TRUE.equals(safetyState.getFieldViolation())) {
|
return true;
|
}
|
return safetyState.getEStop() != null && safetyState.getEStop() != HkEStopType.NONE;
|
}
|
|
private String buildErrorMessage(HkState state) {
|
List<String> errorParts = new ArrayList<>();
|
|
if (!Cools.isEmpty(state.getErrors())) {
|
for (HkStateError error : state.getErrors()) {
|
if (error == null) {
|
continue;
|
}
|
StringBuilder builder = new StringBuilder();
|
if (error.getErrorType() != null) {
|
builder.append(error.getErrorType().name());
|
}
|
if (!Cools.isEmpty(error.getErrorDescription())) {
|
if (builder.length() > 0) {
|
builder.append(": ");
|
}
|
builder.append(error.getErrorDescription());
|
}
|
if (!Cools.isEmpty(error.getErrorReferences())) {
|
List<String> refs = new ArrayList<>();
|
for (HkStateErrorReference ref : error.getErrorReferences()) {
|
if (ref == null || ref.getReferenceKey() == null || Cools.isEmpty(ref.getReferenceValue())) {
|
continue;
|
}
|
refs.add(ref.getReferenceKey().name() + "=" + ref.getReferenceValue());
|
}
|
if (!refs.isEmpty()) {
|
if (builder.length() > 0) {
|
builder.append(" ");
|
}
|
builder.append("(").append(String.join(", ", refs)).append(")");
|
}
|
}
|
if (builder.length() > 0) {
|
errorParts.add(builder.toString());
|
}
|
}
|
}
|
|
HkStateSafetyState safetyState = state.getSafetyState();
|
if (safetyState != null) {
|
if (safetyState.getEStop() != null && safetyState.getEStop() != HkEStopType.NONE) {
|
errorParts.add("eStop=" + safetyState.getEStop().name());
|
}
|
if (Boolean.TRUE.equals(safetyState.getFieldViolation())) {
|
errorParts.add("fieldViolation=true");
|
}
|
}
|
|
return errorParts.isEmpty() ? "" : String.join("; ", errorParts);
|
}
|
|
private PositionResolution resolvePosition(HkState state) {
|
Code code = resolveCodeByDispatcher(state.getAgvPosition());
|
if (code == null) {
|
code = resolveCodeByPosition(state.getAgvPosition());
|
}
|
if (code == null) {
|
code = resolveCodeByNodeId(state.getLastNodeId());
|
}
|
boolean onNode = code != null && !Boolean.TRUE.equals(state.getDriving());
|
String recentCodeData = code == null ? null : code.getData();
|
return new PositionResolution(code, onNode, recentCodeData);
|
}
|
|
private Code resolveCodeByNodeId(String nodeId) {
|
if (Cools.isEmpty(nodeId)) {
|
return null;
|
}
|
|
Code code = codeService.getCacheByData(nodeId);
|
if (code != null) {
|
return code;
|
}
|
|
int idx = nodeId.lastIndexOf('-');
|
if (idx < 0 || idx >= nodeId.length() - 1) {
|
return null;
|
}
|
return codeService.getCacheByData(nodeId.substring(idx + 1));
|
}
|
|
private Code resolveCodeByDispatcher(HkStateAgvPosition agvPosition) {
|
if (agvPosition == null
|
|| !Boolean.TRUE.equals(agvPosition.getPositionInitialized())
|
|| agvPosition.getX() == null
|
|| agvPosition.getY() == null) {
|
return null;
|
}
|
|
String codeData = mapDataDispatcher.resolveNearestCodeByCoordinate(
|
null,
|
revertCoordinate(agvPosition.getX()),
|
revertCoordinate(agvPosition.getY()),
|
resolvePositionTolerance()
|
);
|
if (Cools.isEmpty(codeData)) {
|
return null;
|
}
|
return codeService.getCacheByData(codeData);
|
}
|
|
private Code resolveCodeByPosition(HkStateAgvPosition agvPosition) {
|
if (agvPosition == null
|
|| !Boolean.TRUE.equals(agvPosition.getPositionInitialized())
|
|| agvPosition.getX() == null
|
|| agvPosition.getY() == null) {
|
return null;
|
}
|
|
double x = revertCoordinate(agvPosition.getX());
|
double y = revertCoordinate(agvPosition.getY());
|
Code nearest = null;
|
double minDistance = Double.MAX_VALUE;
|
|
for (Code code : getAllCodes()) {
|
if (code == null || code.getX() == null || code.getY() == null) {
|
continue;
|
}
|
double distance = Math.hypot(code.getX() - x, code.getY() - y);
|
if (distance < minDistance) {
|
minDistance = distance;
|
nearest = code;
|
}
|
}
|
|
return nearest != null && minDistance <= resolvePositionTolerance() ? nearest : null;
|
}
|
|
private double revertCoordinate(Double value) {
|
double scale = hikOrderProperties.getCoordinateScale();
|
if (scale == 0D) {
|
return value;
|
}
|
return value / scale;
|
}
|
|
private double resolvePositionTolerance() {
|
double scale = hikOrderProperties.getCoordinateScale();
|
return Math.max(DEFAULT_POSITION_MATCH_TOLERANCE, hikOrderProperties.getDefaultAllowedDeviationXY()) / scale;
|
}
|
|
private Collection<Code> getAllCodes() {
|
if (!Cools.isEmpty(CodeServiceImpl.CODE_DATA_CACHE)) {
|
return CodeServiceImpl.CODE_DATA_CACHE.values();
|
}
|
return codeService.list();
|
}
|
|
private double defaultDouble(Double value) {
|
return value == null ? 0D : value;
|
}
|
|
private double normalizeAngle(double angle) {
|
return (angle % 360 + 360) % 360;
|
}
|
|
private static class PositionResolution {
|
|
private final Code code;
|
|
private final boolean onNode;
|
|
private final String recentCodeData;
|
|
private PositionResolution(Code code, boolean onNode, String recentCodeData) {
|
this.code = code;
|
this.onNode = onNode;
|
this.recentCodeData = recentCodeData;
|
}
|
}
|
}
|
