#

vincentlu

2026-03-26 55e9047c62d7aeb55305cbb6af2d724acdf3d479

#

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/listen/HkAgvDataSubscriber.java

New file
@@ -0,0 +1,56 @@
package com.zy.acs.manager.core.listen;

import com.zy.acs.common.constant.RedisConstant;
import com.zy.acs.common.hk.state.HkState;
import com.zy.acs.common.utils.RedisSupport;
import com.zy.acs.manager.core.service.HkAgvDataService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.boot.context.event.ApplicationReadyEvent;
import org.springframework.context.event.EventListener;
import org.springframework.stereotype.Component;

import javax.annotation.PreDestroy;

/**
 * Created by vincent on 2023/6/16
 */
@Slf4j
@Component
public class HkAgvDataSubscriber {

    private Thread thread;
    private final RedisSupport redis = RedisSupport.defaultRedisSupport;

    @Autowired
    private HkAgvDataService hkAgvDataService;

    @EventListener(ApplicationReadyEvent.class)
    private void start(){
//        redis.deleteList(RedisConstant.AGV_DATA_FLAG);
        thread = new Thread(() -> {
            while (!Thread.currentThread().isInterrupted()) {
                try {
                    // 间隔
                    Thread.sleep(10);

                    HkState state = redis.pop(RedisConstant.HK_AGV_DATA_FLAG);
                    if (state != null) {
                        hkAgvDataService.dataProcess(state);
                    }

                } catch (Exception e) {
                    log.error("consume hk agv state failed", e);
                }
            }
        });
        thread.start();
    }

    @PreDestroy
    public void shutDown(){
        if (thread != null) thread.interrupt();
    }


}

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/HkAgvDataService.java

New file
@@ -0,0 +1,356 @@
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.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.BackpackDto;
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 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;

    @Autowired
    private AgvService agvService;
    @Autowired
    private AgvDetailService agvDetailService;
    @Autowired
    private CodeService codeService;
    @Autowired
    private MapService mapService;
    @Autowired
    private ThreadPoolRegulator threadPoolRegulator;
    @Autowired
    private JamService jamService;

    @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;
        }

        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(Math.toDegrees(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) {
        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 = 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 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;
        }
    }
}