无人机java后台代码
blame | history | patch | commit | commitdiff | ignore whitespace
rain
2024-07-30 d81fdeef2ad2f7ecc548cebad6ab15455bf3da27 
 src/main/java/com/dji/sample/patches/xml/mode/XMLTemplateModel.java


@@ -1,12 +1,14 @@


package com.dji.sample.patches.xml.mode;




import com.dji.sample.patches.model.entity.LotInfo;


import com.dji.sample.patches.utils.DistanceCalculator;


import com.dji.sample.patches.utils.GeoToolsUtil;


import com.dji.sample.patches.xml.mode.share.ActionGroup;


import com.dji.sample.patches.xml.mode.share.ActionMode;


import com.dji.sample.patches.xml.mode.share.ActionTrigger;


import com.dji.sample.patches.utils.PointPO;


import com.dji.sample.patches.xml.mode.share.*;


import com.dji.sample.patches.xml.mode.share.action.utils.*;


import com.dji.sample.patches.xml.utils.CreateWaylineFileUtils;


import com.dji.sample.wayline.plane.PlaneCourseUtils;


import com.dji.sample.wayline.plane.param.CreateWaylineParam;


import freemarker.template.Configuration;


import freemarker.template.Template;


import lombok.Builder;


@@ -42,51 +44,62 @@




    /**


     * 初始化模板对象


     *


     * @param coordinates


     * @param lotInfos


     * @return


     */


    public static XMLTemplateModel init(Coordinate[] coordinates,List<LotInfo> lotInfos) {


    public static XMLTemplateModel init(List<PointPO> coordinates, List<LotInfo> lotInfos) {




        Folder folder = FolderUtils.setFloder();




        List<Placemark> placemarkList = new ArrayList<>();


        int i = 0;


        for (Coordinate c:coordinates) {


        for (PointPO pointPO : coordinates) {


            if (i != 0) {//去除第一个航点


                Coordinate c = pointPO.getCoordinate();


                Placemark placemark = PlacemarkUtils.setPlacemark(c.x + "," + c.y);




                // 增加事件组


                ActionGroup actionGroup = new ActionGroup();


                actionGroup.setActionGroupId(i-1);//动作组id从0开始单调连续递增。


                actionGroup.setActionGroupStartIndex(i-1);//动作组开始生效的航点


                actionGroup.setActionGroupEndIndex(i-1);//动作组结束生效的航点


                actionGroup.setActionGroupId(i - 1);//动作组id从0开始单调连续递增。


                actionGroup.setActionGroupStartIndex(i - 1);//动作组开始生效的航点


                actionGroup.setActionGroupEndIndex(i - 1);//动作组结束生效的航点


                actionGroup.setActionGroupMode("sequence");


                ActionTrigger at = new ActionTrigger();


                at.setActionTriggerType("reachPoint");


                actionGroup.setActionTrigger(at);


                // 开始增加事件


                List<ActionMode> list = new ArrayList<>();


                ActionMode actionMode = new ActionMode();


                actionMode.setActionId(0);


                actionMode.setActionActuatorFunc(CameraActionEnum.TAKE_PHOTO.getDescription());//拍照事件


                // 计算获取图斑编号


                int quotient = i / 4;


                if (i % 4 != 0) { // 检查是否有余数


                    quotient++; // 如果有余数，商加1


                }




                if (quotient == 0) quotient = 1;


                //拍照事件


                ActionMode actionMode = new ActionMode();


                actionMode.setActionId(2);


                actionMode.setActionActuatorFunc(CameraActionEnum.TAKE_PHOTO.getDescription());//拍照事件




//                actionMode.setActionActuatorFuncParam(ActionUtils.setTakePhoto(lotInfos.get(quotient-1).getDkbh()));


                actionMode.setActionActuatorFuncParam(ActionUtils.setTakePhoto("测试"));


                list.add(actionMode);


                // 设置媒体文件名称后缀


                actionMode.setActionActuatorFuncParam(ActionUtils.setTakePhoto(lotInfos.get(pointPO.getIndex()).getDkbh() + "~" + lotInfos.get(pointPO.getIndex()).getTaskId()));


//                actionMode.setActionActuatorFuncParam(ActionUtils.setTakePhoto("测试"));




                //云台旋转事件


                ActionMode actionMode2 = new ActionMode();


                actionMode2.setActionId(1);


                actionMode2.setActionActuatorFunc(CameraActionEnum.GIMBAL_ROTATE.getDescription());//拍照事件


                actionMode2.setActionActuatorFuncParam(ActionUtils.setGimbalRotate());


                list.add(actionMode2);


                actionMode2.setActionActuatorFunc(CameraActionEnum.GIMBAL_ROTATE.getDescription());//旋转云台


                //设置转动角度


                double bearing = GeoToolsUtil.bearing(pointPO.getCoordinate().y, pointPO.getCoordinate().x, pointPO.getCentro().y, pointPO.getCentro().x);


//                System.out.println("角度：" + bearing);


                actionMode2.setActionActuatorFuncParam(ActionUtils.setGimbalRotate(bearing));




                // 飞行器偏航事件


                ActionMode actionModeFxqph = new ActionMode();


                actionModeFxqph.setActionId(0);


                actionModeFxqph.setActionActuatorFunc(CameraActionEnum.ROTATE_YAW.getDescription());//飞行器偏航


                actionModeFxqph.setActionActuatorFuncParam(ActionUtils.setRotateYaw(bearing));




                list.add(actionModeFxqph);//添加飞行器偏航


                list.add(actionMode2);//添加云台旋转


                list.add(actionMode);//添加拍照


                actionGroup.setActions(list);




                placemark.setActionGroup(actionGroup);


@@ -94,7 +107,7 @@


                placemarkList.add(placemark);


            }




            i = i+1;


            i = i + 1;


        }




        folder.setPlacemarkList(placemarkList);


@@ -103,41 +116,256 @@


                .author("Aix")


                .createTime(String.valueOf(System.currentTimeMillis()))


                .updateTime(String.valueOf(System.currentTimeMillis()))


                .missionConfig(MissionConfigUtils.setMissionConfig(coordinates[0].y + "," + coordinates[0].x + ",0"))


                .missionConfig(MissionConfigUtils.setMissionConfig(coordinates.get(0).getCoordinate().y + "," + coordinates.get(0).getCoordinate().x + ",0"))


                .folder(folder)


                .build();




        return xtm;


    }




    /**


     * 面状航线


     *


     * @return


     */


    public static XMLTemplateModel initPolygon(CreateWaylineParam param) {




        Folder folder = FolderUtils.setFloder(param.getTemplateType(), param.getAutoFlightSpeed(),param.getExecuteHeightMode());




        //坐标系参数


        WaylineCoordinateSysParam wcs = new WaylineCoordinateSysParam("EGM96", String.valueOf(param.getHeight()), String.valueOf(param.getHeight()));


        folder.setWaylineCoordinateSysParam(wcs);




        //建图航拍模板元素


        PlacemarkByPolygon placemarkByPolygon = new PlacemarkByPolygon();


        placemarkByPolygon.setDirection(param.getBearing());






        // 重叠率参数


        Overlap overlap = new Overlap((int) (param.getCourseRatio()* 100),


                (int) (param.getSideRatio()* 100),


                (int) (param.getCourseRatio()* 100),


                (int) (param.getSideRatio()* 100));


        placemarkByPolygon.setOverlap(overlap);




        //测区多边形


        String polygons = "";


        List<String> polygonStrList = new ArrayList<>();


        for (double [] polygon:param.getPolygon()){


            polygons = polygon[0] + "," + polygon[1] + ",0";


            polygonStrList.add(polygons);


        }


        placemarkByPolygon.setPolygons(polygonStrList);




        //全局航线高度


        placemarkByPolygon.setEllipsoidHeight(param.getHeight());


        placemarkByPolygon.setHeight(param.getHeight());




        folder.setPlacemarkByPolygon(placemarkByPolygon);




        //航点


        List<Coordinate> pointList = PlaneCourseUtils.createWaylinePoints(param);


        List<Placemark> placemarkList = new ArrayList<>();


        //计算距离


        Coordinate droneC = param.getCoordinate();//机场经纬度


        int pointListIndex = 0;


        double distanceOne = 0.0; //第一个点到机场得距离


        double distance = 0.0;


        Coordinate pointTmp = null;


        for (Coordinate c:pointList)  {


            //计算距离


            if (pointListIndex == 0) {


                distanceOne = DistanceCalculator.calculateDistance(droneC.y,droneC.x,c.y,c.x) * 1000;


                distance += distanceOne;


                pointTmp = c;


            } else {


                double distanceTmp = DistanceCalculator.calculateDistance(pointTmp.y,pointTmp.x,c.y,c.x) * 1000;


                distance += distanceTmp;


                pointTmp = c;


            }


            pointListIndex += 1;




            Placemark placemark = new Placemark();


            placemark.setCoordinates(c.x + "," + c.y);


            placemark.setEllipsoidHeight(param.getHeight());


            placemark.setHeight(param.getHeight());


            placemark.setExecuteHeight(param.getHeight()); //执行高度 = 机场高度 + 飞行高度 *修改为前端直接传




            placemark.setWaypointSpeed(param.getAutoFlightSpeed() + "");


            placemark.setUseStraightLine(1);




            //


            WaypointTurnParam waypointTurnParam = new WaypointTurnParam();


            waypointTurnParam.setWaypointTurnMode("coordinateTurn");


            waypointTurnParam.setWaypointTurnDampingDist("0.2");


            //添加事件  第一个和结尾需要添加事件


            if (pointListIndex == 1) { //第一个航点事件




                waypointTurnParam.setWaypointTurnMode("toPointAndStopWithDiscontinuityCurvature");


                waypointTurnParam.setWaypointTurnDampingDist("0");




                // 增加事件组


                ActionGroup actionGroup = new ActionGroup();


                actionGroup.setActionGroupId(0);


                actionGroup.setActionGroupStartIndex(pointListIndex - 1);//动作组开始生效的航点


                actionGroup.setActionGroupEndIndex(pointList.size() - 1);//动作组结束生效的航点


                actionGroup.setActionGroupMode("sequence");


                ActionTrigger at = new ActionTrigger();


                at.setActionTriggerType("betweenAdjacentPoints");


                actionGroup.setActionTrigger(at);


                // 开始增加事件


                List<ActionMode> list = new ArrayList<>();


                ActionMode actionMode = new ActionMode();




                actionMode.setActionId(0);


                actionMode.setActionActuatorFunc("gimbalAngleLock");




                ActionMode actionMode2 = new ActionMode();


                actionMode2.setActionId(1);


                actionMode2.setActionActuatorFunc(CameraActionEnum.GIMBAL_ROTATE.getDescription());//旋转云台


                actionMode2.setActionActuatorFuncParam(ActionUtils.setGimbalRotateByPlane());




                ActionMode actionMode3 = new ActionMode();


                actionMode3.setActionId(2);


                actionMode3.setActionActuatorFunc("startTimeLapse");


                actionMode3.setActionActuatorFuncParam(ActionUtils.setStartTimeLapseByPlane());




                list.add(actionMode);


                list.add(actionMode2);


                list.add(actionMode3);


                actionGroup.setActions(list);




                placemark.setActionGroup(actionGroup);




            } else if(pointListIndex == pointList.size()) { // 结尾需要添加事件




                waypointTurnParam.setWaypointTurnMode("toPointAndStopWithDiscontinuityCurvature");


                waypointTurnParam.setWaypointTurnDampingDist("0");




                // 增加事件组


                ActionGroup actionGroup = new ActionGroup();


                actionGroup.setActionGroupId(1);


                actionGroup.setActionGroupStartIndex(pointListIndex - 1);//动作组开始生效的航点


                actionGroup.setActionGroupEndIndex(pointList.size()-1);//动作组结束生效的航点


                actionGroup.setActionGroupMode("sequence");


                ActionTrigger at = new ActionTrigger();


                at.setActionTriggerType("reachPoint");


                actionGroup.setActionTrigger(at);


                // 开始增加事件


                List<ActionMode> list = new ArrayList<>();




                ActionMode actionMode = new ActionMode();


                actionMode.setActionId(0);


                actionMode.setActionActuatorFunc("stopTimeLapse");


                actionMode.setActionActuatorFuncParam(ActionUtils.setStopTimeLapseByPlane());




                ActionMode actionMode2 = new ActionMode();


                actionMode2.setActionId(1);


                actionMode2.setActionActuatorFunc("gimbalAngleUnlock");




                list.add(actionMode);


                list.add(actionMode2);


                actionGroup.setActions(list);




                placemark.setActionGroup(actionGroup);


            }




            placemark.setWaypointTurnParam(waypointTurnParam);




            placemarkList.add(placemark);


        }




        //距离和时间


        folder.setDistance((distance - distanceOne) + "");


        folder.setDuration((distance / param.getAutoFlightSpeed()) + "");//时间 = 长度 / 速度




        folder.setPlacemarkList(placemarkList);




        XMLTemplateModel xtm = XMLTemplateModel.builder()


                .author("Aix")


                .createTime(String.valueOf(System.currentTimeMillis()))


                .updateTime(String.valueOf(System.currentTimeMillis()))


                .missionConfig(MissionConfigUtils.setMissionConfigByPolygon(param.getCoordinate().y + "," + param.getCoordinate().x + ",0",


                        param.getAutoFlightSpeed(),


                        param.getDroneInfo(),


                        param.getPayloadInfo()))


                .folder(folder)


                .build();




        return xtm;


    }




    //新建面状航线测试


    public static void main(String[] args) {




        //测试


        List<LotInfo> list = new ArrayList<>();


        list.add(LotInfo.builder().dkbh("dkbh01").dkfw("POLYGON((115.866465564947 28.6344502965542, 115.86425430209 28.6357383285408, 115.864551734716 28.633120921433, 115.866977149064 28.6338435339976, 115.866465564947 28.6344502965542))").build());


        list.add(LotInfo.builder().dkbh("dkbh02").dkfw("POLYGON((115.864006690605 28.6202713913694, 115.86002109342 28.6162025130492, 115.866374254306 28.6142037658042, 115.865912044006 28.6172001020759, 115.864006690605 28.6202713913694))").build());


        list.add(LotInfo.builder().dkbh("dkbh03").dkfw("POLYGON((115.839366933455 28.6161999317332, 115.841288489469 28.6160843601496, 115.840931570318 28.6181544912247, 115.838147600941 28.618654178036, 115.839366933455 28.6161999317332))").build());


//        list.add(LotInfo.builder().dkbh("dkbh04").dkfw("POLYGON((115.857499052697 28.6784702230642, 115.859109158101 28.6762273976226, 115.863677723232 28.6766081113836, 115.862154868188 28.6790827508297, 115.857499052697 28.6784702230642))").build());


//        list.add(LotInfo.builder().dkbh("dkbh05").dkfw("POLYGON((115.834974056705 28.6659171428962, 115.833760531592 28.6634960413229, 115.832422084777 28.6624550271329, 115.829745191145 28.6631986086972, 115.831232354274 28.6608191476914, 115.833314382654 28.6603729987527, 115.835545127347 28.6618601618814, 115.837032290475 28.6639421902615, 115.834974056705 28.6659171428962))").build());


//        list.add(LotInfo.builder().dkbh("dkbh06").dkfw("POLYGON((115.885622116006 28.5766308429787, 115.883936664461 28.5771582901683, 115.883365593819 28.5752547213636, 115.883555950699 28.5740174016407, 115.88365112914 28.5724945465969, 115.885364341064 28.5721138328361, 115.886696839227 28.5725897250371, 115.887458266749 28.5736366878797, 115.886792017668 28.5753498998039, 115.885622116006 28.5766308429787))").build());


//        list.add(LotInfo.builder().dkbh("dkbh07").dkfw("POLYGON((115.857644341395 28.5750890964568, 115.857572957565 28.5729475815515, 115.858429563527 28.5728761977213, 115.859072017998 28.5738041875136, 115.859072017998 28.5748035611361, 115.857644341395 28.5750890964568))").build());


//        list.add(LotInfo.builder().dkbh("dkbh08").dkfw("POLYGON((115.912181587649 28.6231542087745, 115.912181587649 28.6215123806805, 115.915893546818 28.6212268453598, 115.916036314478 28.6231542087745, 115.912181587649 28.6231542087745))").build());


//        list.add(LotInfo.builder().dkbh("dkbh09").dkfw("POLYGON((115.842039042965 28.6314426646115, 115.840992080122 28.631252307731, 115.842324578286 28.6305860586493, 115.843181184248 28.6305860586493, 115.84403779021 28.6304908802091, 115.84394261177 28.6317281999322, 115.842039042965 28.6314426646115))").build());


//        list.add(LotInfo.builder().dkbh("dkbh10").dkfw("POLYGON((115.807011889796 28.623935465138, 115.805869748513 28.6224126100944, 115.810247956764 28.6220318963334, 115.809581707682 28.623935465138, 115.807011889796 28.623935465138))").build());




        // 机场经纬度


        double airportLat = 28.624514734; // 机场纬度


        double airportLon = 115.856725497; // 机场经度


        // 解析图斑生成航点，按顺序返回


        Coordinate[] coordinates = GeoToolsUtil.getRoutePointOrder(list, airportLat, airportLon);


        // 初始化模板对象


        CreateWaylineParam param = new CreateWaylineParam();


        param.setBearing(90);


        param.setCoordinate(new Coordinate(116.020940643,25.8917266));


        param.setHeight(372.3);


        param.setCourseRatio(0.8);


        param.setSideRatio(0.8);


        param.setFocal(10*6.9999943);


        param.setFrame(100);


        param.setDroneHeight(14.731968792284789);


        List<double[]> polygon = new ArrayList<>();


        double[] a = {116.028037250229,25.8948290570725};


        double[] a2 = {116.031565260299,25.8950672687002};


        double[] a3 = {116.032112214864,25.8901339309971};


        double[] a4 = {116.028726270654,25.8895106128711};


        polygon.add(a);


        polygon.add(a2);


        polygon.add(a3);


        polygon.add(a4);


        param.setPolygon(polygon);


        param.setTemplateType("mapping2d");


        param.setAutoFlightSpeed(15);


        DroneInfo info = new DroneInfo();


        info.setDroneEnumValue("91");


        info.setDroneSubEnumValue("0");


        param.setDroneInfo(info);


        PayloadInfo payloadInfo = new PayloadInfo();


        payloadInfo.setPayloadEnumValue("80");


        payloadInfo.setPayloadPositionIndex("0");


        payloadInfo.setPayloadSubEnumValue("0");


        param.setPayloadInfo(payloadInfo);




        // 初始化模板对象


        XMLTemplateModel xmlModel = XMLTemplateModel.init(coordinates, list);


        XMLTemplateModel xmlModel = XMLTemplateModel.initPolygon(param);




        //生成航线文件


        CreateWaylineFileUtils.createWaylineFile(xmlModel,"src\\main\\resources\\template\\template.xml","src\\main\\resources\\template\\wpmz\\template.xml","src\\main\\resources\\template\\waylines.xml","src\\main\\resources\\template\\wpmz\\waylines.xml");






        CreateWaylineFileUtils.createWaylineFileByPolygon(xmlModel, "src\\main\\resources\\template\\template-polygon.xml", "src\\main\\resources\\template\\wpmz\\template.kml");


        CreateWaylineFileUtils.createWaylineFileByPolygon(xmlModel, "src\\main\\resources\\template\\waylines-polygon.xml", "src\\main\\resources\\template\\wpmz\\waylines.wpml");


    }




    //新建点航线测试


//    public static void main(String[] args) {


//


//        //测试


//        List<LotInfo> list = new ArrayList<>();


//        list.add(LotInfo.builder().dkbh("dkbh01").dkfw("POLYGON((115.866465564947 28.6344502965542, 115.86425430209 28.6357383285408, 115.864551734716 28.633120921433, 115.866977149064 28.6338435339976, 115.866465564947 28.6344502965542))").build());


//        list.add(LotInfo.builder().dkbh("dkbh02").dkfw("POLYGON((115.864006690605 28.6202713913694, 115.86002109342 28.6162025130492, 115.866374254306 28.6142037658042, 115.865912044006 28.6172001020759, 115.864006690605 28.6202713913694))").build());


//        list.add(LotInfo.builder().dkbh("dkbh03").dkfw("POLYGON((115.839366933455 28.6161999317332, 115.841288489469 28.6160843601496, 115.840931570318 28.6181544912247, 115.838147600941 28.618654178036, 115.839366933455 28.6161999317332))").build());


////        list.add(LotInfo.builder().dkbh("dkbh04").dkfw("POLYGON((115.857499052697 28.6784702230642, 115.859109158101 28.6762273976226, 115.863677723232 28.6766081113836, 115.862154868188 28.6790827508297, 115.857499052697 28.6784702230642))").build());


////        list.add(LotInfo.builder().dkbh("dkbh05").dkfw("POLYGON((115.834974056705 28.6659171428962, 115.833760531592 28.6634960413229, 115.832422084777 28.6624550271329, 115.829745191145 28.6631986086972, 115.831232354274 28.6608191476914, 115.833314382654 28.6603729987527, 115.835545127347 28.6618601618814, 115.837032290475 28.6639421902615, 115.834974056705 28.6659171428962))").build());


////        list.add(LotInfo.builder().dkbh("dkbh06").dkfw("POLYGON((115.885622116006 28.5766308429787, 115.883936664461 28.5771582901683, 115.883365593819 28.5752547213636, 115.883555950699 28.5740174016407, 115.88365112914 28.5724945465969, 115.885364341064 28.5721138328361, 115.886696839227 28.5725897250371, 115.887458266749 28.5736366878797, 115.886792017668 28.5753498998039, 115.885622116006 28.5766308429787))").build());


////        list.add(LotInfo.builder().dkbh("dkbh07").dkfw("POLYGON((115.857644341395 28.5750890964568, 115.857572957565 28.5729475815515, 115.858429563527 28.5728761977213, 115.859072017998 28.5738041875136, 115.859072017998 28.5748035611361, 115.857644341395 28.5750890964568))").build());


////        list.add(LotInfo.builder().dkbh("dkbh08").dkfw("POLYGON((115.912181587649 28.6231542087745, 115.912181587649 28.6215123806805, 115.915893546818 28.6212268453598, 115.916036314478 28.6231542087745, 115.912181587649 28.6231542087745))").build());


////        list.add(LotInfo.builder().dkbh("dkbh09").dkfw("POLYGON((115.842039042965 28.6314426646115, 115.840992080122 28.631252307731, 115.842324578286 28.6305860586493, 115.843181184248 28.6305860586493, 115.84403779021 28.6304908802091, 115.84394261177 28.6317281999322, 115.842039042965 28.6314426646115))").build());


////        list.add(LotInfo.builder().dkbh("dkbh10").dkfw("POLYGON((115.807011889796 28.623935465138, 115.805869748513 28.6224126100944, 115.810247956764 28.6220318963334, 115.809581707682 28.623935465138, 115.807011889796 28.623935465138))").build());


//


//        // 机场经纬度


//        double airportLat = 28.624514734; // 机场纬度


//        double airportLon = 115.856725497; // 机场经度


//        // 解析图斑生成航点，按顺序返回


//        List<PointPO> coordinates = GeoToolsUtil.getRoutePointOrder(list, airportLat, airportLon);


//


//        // 初始化模板对象


//        XMLTemplateModel xmlModel = XMLTemplateModel.init(coordinates, list);


//


//        //生成航线文件


//        CreateWaylineFileUtils.createWaylineFile(xmlModel, "src\\main\\resources\\template\\template.kml", "src\\main\\resources\\template\\wpmz\\template.kml", "src\\main\\resources\\template\\waylines.wpml", "src\\main\\resources\\template\\wpmz\\waylines.wpml");


//


//


//    }




}