a-tuoheng-airline/src/main/java/com/ruoyi/airline/domain/uitl/WayPointUitls.java

package com.ruoyi.airline.domain.uitl;

import com.ruoyi.airline.api.domain.AirLinePointVO;
import com.ruoyi.airline.domain.model.kml.*;
import com.ruoyi.airline.service.dto.AirLinePointDTO;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.util.CollectionUtils;
import org.springframework.util.ObjectUtils;

import java.util.ArrayList;
import java.util.List;
import java.util.Locale;

public class WayPointUitls {

    private static final Logger log = LoggerFactory.getLogger(WayPointUitls.class);

    /**
     * kmz转换waypoint，并同时上传到OSS，返回两个url
     *
     * @param kmlInfo
     * @return
     */

    public static String kmz2waypoint(KmlInfo kmlInfo) {
        //准备一个waypoint文件的标头文本
        StringBuilder waypointBuilder = new StringBuilder("QGC WPL 110\n");
        // 重置全局id计数器
        //定义waypoint的航点对象集合备用
        List<AirLinePointDTO> linePoints = new ArrayList<>();
        //m3和m4TD的机型可以从missionConfig获取部分基础数据
        KmlMissionConfig missionConfig = kmlInfo.getDocument().getKmlMissionConfig();
        //其他场景下都可以从missionConfig 拿航点和动作集合
        List<KmlPlacemark> placeMarkList = kmlInfo.getDocument().getFolder().getPlacemarkList();

        if (placeMarkList == null || placeMarkList.isEmpty()) {
            return "";
        }
        //获取kml文件中的第一个坐标
        String[] takeoffCoords;
        if (missionConfig.getTakeOffRefPoint() != null) {
            takeoffCoords = missionConfig.getTakeOffRefPoint().split(",");
        } else {
            // M300/M350：使用第一个航点的坐标
            KmlPlacemark kmlPlacemark = placeMarkList.get(0);
            String[] coords = kmlPlacemark.getKmlPoint().getCoordinates()
                    .replaceAll("[\\n ]", "")
                    .split(",");
            takeoffCoords = new String[]{coords[1], coords[0]}; // 纬度, 经度
        }

        //1.设置waypoint文件的起飞点（第一个command 16 ，不需要高度）
        linePoints.add(buildPoint(16,
                takeoffCoords[0], takeoffCoords[1], 0,
                null, null, null, null, 0, 0, 0
        ));

        //2.设置waypoint文件的安全高度点（command 22，需要高度,使用第一个航点的高度）
        linePoints.add(buildPoint(22,
                takeoffCoords[0], takeoffCoords[1],
                Math.round(Float.parseFloat(placeMarkList.get(0).getHeight())),   //高度转换成整数
                null, null, null, null, 0, 0, 0
        ));

        //3.处理普通航点和航线动作
        for (KmlPlacemark placeMark : placeMarkList) {
            // 航点坐标（纬度+经度的排布顺序）
            KmlPoint point = placeMark.getKmlPoint();
            String cleanCoords = point.getCoordinates().replaceAll("[\n ]", "");
            String[] coords = cleanCoords.split(",");

            // 基础航点（安全点22和结束点20之间的其他16普通航点）
            AirLinePointDTO waypoint = buildPoint(
                    16,
                    coords[1], coords[0],
                    (int) Float.parseFloat(placeMark.getHeight()),
                    null, null, null, null, 0, 0, 0
            );
            linePoints.add(waypoint);

            // 解析该航点对应的附加动作（如拍照、悬停等）
            //先判断动作组有没有动作
            KmlActionGroup actionGroup = placeMark.getActionGroup();
            if (!ObjectUtils.isEmpty(actionGroup)) {
                // 获取动作组中的动作列表
                List<KmlAction> actionList = actionGroup.getAction();

                //动作组没有动作也结束动作组合
                if (!CollectionUtils.isEmpty(actionList)) {
                    // 遍历所有动作
                    for (KmlAction action : actionList) {
                        // 使用全局id累加
                        processActions(action, linePoints);
                    }
                }
            }
        }

        // 4. 返航点（command 20）
        linePoints.add(buildPoint(20, takeoffCoords[0], takeoffCoords[1], Math.round(Float.parseFloat(placeMarkList.get(0).getHeight())), null, null, null, null, 0, 0, 0));

        // 5. 生成Waypoints文件内容
        for (int i = 0; i < linePoints.size(); i++) {
            AirLinePointDTO point = linePoints.get(i);
            waypointBuilder.append(formatWaypointLine(point, i)).append("\n");
        }

        return waypointBuilder.toString();

    }

    // 格式化航点行（直接使用DTO字段）
    public static String formatWaypointLine(AirLinePointDTO point, Integer index) {
        switch (point.getCommand()) {
            case 16:
                if (index == 0) {
                    // 标准航点（第一个16航点的时候，字段3是0）
                    return String.format(Locale.US,
                            "%d\t0\t0\t16\t0\t0\t0\t0\t%s\t%s\t%d.000000\t1",
                            index, point.getLat(), point.getLon(), point.getAlt());
                } else {
                    //其他16的时候，字段3是3
                    return String.format(Locale.US,
                            "%d\t0\t3\t16\t0\t0\t0\t0\t%s\t%s\t%d.000000\t1",
                            index, point.getLat(), point.getLon(), point.getAlt());
                }
            case 22: // 安全高度
                return String.format(Locale.US,
                        "%d\t0\t3\t22\t0\t0\t0\t0\t%s\t%s\t%d.000000\t1",
                        index, point.getLat(), point.getLon(), point.getAlt());

            case 93: // 悬停（参数1=悬停时间）
                return String.format(Locale.US,
                        "%d\t0\t3\t93\t%s\t0\t0\t0\t0\t0\t0.000000\t1", index, point.getLoiterTime());

            case 203: // 拍照（没有参数，只是个触发点）
                return String.format(Locale.US,
                        "%d\t0\t3\t203\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);

            case 205: // 云台角度（参数1=俯仰角，参数3=偏航角）
                return String.format(Locale.US,
                        "%d\t0\t3\t205\t%s\t0\t%s\t0\t0\t0\t0.000000\t1",
                        index, point.getCameraPitch(), point.getCameraYaw());

            case 115: // 旋转无人机（参数1=索引，参数2=角度，参数3=速度写死5，参数4=方向写死1全都代表正向，角度自带正负值避免负负得正，参数5=1默认写死）
                return String.format(Locale.US,
                        "%d\t0\t3\t115\t%s\t5\t1\t1\t0\t0\t0.000000\t1",
                        index, point.getCameraYaw());

            case 531: // 变焦（第二个参数是变焦倍数）
                return String.format(Locale.US,
                        "%d\t0\t3\t531\t0\t%s\t0\t0\t0\t0\t0.000000\t1", index, point.getZoomAbsolute());

            case 532: // 聚焦（没有参数）
                return String.format(Locale.US,
                        "%d\t0\t3\t532\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);

            case 2500: // 视频录制（无参数）
                return String.format("%d\t0\t3\t2500\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);

            case 2501: // 结束录制（无参数）
                return String.format("%d\t0\t3\t2501\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);

            case 20: // 返航 （使用的是第一个航点的经纬度和高度）
                return String.format("%d\t0\t3\t20\t0\t0\t0\t0\t%s\t%s\t%d.000000\t1", index, point.getLat(), point.getLon(), point.getAlt());

            case 4000: // 飞行器偏航角
                return String.format(Locale.US,
                        "%d\t0\t3\t4000\t0\t%s\t0\t0\t0\t0\t0.000000\t1",
                        index, point.getDroneYaw());

            case 5000: // 开始等时间隔拍照
                return String.format(Locale.US,
                        "%d\t0\t3\t5000\t%s\t0\t%s\t0\t0\t0\t0.000000\t1",
                        index, 
                        point.getPhotoTime() != null ? String.valueOf(point.getPhotoTime()) : "0",
                        point.getPhotoType() != null ? String.valueOf(point.getPhotoType()) : "0");

            case 6000: // 开始等间距隔拍照
                return String.format(Locale.US,
                        "%d\t0\t3\t6000\t%s\t0\t%s\t0\t0\t0\t0.000000\t1",
                        index,
                        point.getPhotoDistance() != null ? String.valueOf(point.getPhotoDistance()) : "0",
                        point.getPhotoType() != null ? String.valueOf(point.getPhotoType()) : "0");

            case 7000: // 结束间隔拍照
                return String.format(Locale.US,
                        "%d\t0\t3\t2501\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);

            case 8000: // 全景拍照
                return String.format(Locale.US,
                        "%d\t0\t3\t2501\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);
 
            default:
                throw new IllegalArgumentException("未知命令: " + point.getCommand());
        }
    }

    // 动作解析逻辑
    public static void processActions(KmlAction action, List<AirLinePointDTO> linePoints) {

        //kmz航线的动作code
        String actionType = action.getActionActuatorFunc();
        //动作参数
        KmlActionActuatorFuncParam actionActuatorFuncParam = action.getActionActuatorFuncParam();

        switch (actionType) {
            case "hover":
                // 悬停（命令93）
                linePoints.add(buildPoint(
                        93,
                        "0", "0", 0,
                        actionActuatorFuncParam.getHoverTime(), // loiterTime（悬停时间/s）
                        "0",
                        "0",
                        "0",
                        0,
                        0,
                        0
                ));
                break;

            case "takePhoto":
                // 拍照动作（命令203） 没啥参数
                linePoints.add(buildPoint(
                        203,
                        "0", "0", 0,
                        "0",
                        "0",
                        "0",
                        "0",
                        0,
                        0,
                        0
                ));
                break;

            case "startRecord":
                // 开始录像（命令2500）没啥参数
                linePoints.add(buildPoint(
                        2500,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0
                ));
                break;

            case "stopRecord":
                // 结束录像（命令2501） 没啥参数
                linePoints.add(buildPoint(
                        2501,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0
                ));
                break;

            case "zoom":
                // 变焦动作（通过命令531的参数6实现）
                linePoints.add(buildPoint(
                        531,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0,
                        Math.round(Float.parseFloat(actionActuatorFuncParam.getFocalLength()) / 24)  //FocalLength=kmz的这个值的mm的焦距，除24（一般是最低焦距）取绝对值得到变倍整数
                        , 0
                ));
                break;

            case "focus":
                // 聚焦动作（命令532） 没啥参数
                linePoints.add(buildPoint(
                        532,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0
                ));
                break;

            case "gimbalRotate":
                // 云台控制（命令205）
                linePoints.add(buildPoint(
                        205,
                        "0", "0", 0,
                        "0",
                        String.valueOf(Math.round(Float.parseFloat(actionActuatorFuncParam.getGimbalPitchRotateAngle().toString()))), // 参数1=俯仰角 转换成整数
                        "0",
                        String.valueOf(Math.round(Float.parseFloat(actionActuatorFuncParam.getGimbalYawRotateAngle().toString()))),   // 参数3=偏航角，转换成整数
                        0, 0, 0
                ));
                break;

            case "rotateYaw":
                // 无人机旋转（命令115）
                linePoints.add(buildPoint(
                        115,
                        "0", "0", 0, "0", "0", "0",
                        String.valueOf(Math.round(Float.parseFloat(actionActuatorFuncParam.getAircraftHeading().toString()))),   //旋转角度（偏航角），转换成整数
                        0, 0, "counterClockwise".equals(actionActuatorFuncParam.getAircraftPathMode()) ? -1 : 1    // 旋转方向（逆时针/顺时针）
                ));
                break;

            case "panoPhoto":
                // 全景拍照（命令8000）
                linePoints.add(buildPoint(
                        8000,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0
                ));
                break;

            case "startTimeIntervalPhoto":
                // 开始等时间隔拍照（命令5000）
                linePoints.add(buildPoint(
                        5000,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0,
                        actionActuatorFuncParam.getPhotoTime() != null ? Integer.parseInt(actionActuatorFuncParam.getPhotoTime()) : null,
                        null,
                        actionActuatorFuncParam.getPhotoType() != null ? Integer.parseInt(actionActuatorFuncParam.getPhotoType()) : null,
                        null
                ));
                break;

            case "startDistanceIntervalPhoto":
                // 开始等间距隔拍照（命令6000）
                linePoints.add(buildPoint(
                        6000,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0,
                        null,
                        actionActuatorFuncParam.getPhotoDistance() != null ? Integer.parseInt(actionActuatorFuncParam.getPhotoDistance()) : null,
                        actionActuatorFuncParam.getPhotoType() != null ? Integer.parseInt(actionActuatorFuncParam.getPhotoType()) : null,
                        null
                ));
                break;

            case "stopIntervalPhoto":
                // 结束间隔拍照（命令7000）
                linePoints.add(buildPoint(
                        7000,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0
                ));
                break;

            case "aircraftYaw":
                // 飞行器偏航角（命令4000）
                linePoints.add(buildPoint(
                        4000,
                        "0", "0", 0,
                        "0", "0", "0", "0",
                        0, 0, 0,
                        null, null, null,
                        actionActuatorFuncParam.getDroneYaw()
                ));
                break;

            default:
                log.error("未知动作类型: {}", actionType);
        }
    }


    //构建waypoint航点
    public static AirLinePointDTO buildPoint(int command, String lat, String lon, int alt,
                                            String loiterTime, String cameraPitch, String cameraRoll, String cameraYaw,
                                            int sessionControl, int zoomAbsolute, int rotateDirection) {
        AirLinePointDTO point = new AirLinePointDTO();
        point.setCommand(command);
        point.setLat(lat);
        point.setLon(lon);
        point.setAlt(alt);
        point.setLoiterTime(loiterTime);
        point.setCameraPitch(cameraPitch);
        point.setCameraRoll(cameraRoll);
        point.setCameraYaw(cameraYaw);
        point.setSessionControl(sessionControl);
        point.setZoomAbsolute(zoomAbsolute);
        point.setRotateDirection(rotateDirection);
        return point;
    }

    //构建waypoint航点（带间隔拍照参数）
    public static AirLinePointDTO buildPoint(int command, String lat, String lon, int alt,
                                            String loiterTime, String cameraPitch, String cameraRoll, String cameraYaw,
                                            int sessionControl, int zoomAbsolute, int rotateDirection,
                                            Integer photoTime, Integer photoDistance, Integer photoType, String droneYaw) {
        AirLinePointDTO point = buildPoint(command, lat, lon, alt, loiterTime, cameraPitch, cameraRoll, cameraYaw,
                sessionControl, zoomAbsolute, rotateDirection);
        point.setPhotoTime(photoTime);
        point.setPhotoDistance(photoDistance);
        point.setPhotoType(photoType);
        point.setDroneYaw(droneYaw);
        return point;
    }


}