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());

            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;

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


}