图斑飞行

guoshilong

2023-11-15 923a5e9fdfcfcc765606bce6fa7f99cb3717dcbe

 src/main/java/com/dji/sample/wayline/service/impl/WaylineJobServiceImpl.java

@@ -1,7 +1,11 @@
package com.dji.sample.wayline.service.impl;

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.baomidou.mybatisplus.core.conditions.update.LambdaUpdateWrapper;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.baomidou.mybatisplus.extension.conditions.update.LambdaUpdateChainWrapper;
import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
import com.dji.sample.common.error.CommonErrorEnum;
import com.dji.sample.common.model.CustomClaim;
@@ -12,11 +16,18 @@
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.control.model.dto.PointDTO;
import com.dji.sample.control.model.param.DrcModeParam;
import com.dji.sample.control.model.param.FlyToPointParam;
import com.dji.sample.control.model.param.TakeoffToPointParam;
import com.dji.sample.control.service.IControlService;
import com.dji.sample.control.service.IDrcService;
import com.dji.sample.geo.entity.GeoJson;
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.DroneRcLostActionEnum;
import com.dji.sample.manage.model.enums.WaylineRcLostActionEnum;
import com.dji.sample.manage.model.receiver.OsdDockReceiver;
import com.dji.sample.manage.model.receiver.OsdSubDeviceReceiver;
import com.dji.sample.manage.service.IDeviceRedisService;
@@ -24,13 +35,12 @@
import com.dji.sample.media.model.MediaFileCountDTO;
import com.dji.sample.media.model.MediaMethodEnum;
import com.dji.sample.media.service.IFileService;
import com.dji.sample.geo.utils.GeoUtils;
import com.dji.sample.wayline.dao.IWaylineJobMapper;
import com.dji.sample.wayline.model.dto.*;
import com.dji.sample.wayline.model.entity.WaylineJobEntity;
import com.dji.sample.wayline.model.enums.*;
import com.dji.sample.wayline.model.param.CreateJobParam;
import com.dji.sample.wayline.model.param.UpdateJobParam;
import com.dji.sample.wayline.model.param.WaylineJobQueryParam;
import com.dji.sample.wayline.model.param.*;
import com.dji.sample.wayline.service.IWaylineFileService;
import com.dji.sample.wayline.service.IWaylineJobService;
import com.dji.sample.wayline.service.IWaylineRedisService;
@@ -47,6 +57,7 @@
import org.springframework.util.CollectionUtils;
import org.springframework.util.StringUtils;

import java.io.File;
import java.net.URL;
import java.sql.SQLException;
import java.time.*;
@@ -89,6 +100,9 @@

    @Autowired
    private IWaylineRedisService waylineRedisService;

    @Autowired
    private IControlService controlService;

    private Optional<WaylineJobDTO> insertWaylineJob(WaylineJobEntity jobEntity) {
        int id = mapper.insert(jobEntity);
@@ -238,42 +252,39 @@

        WaylineJobDTO waylineJob = waylineJobOpt.get();

        //循环执行时间
        for (List<Long> timeArr : param.getExecuteStartTimeArr()) {
            LocalDate date = LocalDate.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault());
            long beginTime = LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault()))
                    .atZone(ZoneId.systemDefault()).toInstant().toEpochMilli();
        //存一条记录,
        List<Long> timeArr = param.getExecuteStartTimeArr().get(0);

            long endTime = timeArr.size() > 1 && Objects.nonNull(timeArr.get(1)) ?
                    LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(1)), ZoneId.systemDefault()))
                            .atZone(ZoneId.systemDefault()).toInstant().toEpochMilli() : beginTime;
        LocalDate date = LocalDate.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault());
        LocalDateTime beginTime = LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault()));

            if (WaylineTaskTypeEnum.IMMEDIATE != param.getTaskType() && endTime < System.currentTimeMillis()) {
                //中断当前循环执行下一个
                continue;
            }
        LocalDateTime endTime = timeArr.size() > 1 && Objects.nonNull(timeArr.get(1)) ?
                LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(1)), ZoneId.systemDefault())) : beginTime;


            //条件任务
            if (param.getTaskType() == WaylineTaskTypeEnum.CONDITION) {
                //如果是条件任务类型，需要在任务参数中添加条件。
                waylineJob.setConditions(
                        WaylineTaskConditionDTO.builder()
                                .executableConditions(Objects.nonNull(param.getMinStorageCapacity()) ? WaylineTaskExecutableConditionDTO.builder().storageCapacity(param.getMinStorageCapacity()).build() : null)
                                .readyConditions(WaylineTaskReadyConditionDTO.builder()
                                        .batteryCapacity(param.getMinBatteryCapacity())
                                        .beginTime(beginTime)
                                        .endTime(endTime)
                                        .build())
                                .build());
            }

            ResponseResult response = this.publishOneFlightTask(waylineJob);
            if (ResponseResult.CODE_SUCCESS != response.getCode()) {
                return response;
            }

        if (WaylineTaskTypeEnum.IMMEDIATE != param.getTaskType() && endTime.atZone(ZoneId.systemDefault()).toInstant().toEpochMilli() < System.currentTimeMillis()) {
            return ResponseResult.error("任务已过期");
        }
        //条件任务
        if (param.getTaskType() == WaylineTaskTypeEnum.CONDITION) {
            //如果是条件任务类型，需要在任务参数中添加条件。
            waylineJob.setConditions(
                    WaylineTaskConditionDTO.builder()
                            .executableConditions(Objects.nonNull(param.getMinStorageCapacity()) ? WaylineTaskExecutableConditionDTO.builder().storageCapacity(param.getMinStorageCapacity()).build() : null)
                            .readyConditions(WaylineTaskReadyConditionDTO.builder()
                                    .batteryCapacity(param.getMinBatteryCapacity())
                                    .beginTime(beginTime.atZone(ZoneId.systemDefault()).toInstant().toEpochMilli())
                                    .endTime(endTime.atZone(ZoneId.systemDefault()).toInstant().toEpochMilli())
                                    .build())
                            .build());
        } else {
            waylineJob.setBeginTime(beginTime);
        }

        ResponseResult response = this.publishOneFlightTask(waylineJob);
        if (ResponseResult.CODE_SUCCESS != response.getCode()) {
            return response;
        }

        return ResponseResult.success();
    }

@@ -482,6 +493,16 @@

    @Override
    public Boolean updateJob(WaylineJobDTO dto) {

        try {
            if (dto.getStatus() == 3) {
                this.checkNextJob(dto);
            }
        } catch (SQLException e) {
            throw new RuntimeException(e);
        }


        return mapper.update(this.dto2Entity(dto),
                new LambdaUpdateWrapper<WaylineJobEntity>()
                        .eq(WaylineJobEntity::getJobId, dto.getJobId())) > 0;
@@ -489,19 +510,7 @@

    @Override
    public PaginationData<WaylineJobDTO> getJobsByWorkspaceId(String workspaceId, long page, long pageSize, WaylineJobQueryParam waylineJobQueryParam) {
//        Page<WaylineJobEntity> pageData = mapper.selectPage(
//                new Page<WaylineJobEntity>(page, pageSize),
//                new LambdaQueryWrapper<WaylineJobEntity>()
//                        .eq(WaylineJobEntity::getWorkspaceId, workspaceId)
//                        .eq(waylineJobDTO.getTaskType() !=null, WaylineJobEntity::getTaskType, waylineJobDTO.getTaskType())
//                        .eq(waylineJobDTO.getStatus()!=null, WaylineJobEntity::getStatus, waylineJobDTO.getStatus())
//                        .like(StringUtils.hasText(waylineJobDTO.getJobName()),WaylineJobEntity::getName,waylineJobDTO.getJobName())
//
//
//                        .orderByDesc(WaylineJobEntity::getId));

        Page<WaylineJobEntity> pageData = mapper.getPage(new Page<WaylineJobEntity>(page, pageSize), waylineJobQueryParam, workspaceId);


        List<WaylineJobDTO> records = pageData.getRecords()
                .stream()
@@ -509,6 +518,18 @@
                .collect(Collectors.toList());

        return new PaginationData<WaylineJobDTO>(records, new Pagination(pageData));
    }

    @Override
    public List<WaylineJobDTO> getChildrenJobs(String workspaceId, WaylineJobQueryParam waylineJobQueryParam) {

        List<WaylineJobEntity> list = mapper.getJobs(workspaceId, waylineJobQueryParam);
        List<WaylineJobDTO> records = list
                .stream()
                .map(this::entity2Dto)
                .collect(Collectors.toList());

        return records;
    }


@@ -675,18 +696,233 @@

    @Override
    public WaylineJobEntity getLatestJob(String workspaceId, WaylineJobQueryParam waylineJobQueryParam) {
//        List<WaylineJobEntity> waylineJobEntities = mapper.selectList(new LambdaQueryWrapper<>(WaylineJobEntity.class)
//                .eq(WaylineJobEntity::getWorkspaceId, workspaceId)
//                .eq(WaylineJobEntity::getDockSn, waylineJobQueryParam.getDockSn())
//                //获取状态为待执行
//                        .eq(WaylineJobEntity::getStatus,1)
//                .orderByDesc(WaylineJobEntity::getBeginTime)
//        );
        WaylineJobEntity waylineJobEntity = mapper.getLatest(workspaceId, waylineJobQueryParam);
        return waylineJobEntity;
    }

        WaylineJobEntity waylineJobEntity = mapper.getLatest(workspaceId,waylineJobQueryParam);
    @Override
    public ResponseResult flyByArea(String sn, FlyAreaParam flyAreaParam, String deviceSn) {

        //获取所有中心点
//        List<PointPOJO> targetList = GeoUtils.caculatePointList(flyAreaParam);
        PointPOJO dockPoint = flyAreaParam.getDockPoint();
        GeoJson geoJson = GeoUtils.readJsonFile(flyAreaParam.getJsonPath());
        List<PointPOJO> targetList = GeoUtils.caculatePointList(geoJson, dockPoint, flyAreaParam.getRadius());

        //一键起飞
        TakeoffToPointParam takeoffToPointParam = buildTakeoffToPointParam(dockPoint);

        ResponseResult takeoffToPointRes = controlService.takeoffToPoint(sn, takeoffToPointParam);

        //设置飞向第一个点
        while (takeoffToPointRes.getCode() == ResponseResult.CODE_SUCCESS) {
            //获取无人机状态
            DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(deviceSn);

            //当无人机状态为手动飞行
            if (deviceMode == DeviceModeCodeEnum.MANUAL) {
                //飞向目标点
                FlyToPointParam flyToPointParam = new FlyToPointParam();
                flyToPointParam.setMaxSpeed(14);
                List<PointDTO> pointDTOS = new ArrayList<>();

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


       return waylineJobEntity;
                ResponseResult flyToRes = controlService.flyToPoint(sn, flyToPointParam);

                if (flyToRes.getCode() == ResponseResult.CODE_SUCCESS) {
                    //第一个点指令飞行成功后，把数组存到redis中
                    /**
                     * {
                     *      targetList:[],
                     *      curIndex:
                     * }
                     */

                    JSONObject jsonObject = new JSONObject();
                    jsonObject.put("targetList",targetList);
                    jsonObject.put("curIndex",0);


                    RedisOpsUtils.set("tuban:"+sn,jsonObject);
                }
                break;
            }
        }

        return ResponseResult.success();
    }

    public TakeoffToPointParam buildTakeoffToPointParam(PointPOJO dockPoint) {
        TakeoffToPointParam takeoffToPointParam = new TakeoffToPointParam();

        takeoffToPointParam.setTargetLatitude(dockPoint.getLat());
        takeoffToPointParam.setTargetLongitude(dockPoint.getLon());
        //设置飞行高度
        takeoffToPointParam.setTargetHeight(120.0);
        //设置安全起飞高度
        takeoffToPointParam.setSecurityTakeoffHeight(100.0);
        //设置返航高度
        takeoffToPointParam.setRthAltitude(100.0);
        //设置失控操作
        takeoffToPointParam.setRcLostAction(DroneRcLostActionEnum.RETURN_HOME);
        //设置起飞速度
        takeoffToPointParam.setMaxSpeed(10.0);
        takeoffToPointParam.setExitWaylineWhenRcLost(WaylineRcLostActionEnum.EXECUTE_RC_LOST_ACTION);

        return takeoffToPointParam;
    }


    @Override
    public ResponseResult checkNextJob(WaylineJobDTO job) throws SQLException {

        WaylineJobEntity params = new WaylineJobEntity();
        params.setJobId(job.getJobId());


        WaylineJobEntity job1 = mapper.selectOne(Wrappers.query(params));

        WaylineJobEntity currentJob = JSON.parseObject(JSON.toJSONString(job1), WaylineJobEntity.class);

        if (currentJob == null) {
            return ResponseResult.error("该任务不存在");
        }

        //该任务没有多次时间
        if (currentJob.getExecuteStartTimeArr() == null) {
            return ResponseResult.success();
        }

        List<List<Long>> executeStartTimeArr = currentJob.getExecuteStartTimeArr();
        int indexTime = -1;

        long currentTime = System.currentTimeMillis();
        for (int i = 0; i < executeStartTimeArr.size(); i++) {
            Long startTime = executeStartTimeArr.get(i).get(0) * 1000;
            //如果当前时间小于开始时间
            if (currentTime < startTime) {
                //当前索引就是下一次要执行的时间
                indexTime = i;
                break;
            }
        }

        if (indexTime == -1) {
            return ResponseResult.success();
        }


        List<Long> timeArr = currentJob.getExecuteStartTimeArr().get(indexTime);
        LocalDate date = LocalDate.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault());
        LocalDateTime beginDate = LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault()));
        LocalDateTime endDate = timeArr.size() > 1 && Objects.nonNull(timeArr.get(1)) ?
                LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(1)), ZoneId.systemDefault()))
                : beginDate;

        long beginTime = beginDate
                .atZone(ZoneId.systemDefault()).toInstant().toEpochMilli();
        long endTime = endDate
                .atZone(ZoneId.systemDefault()).toInstant().toEpochMilli();


        //添加一条新数据
        WaylineJobEntity waylineJobEntity = WaylineJobEntity.builder()
                .jobId(UUID.randomUUID().toString())
                //大于1则拿前面的parentId,为1则拿第0个的id
                .parentId(indexTime > 1 ? currentJob.getParentId().toString() : currentJob.getId().toString())
                .name(currentJob.getName())
                .dockSn(currentJob.getDockSn())
                .fileId(currentJob.getFileId())
                .username(currentJob.getUsername())
                .workspaceId(currentJob.getWorkspaceId())

                .beginTime(beginTime)
                .endTime(endTime)
                .status(WaylineJobStatusEnum.PENDING.getVal())
                .taskType(currentJob.getTaskType())
                .waylineType(currentJob.getWaylineType())
                .outOfControlAction(currentJob.getOutOfControlAction())
                .batteryCapacity(currentJob.getBatteryCapacity())
                .rthAltitude(currentJob.getRthAltitude())
                .mediaCount(0)
                .repFreVal(currentJob.getRepFreVal())
                .repFreType(currentJob.getRepFreType())
                .repRuleType(currentJob.getRepRuleType())
                .repRuleVal(currentJob.getRepRuleVal())
                .executeTimeArr(currentJob.getExecuteTimeArr())
                .executeStartTimeArr(currentJob.getExecuteStartTimeArr())
                .build();

        Optional<WaylineJobDTO> waylineJobOpt = insertWaylineJob(waylineJobEntity);

        if (waylineJobOpt.isEmpty()) {
            return ResponseResult.error("任务创建失败");
        }

        WaylineJobDTO waylineJob = waylineJobOpt.get();

        LocalDateTime beginTimeNext = LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(0)), ZoneId.systemDefault()));

        LocalDateTime endTimeNext = timeArr.size() > 1 && Objects.nonNull(timeArr.get(1)) ?
                LocalDateTime.of(date, LocalTime.ofInstant(Instant.ofEpochSecond(timeArr.get(1)), ZoneId.systemDefault())) : beginTimeNext;


        //条件任务
        if (currentJob.getTaskType() == 2) {
            //如果是条件任务类型，需要在任务参数中添加条件。
            waylineJob.setConditions(
                    WaylineTaskConditionDTO.builder()
                            .executableConditions(null)
                            .readyConditions(WaylineTaskReadyConditionDTO.builder()
                                    .batteryCapacity(currentJob.getBatteryCapacity())
                                    .beginTime(beginTime)
                                    .endTime(endTime)
                                    .build())
                            .build());
        } else {
            waylineJob.setBeginTime(beginDate);
        }

        ResponseResult response = this.publishOneFlightTask(waylineJob);
        if (ResponseResult.CODE_SUCCESS != response.getCode()) {
            return response;
        }

        return ResponseResult.success();
    }


    @Override
    public WaylineJobCountDTO patrolStatistics(String workspaceId, String queryTime) {
        WaylineJobCountDTO waylineJobCountDTO = new WaylineJobCountDTO();
        List<WaylineJobEntity> list = mapper.patrolStatistics(workspaceId, queryTime);
        if (!CollectionUtils.isEmpty(list)) {
            waylineJobCountDTO.setTotalNumber(list.size());
            long totalTime = list.stream().mapToLong(s -> s.getEndTime() - s.getBeginTime()).sum() / 1000;
            StringBuffer buffer = new StringBuffer();
            long h = totalTime / 3600;
            long m = (totalTime % 3600) / 60;
            buffer.append(h).append(" h ");
            buffer.append(m).append(" min");
            waylineJobCountDTO.setTotalDuration(buffer.toString());
        }
        return waylineJobCountDTO;
    }

    @Override
    public void updateJobCollect(WaylineJobEntity waylineJob) {
        new LambdaUpdateChainWrapper<>(mapper)
                .eq(WaylineJobEntity::getJobId, waylineJob.getJobId())
                .set(WaylineJobEntity::getCollectStatus, waylineJob.getCollectStatus())
                .set(WaylineJobEntity::getUserId, waylineJob.getUserId())
                .update();
    }

    private void pauseJob(String workspaceId, String dockSn, String jobId, WaylineJobStatusEnum statusEnum) {
@@ -757,7 +993,8 @@
                .waylineType(WaylineTemplateTypeEnum.find(entity.getWaylineType()))
                .rthAltitude(entity.getRthAltitude())
                .outOfControlAction(entity.getOutOfControlAction())
                .mediaCount(entity.getMediaCount());
                .mediaCount(entity.getMediaCount())
                .hasChildren(entity.getHasChildren());

        if (Objects.nonNull(entity.getEndTime())) {
            builder.endTime(LocalDateTime.ofInstant(Instant.ofEpochMilli(entity.getEndTime()), ZoneId.systemDefault()));