401 lines
16 KiB
Java
401 lines
16 KiB
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(), point.getPhotoType());
|
||
|
||
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(), point.getPhotoType());
|
||
|
||
case 7000: // 结束间隔拍照
|
||
return String.format(Locale.US,
|
||
"%d\t0\t3\t7000\t0\t0\t0\t0\t0\t0\t0.000000\t1", index);
|
||
|
||
case 8000: // 全景拍照
|
||
return String.format(Locale.US,
|
||
"%d\t0\t3\t8000\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,
|
||
actionActuatorFuncParam.getPhotoType(),
|
||
null
|
||
));
|
||
break;
|
||
|
||
case "startDistanceIntervalPhoto":
|
||
// 开始等间距隔拍照（命令6000）
|
||
linePoints.add(buildPoint(
|
||
6000,
|
||
"0", "0", 0,
|
||
"0", "0", "0", "0",
|
||
0, 0, 0,
|
||
null,
|
||
actionActuatorFuncParam.getPhotoDistance(),
|
||
actionActuatorFuncParam.getPhotoType(),
|
||
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,
|
||
String photoTime, String photoDistance, String 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;
|
||
}
|
||
|
||
|
||
}
|