增强图传开关

xieb

2024-04-10 d7935e283b0171819f11fe1dc1608f94d455e892

 src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java

@@ -1,32 +1,41 @@
package com.dji.sample.control.service.impl;

import com.alibaba.fastjson.JSONObject;
import com.dji.sample.common.error.CommonErrorEnum;
import com.dji.sample.common.model.ResponseResult;
import com.dji.sample.common.util.SpringBeanUtils;
import com.dji.sample.component.mqtt.model.*;
import com.dji.sample.component.mqtt.service.IMessageSenderService;
import com.dji.sample.component.redis.RedisConst;
import com.dji.sample.component.redis.RedisOpsUtils;
import com.dji.sample.component.websocket.model.CustomWebSocketMessage;
import com.dji.sample.component.websocket.model.BizCodeEnum;
import com.dji.sample.component.websocket.service.ISendMessageService;
import com.dji.sample.component.websocket.service.IWebSocketManageService;
import com.dji.sample.control.model.enums.RemoteControlMethodEnum;
import com.dji.sample.control.model.param.RemoteDebugParam;
import com.dji.sample.control.model.dto.FlyToProgressReceiver;
import com.dji.sample.control.model.dto.PointDTO;
import com.dji.sample.control.model.dto.ResultNotifyDTO;
import com.dji.sample.control.model.dto.TakeoffProgressReceiver;
import com.dji.sample.control.model.enums.*;
import com.dji.sample.control.model.param.*;
import com.dji.sample.control.service.IControlService;
import com.dji.sample.manage.model.dto.DeviceDTO;
import com.dji.sample.manage.model.enums.DeviceModeCodeEnum;
import com.dji.sample.manage.model.enums.DockModeCodeEnum;
import com.dji.sample.manage.model.enums.UserTypeEnum;
import com.dji.sample.manage.model.receiver.BasicDeviceProperty;
import com.dji.sample.manage.service.IDevicePayloadService;
import com.dji.sample.manage.service.IDeviceRedisService;
import com.dji.sample.manage.service.IDeviceService;
import com.dji.sample.wayline.model.enums.WaylineErrorCodeEnum;
import com.dji.sample.wayline.model.param.PointPOJO;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.ObjectMapper;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.integration.annotation.ServiceActivator;
import org.springframework.integration.mqtt.support.MqttHeaders;
import org.springframework.messaging.MessageHeaders;
import org.springframework.stereotype.Service;
import org.springframework.util.StringUtils;

import java.util.Objects;
import java.util.UUID;
import java.util.*;

/**
 * @author sean
@@ -44,74 +53,85 @@
    private ISendMessageService webSocketMessageService;

    @Autowired
    private IWebSocketManageService webSocketManageService;
    private IDeviceService deviceService;

    @Autowired
    private IDeviceService deviceService;
    private IDeviceRedisService deviceRedisService;

    @Autowired
    private ObjectMapper mapper;

    @Override
    public ResponseResult controlDock(String sn, String serviceIdentifier, RemoteDebugParam param) {
        RemoteControlMethodEnum controlMethodEnum = RemoteControlMethodEnum.find(serviceIdentifier);
        if (RemoteControlMethodEnum.UNKNOWN == controlMethodEnum) {
            return ResponseResult.error("The " + serviceIdentifier + " method does not exist.");
        }
    @Autowired
    private IDevicePayloadService devicePayloadService;

        Object data = "";
        // Add parameter validation.
        if (Objects.nonNull(controlMethodEnum.getClazz())) {
            if (Objects.isNull(param)) {
                return ResponseResult.error(CommonErrorEnum.ILLEGAL_ARGUMENT);
            }
            BasicDeviceProperty basicDeviceProperty = mapper.convertValue(param.getAction(), controlMethodEnum.getClazz());
            if (!basicDeviceProperty.valid()) {
                return ResponseResult.error(CommonErrorEnum.ILLEGAL_ARGUMENT);
            }
            data = basicDeviceProperty;
    private RemoteDebugHandler checkDebugCondition(String sn, RemoteDebugParam param, RemoteDebugMethodEnum controlMethodEnum) {
        RemoteDebugHandler handler = Objects.nonNull(controlMethodEnum.getClazz()) ?
                mapper.convertValue(Objects.nonNull(param) ? param : new Object(), controlMethodEnum.getClazz())
                : new RemoteDebugHandler();
        if (!handler.canPublish(sn)) {
            throw new RuntimeException("当前的机场状态不支持此功能。");
        }

        boolean isExist = deviceService.checkDeviceOnline(sn);
        if (!isExist) {
            return ResponseResult.error("The dock is offline.");
        if (Objects.nonNull(param) && !handler.valid()) {
            throw new RuntimeException(CommonErrorEnum.ILLEGAL_ARGUMENT.getErrorMsg());
        }
        String topic = TopicConst.THING_MODEL_PRE + TopicConst.PRODUCT + sn + TopicConst.SERVICES_SUF;
        String bid = UUID.randomUUID().toString();
        ServiceReply serviceReplyOpt = messageSenderService.publishWithReply(
                topic, CommonTopicResponse.builder()
                        .tid(UUID.randomUUID().toString())
                        .bid(bid)
                        .method(serviceIdentifier)
                        .timestamp(System.currentTimeMillis())
                        .data(data)
                        .build());

        ServiceReply<EventsOutputReceiver> serviceReply = mapper.convertValue(
                serviceReplyOpt, new TypeReference<ServiceReply<EventsOutputReceiver>>() {});
        if (ResponseResult.CODE_SUCCESS != serviceReply.getResult()) {
            return ResponseResult.error(serviceReply.getResult(),
                    Objects.nonNull(serviceReply.getOutput()) ? serviceReply.getOutput().getStatus()
                            : "error: " + serviceIdentifier + serviceReply.getResult());
        }
        if (controlMethodEnum.getProgress()) {
            RedisOpsUtils.setWithExpire(serviceIdentifier + RedisConst.DELIMITER +  bid, sn,
                    RedisConst.DEVICE_ALIVE_SECOND * RedisConst.DEVICE_ALIVE_SECOND);
        }
        return ResponseResult.success();
        return handler;
    }

    @Override
    @ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_CONTROL_PROGRESS, outputChannel = ChannelName.OUTBOUND)
    public void handleControlProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
    public ResponseResult controlDockDebug(String sn, String serviceIdentifier, RemoteDebugParam param) {
        RemoteDebugMethodEnum controlMethodEnum = RemoteDebugMethodEnum.find(serviceIdentifier);
        if (RemoteDebugMethodEnum.UNKNOWN == controlMethodEnum) {
            return ResponseResult.error(" 没有找到" + serviceIdentifier + " 这个指令。");
        }

        RemoteDebugHandler data = checkDebugCondition(sn, param, controlMethodEnum);

        boolean isExist = deviceRedisService.checkDeviceOnline(sn);
        if (!isExist) {
            return ResponseResult.error("设备离线");
        }
        String bid = UUID.randomUUID().toString();
        ServiceReply serviceReply = messageSenderService.publishServicesTopic(sn, serviceIdentifier, data, bid);

        if (ResponseResult.CODE_SUCCESS != serviceReply.getResult()) {
            return ResponseResult.error(serviceReply.getResult(),
                    "error: " + serviceIdentifier + serviceReply.getResult());
        }
        if (controlMethodEnum.getProgress()) {
            RedisOpsUtils.setWithExpire(serviceIdentifier + RedisConst.DELIMITER + bid, sn,
                    RedisConst.DEVICE_ALIVE_SECOND * RedisConst.DEVICE_ALIVE_SECOND);
        }

        //当执行返航指令时，删除图斑redis
        if (serviceIdentifier == "return_home"){
            if (RedisOpsUtils.checkExist("tuban:" + sn)){
                RedisOpsUtils.del("tuban:" + sn);
            }
        }



        return ResponseResult.success();
    }

    /**
     * Handles multi-state command progress information.
     *
     * @param receiver
     * @param headers
     * @return
     */
    @ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_CONTROL_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
    public CommonTopicReceiver handleControlProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
        String key = receiver.getMethod() + RedisConst.DELIMITER + receiver.getBid();
        if (RedisOpsUtils.getExpire(key) <= 0) {
            return;
            return receiver;
        }
        String sn = RedisOpsUtils.get(key).toString();

        EventsReceiver<EventsOutputReceiver> eventsReceiver = mapper.convertValue(receiver.getData(),
                new TypeReference<EventsReceiver<EventsOutputReceiver>>(){});
        EventsReceiver<EventsOutputProgressReceiver> eventsReceiver = mapper.convertValue(receiver.getData(),
                new TypeReference<EventsReceiver<EventsOutputProgressReceiver>>() {
                });
        eventsReceiver.setBid(receiver.getBid());
        eventsReceiver.setSn(sn);

@@ -127,26 +147,281 @@
            RedisOpsUtils.del(key);
        }

        DeviceDTO device = (DeviceDTO) RedisOpsUtils.get(RedisConst.DEVICE_ONLINE_PREFIX + sn);
        webSocketMessageService.sendBatch(
                webSocketManageService.getValueWithWorkspaceAndUserType(
                        device.getWorkspaceId(), UserTypeEnum.WEB.getVal()),
                CustomWebSocketMessage.builder()
                        .data(eventsReceiver)
                        .timestamp(System.currentTimeMillis())
                        .bizCode(receiver.getMethod())
                        .build());
        Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(sn);

        if (receiver.getNeedReply() != null && receiver.getNeedReply() == 1) {
            String topic = headers.get(MqttHeaders.RECEIVED_TOPIC) + TopicConst._REPLY_SUF;
            messageSenderService.publish(topic,
                    CommonTopicResponse.builder()
                            .tid(receiver.getTid())
                            .bid(receiver.getBid())
                            .method(receiver.getMethod())
                            .timestamp(System.currentTimeMillis())
                            .data(RequestsReply.success())
                            .build());
        if (deviceOpt.isEmpty()) {
            throw new RuntimeException("设备离线.");
        }

        DeviceDTO device = deviceOpt.get();
        webSocketMessageService.sendBatch(device.getWorkspaceId(), UserTypeEnum.WEB.getVal(),
                receiver.getMethod(), eventsReceiver);

        return receiver;
    }

    private void checkFlyToCondition(String dockSn) {
        // TODO 设备固件版本不兼容情况
        Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(dockSn);
        if (dockOpt.isEmpty()) {
            throw new RuntimeException("机场离线请重启机场");
        }

        DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(dockOpt.get().getChildDeviceSn());
        if (DeviceModeCodeEnum.MANUAL != deviceMode) {
            throw new RuntimeException("无人机当前状态不支持此功能，请稍后再试");
        }

        ResponseResult result = seizeAuthority(dockSn, DroneAuthorityEnum.FLIGHT, null);
        if (ResponseResult.CODE_SUCCESS != result.getCode()) {
            throw new IllegalArgumentException(result.getMessage());
        }
    }

    @Override
    public ResponseResult flyToPoint(String sn, FlyToPointParam param) {
        checkFlyToCondition(sn);

        param.setFlyToId(UUID.randomUUID().toString());
        ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.FLY_TO_POINT.getMethod(), param, param.getFlyToId());
        return ResponseResult.CODE_SUCCESS != reply.getResult() ?
                ResponseResult.error("飞向目标点失败。" + reply.getResult())
                : ResponseResult.success();
    }

    @Override
    public ResponseResult flyToPointStop(String sn) {
        ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.FLY_TO_POINT_STOP.getMethod(), null);
        return ResponseResult.CODE_SUCCESS != reply.getResult() ?
                ResponseResult.error("飞向目标点的无人机停止失败" + reply.getResult())
                : ResponseResult.success();
    }

    @ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_FLY_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
    public CommonTopicReceiver handleFlyToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) throws Exception {
        String dockSn = receiver.getGateway();

        Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
        if (deviceOpt.isEmpty()) {
            log.error("机场离线");
            return null;
        }

        FlyToProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<FlyToProgressReceiver>() {
        });
        webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
                BizCodeEnum.FLY_TO_POINT_PROGRESS.getCode(),
                ResultNotifyDTO.builder().sn(dockSn)
                        .message(WaylineErrorCodeEnum.SUCCESS == eventsReceiver.getResult() ?
                                eventsReceiver.getStatus().getMessage() : eventsReceiver.getResult().getErrorMsg())
                        .result(eventsReceiver.getResult().getErrorCode())
                        .build());
        //当飞向目标点成功后
        if (eventsReceiver.getStatus().equals(FlyToStatusEnum.WAYLINE_OK)) {
            JSONObject jsonObject = (JSONObject) RedisOpsUtils.get("tuban:" + dockSn);
            if (jsonObject != null) {
                List<PointPOJO> targetList = (List<PointPOJO>) jsonObject.get("targetList");
                int curIndex = (Integer) jsonObject.get("curIndex");
                String payloadIndex = jsonObject.getString("payloadIndex");
                flyToNextPoint(targetList, curIndex+1, dockSn, payloadIndex);
            }
        }
        return receiver;
    }

    @Override
    public ResponseResult flyToNextPoint(List<PointPOJO> targetList, int curIndex, String sn, String payloadIndex) throws Exception {
        //当无人机状态为人工时再发布下一个命令
        while (true) {
            Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(sn);
            DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(dockOpt.get().getChildDeviceSn());

            if (DeviceModeCodeEnum.MANUAL == deviceMode) {

                //执行拍照
                ResponseResult responseResult = takePhoto(sn, payloadIndex);

                //发布飞行指令
                if (curIndex == targetList.size()) {
                    //当前是最后一个点，返航
                    ResponseResult returnHome = controlDockDebug(sn, "return_home", null);

                    RedisOpsUtils.del("tuban:" + sn);

                    return returnHome;
                } else {
                    //当前不是最后一个点，飞行到下一个点
                    FlyToPointParam flyToPointParam = new FlyToPointParam();
                    flyToPointParam.setMaxSpeed(14);
                    List<PointDTO> pointDTOS = new ArrayList<>();

                    PointDTO pointDTO = new PointDTO();
                    pointDTO.setHeight(150.0);
                    pointDTO.setLongitude(targetList.get(curIndex).getLon());
                    pointDTO.setLatitude(targetList.get(curIndex).getLat());
                    pointDTOS.add(pointDTO);
                    flyToPointParam.setPoints(pointDTOS);

                    //发布下一个飞行指令
                    ResponseResult flyToRes = flyToPoint(sn, flyToPointParam);
                    if (flyToRes.getCode() == ResponseResult.CODE_SUCCESS) {
                        JSONObject jsonObject = new JSONObject();
                        jsonObject.put("targetList", targetList);
                        jsonObject.put("curIndex", curIndex);
                        jsonObject.put("payloadIndex",payloadIndex);
                        RedisOpsUtils.set("tuban:" + sn, jsonObject);
                    }
                    return flyToRes;

                }
            }
        }
    }

    @Override
    public ResponseResult takePhoto(String sn, String payloadIndex) throws Exception {

        //获取负载控制权
        DronePayloadParam dronePayloadParam = new DronePayloadParam();
        dronePayloadParam.setPayloadIndex(payloadIndex);

        ResponseResult seizeAuthorityRes = seizeAuthority(sn, DroneAuthorityEnum.PAYLOAD, dronePayloadParam);

        //切换为相机模式
        if (seizeAuthorityRes.getCode() != ResponseResult.CODE_SUCCESS) {
            return seizeAuthorityRes;
        }

        PayloadCommandsParam payloadCommandsParam = new PayloadCommandsParam();

//            DronePayloadParam switchParam = new DronePayloadParam();
//            switchParam.setCameraMode(CameraModeEnum.PHOTO);
//            switchParam.setPayloadIndex(payloadIndex);

//            payloadCommandsParam.setSn(sn);
//            payloadCommandsParam.setCmd(PayloadCommandsEnum.CAMERA_MODE_SWitCH);
//            payloadCommandsParam.setData(switchParam);
//
//            ResponseResult switchModeRes = payloadCommands(payloadCommandsParam);

//            if (switchModeRes.getCode() != ResponseResult.CODE_SUCCESS){
//                return  switchModeRes;
//            }
        //拍照
        payloadCommandsParam.setCmd(PayloadCommandsEnum.CAMERA_PHOTO_TAKE);
        DronePayloadParam takePhotoParam = new DronePayloadParam();
        takePhotoParam.setPayloadIndex(payloadIndex);

        payloadCommandsParam.setData(takePhotoParam);
        payloadCommandsParam.setSn(sn);

        ResponseResult responseResult = payloadCommands(payloadCommandsParam);

        return responseResult;
    }


    private void checkTakeoffCondition(String dockSn) {
        Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(dockSn);
        if (dockOpt.isEmpty() || DockModeCodeEnum.IDLE != deviceService.getDockMode(dockSn)) {
            throw new RuntimeException("当前状态不支持起飞");
        }

        ResponseResult result = seizeAuthority(dockSn, DroneAuthorityEnum.FLIGHT, null);
        if (ResponseResult.CODE_SUCCESS != result.getCode()) {
            throw new IllegalArgumentException(result.getMessage());
        }

    }

    @Override
    public ResponseResult takeoffToPoint(String sn, TakeoffToPointParam param) {
        checkTakeoffCondition(sn);
        if (!StringUtils.hasText(param.getFlightId())){
            param.setFlightId(UUID.randomUUID().toString());
        }
        ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.TAKE_OFF_TO_POINT.getMethod(), param, param.getFlightId());
        return ResponseResult.CODE_SUCCESS != reply.getResult() ?
                ResponseResult.error("无人机起飞失败 " + reply.getResult())
                : ResponseResult.success();
    }

    @ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_TAKE_OFF_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
    public CommonTopicReceiver handleTakeoffToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
        String dockSn = receiver.getGateway();

        Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
        if (deviceOpt.isEmpty()) {
            log.error("机场离线");
            return null;
        }
        TakeoffProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<TakeoffProgressReceiver>() {
        });

        webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
                BizCodeEnum.TAKE_OFF_TO_POINT_PROGRESS.getCode(),
                ResultNotifyDTO.builder().sn(dockSn)
                        .message(WaylineErrorCodeEnum.SUCCESS == eventsReceiver.getResult() ?
                                eventsReceiver.getStatus().getMessage() : eventsReceiver.getResult().getErrorMsg())
                        .result(eventsReceiver.getResult().getErrorCode())
                        .build());

        return receiver;
    }

    @Override
    public ResponseResult seizeAuthority(String sn, DroneAuthorityEnum authority, DronePayloadParam param) {
        String method;
        switch (authority) {
            case FLIGHT:
                if (deviceService.checkAuthorityFlight(sn)) {
                    return ResponseResult.success();
                }
                method = DroneControlMethodEnum.FLIGHT_AUTHORITY_GRAB.getMethod();
                break;
            case PAYLOAD:
                if (checkPayloadAuthority(sn, param.getPayloadIndex())) {
                    return ResponseResult.success();
                }
                method = DroneControlMethodEnum.PAYLOAD_AUTHORITY_GRAB.getMethod();
                break;
            default:
                return ResponseResult.error(CommonErrorEnum.ILLEGAL_ARGUMENT);
        }
        ServiceReply serviceReply = messageSenderService.publishServicesTopic(sn, method, param);
        return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
                ResponseResult.error(serviceReply.getResult(), "方法: " + method + " 错误码：" + serviceReply.getResult())
                : ResponseResult.success();
    }

    private Boolean checkPayloadAuthority(String sn, String payloadIndex) {
        Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(sn);
        if (dockOpt.isEmpty()) {
            throw new RuntimeException("机场离线请重启机场");
        }
        return devicePayloadService.checkAuthorityPayload(dockOpt.get().getChildDeviceSn(), payloadIndex);
    }


    @Override
    public ResponseResult payloadCommands(PayloadCommandsParam param) throws Exception {
        param.getCmd().getClazz()
                .getDeclaredConstructor(DronePayloadParam.class)
                .newInstance(param.getData())
                .checkCondition(param.getSn());

        ServiceReply serviceReply = messageSenderService.publishServicesTopic(param.getSn(), param.getCmd().getCmd(), param.getData());
        return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
                ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
                : ResponseResult.success();
    }

    @Override
    public ResponseResult requestsConfig(String sn, String method, RequestsParam param) {
        ServiceReply serviceReply = messageSenderService.publishRequestsTopic(sn, method, param);
        return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
                ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
                : ResponseResult.success();
    }

}