package com.dji.sample.control.service.impl;
|
|
import com.alibaba.fastjson.JSONObject;
|
import com.dji.sample.common.error.CommonErrorEnum;
|
import com.dji.sample.common.model.ResponseResult;
|
import com.dji.sample.common.util.SpringBeanUtils;
|
import com.dji.sample.component.mqtt.model.*;
|
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.component.websocket.model.BizCodeEnum;
|
import com.dji.sample.component.websocket.service.ISendMessageService;
|
import com.dji.sample.control.model.dto.FlyToProgressReceiver;
|
import com.dji.sample.control.model.dto.PointDTO;
|
import com.dji.sample.control.model.dto.ResultNotifyDTO;
|
import com.dji.sample.control.model.dto.TakeoffProgressReceiver;
|
import com.dji.sample.control.model.enums.*;
|
import com.dji.sample.control.model.param.*;
|
import com.dji.sample.control.service.IControlService;
|
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.UserTypeEnum;
|
import com.dji.sample.manage.service.IDevicePayloadService;
|
import com.dji.sample.manage.service.IDeviceRedisService;
|
import com.dji.sample.manage.service.IDeviceService;
|
import com.dji.sample.wayline.model.enums.WaylineErrorCodeEnum;
|
import com.dji.sample.wayline.model.param.PointPOJO;
|
import com.fasterxml.jackson.core.type.TypeReference;
|
import com.fasterxml.jackson.databind.ObjectMapper;
|
import lombok.extern.slf4j.Slf4j;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.integration.annotation.ServiceActivator;
|
import org.springframework.messaging.MessageHeaders;
|
import org.springframework.stereotype.Service;
|
import org.springframework.util.StringUtils;
|
|
import java.util.*;
|
|
/**
|
* @author sean
|
* @version 1.2
|
* @date 2022/7/29
|
*/
|
@Service
|
@Slf4j
|
public class ControlServiceImpl implements IControlService {
|
|
@Autowired
|
private IMessageSenderService messageSenderService;
|
|
@Autowired
|
private ISendMessageService webSocketMessageService;
|
|
@Autowired
|
private IDeviceService deviceService;
|
|
@Autowired
|
private IDeviceRedisService deviceRedisService;
|
|
@Autowired
|
private ObjectMapper mapper;
|
|
@Autowired
|
private IDevicePayloadService devicePayloadService;
|
|
private RemoteDebugHandler checkDebugCondition(String sn, RemoteDebugParam param, RemoteDebugMethodEnum controlMethodEnum) {
|
RemoteDebugHandler handler = Objects.nonNull(controlMethodEnum.getClazz()) ?
|
mapper.convertValue(Objects.nonNull(param) ? param : new Object(), controlMethodEnum.getClazz())
|
: new RemoteDebugHandler();
|
if (!handler.canPublish(sn)) {
|
throw new RuntimeException("当前的机场状态不支持此功能。");
|
}
|
if (Objects.nonNull(param) && !handler.valid()) {
|
throw new RuntimeException(CommonErrorEnum.ILLEGAL_ARGUMENT.getErrorMsg());
|
}
|
return handler;
|
}
|
|
@Override
|
public ResponseResult controlDockDebug(String sn, String serviceIdentifier, RemoteDebugParam param) {
|
RemoteDebugMethodEnum controlMethodEnum = RemoteDebugMethodEnum.find(serviceIdentifier);
|
if (RemoteDebugMethodEnum.UNKNOWN == controlMethodEnum) {
|
return ResponseResult.error(" 没有找到" + serviceIdentifier + " 这个指令。");
|
}
|
|
RemoteDebugHandler data = checkDebugCondition(sn, param, controlMethodEnum);
|
|
boolean isExist = deviceRedisService.checkDeviceOnline(sn);
|
if (!isExist) {
|
return ResponseResult.error("设备离线");
|
}
|
String bid = UUID.randomUUID().toString();
|
ServiceReply serviceReply = messageSenderService.publishServicesTopic(sn, serviceIdentifier, data, bid);
|
|
if (ResponseResult.CODE_SUCCESS != serviceReply.getResult()) {
|
return ResponseResult.error(serviceReply.getResult(),
|
"error: " + serviceIdentifier + serviceReply.getResult());
|
}
|
if (controlMethodEnum.getProgress()) {
|
RedisOpsUtils.setWithExpire(serviceIdentifier + RedisConst.DELIMITER + bid, sn,
|
RedisConst.DEVICE_ALIVE_SECOND * RedisConst.DEVICE_ALIVE_SECOND);
|
}
|
|
//当执行返航指令时，删除图斑redis
|
if (serviceIdentifier == "return_home"){
|
if (RedisOpsUtils.checkExist("tuban:" + sn)){
|
RedisOpsUtils.del("tuban:" + sn);
|
}
|
}
|
|
|
|
return ResponseResult.success();
|
}
|
|
/**
|
* Handles multi-state command progress information.
|
*
|
* @param receiver
|
* @param headers
|
* @return
|
*/
|
@ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_CONTROL_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
|
public CommonTopicReceiver handleControlProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
|
String key = receiver.getMethod() + RedisConst.DELIMITER + receiver.getBid();
|
if (RedisOpsUtils.getExpire(key) <= 0) {
|
return receiver;
|
}
|
String sn = RedisOpsUtils.get(key).toString();
|
|
EventsReceiver<EventsOutputProgressReceiver> eventsReceiver = mapper.convertValue(receiver.getData(),
|
new TypeReference<EventsReceiver<EventsOutputProgressReceiver>>() {
|
});
|
eventsReceiver.setBid(receiver.getBid());
|
eventsReceiver.setSn(sn);
|
|
log.info("SN: {}, {} ===> Control progress: {}",
|
sn, receiver.getMethod(), eventsReceiver.getOutput().getProgress().toString());
|
|
if (eventsReceiver.getResult() != ResponseResult.CODE_SUCCESS) {
|
log.error("SN: {}, {} ===> Error code: {}", sn, receiver.getMethod(), eventsReceiver.getResult());
|
}
|
|
if (eventsReceiver.getOutput().getProgress().getPercent() == 100 ||
|
EventsResultStatusEnum.find(eventsReceiver.getOutput().getStatus()).getEnd()) {
|
RedisOpsUtils.del(key);
|
}
|
|
Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(sn);
|
|
if (deviceOpt.isEmpty()) {
|
throw new RuntimeException("设备离线.");
|
}
|
|
DeviceDTO device = deviceOpt.get();
|
webSocketMessageService.sendBatch(device.getWorkspaceId(), UserTypeEnum.WEB.getVal(),
|
receiver.getMethod(), eventsReceiver);
|
|
return receiver;
|
}
|
|
private void checkFlyToCondition(String dockSn) {
|
// TODO 设备固件版本不兼容情况
|
Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(dockSn);
|
if (dockOpt.isEmpty()) {
|
throw new RuntimeException("机场离线请重启机场");
|
}
|
|
DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(dockOpt.get().getChildDeviceSn());
|
if (DeviceModeCodeEnum.MANUAL != deviceMode) {
|
throw new RuntimeException("无人机当前状态不支持此功能，请稍后再试");
|
}
|
|
ResponseResult result = seizeAuthority(dockSn, DroneAuthorityEnum.FLIGHT, null);
|
if (ResponseResult.CODE_SUCCESS != result.getCode()) {
|
throw new IllegalArgumentException(result.getMessage());
|
}
|
}
|
|
@Override
|
public ResponseResult flyToPoint(String sn, FlyToPointParam param) {
|
checkFlyToCondition(sn);
|
|
param.setFlyToId(UUID.randomUUID().toString());
|
ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.FLY_TO_POINT.getMethod(), param, param.getFlyToId());
|
return ResponseResult.CODE_SUCCESS != reply.getResult() ?
|
ResponseResult.error("飞向目标点失败。" + reply.getResult())
|
: ResponseResult.success();
|
}
|
|
@Override
|
public ResponseResult flyToPointStop(String sn) {
|
ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.FLY_TO_POINT_STOP.getMethod(), null);
|
return ResponseResult.CODE_SUCCESS != reply.getResult() ?
|
ResponseResult.error("飞向目标点的无人机停止失败" + reply.getResult())
|
: ResponseResult.success();
|
}
|
|
@ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_FLY_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
|
public CommonTopicReceiver handleFlyToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) throws Exception {
|
String dockSn = receiver.getGateway();
|
|
Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
|
if (deviceOpt.isEmpty()) {
|
log.error("机场离线");
|
return null;
|
}
|
|
FlyToProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<FlyToProgressReceiver>() {
|
});
|
webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
|
BizCodeEnum.FLY_TO_POINT_PROGRESS.getCode(),
|
ResultNotifyDTO.builder().sn(dockSn)
|
.message(WaylineErrorCodeEnum.SUCCESS == eventsReceiver.getResult() ?
|
eventsReceiver.getStatus().getMessage() : eventsReceiver.getResult().getErrorMsg())
|
.result(eventsReceiver.getResult().getErrorCode())
|
.build());
|
//当飞向目标点成功后
|
if (eventsReceiver.getStatus().equals(FlyToStatusEnum.WAYLINE_OK)) {
|
JSONObject jsonObject = (JSONObject) RedisOpsUtils.get("tuban:" + dockSn);
|
if (jsonObject != null) {
|
List<PointPOJO> targetList = (List<PointPOJO>) jsonObject.get("targetList");
|
int curIndex = (Integer) jsonObject.get("curIndex");
|
String payloadIndex = jsonObject.getString("payloadIndex");
|
flyToNextPoint(targetList, curIndex+1, dockSn, payloadIndex);
|
}
|
}
|
return receiver;
|
}
|
|
@Override
|
public ResponseResult flyToNextPoint(List<PointPOJO> targetList, int curIndex, String sn, String payloadIndex) throws Exception {
|
//当无人机状态为人工时再发布下一个命令
|
while (true) {
|
Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(sn);
|
DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(dockOpt.get().getChildDeviceSn());
|
|
if (DeviceModeCodeEnum.MANUAL == deviceMode) {
|
|
//执行拍照
|
ResponseResult responseResult = takePhoto(sn, payloadIndex);
|
|
//发布飞行指令
|
if (curIndex == targetList.size()) {
|
//当前是最后一个点，返航
|
ResponseResult returnHome = controlDockDebug(sn, "return_home", null);
|
|
RedisOpsUtils.del("tuban:" + sn);
|
|
return returnHome;
|
} else {
|
//当前不是最后一个点，飞行到下一个点
|
FlyToPointParam flyToPointParam = new FlyToPointParam();
|
flyToPointParam.setMaxSpeed(14);
|
List<PointDTO> pointDTOS = new ArrayList<>();
|
|
PointDTO pointDTO = new PointDTO();
|
pointDTO.setHeight(150.0);
|
pointDTO.setLongitude(targetList.get(curIndex).getLon());
|
pointDTO.setLatitude(targetList.get(curIndex).getLat());
|
pointDTOS.add(pointDTO);
|
flyToPointParam.setPoints(pointDTOS);
|
|
//发布下一个飞行指令
|
ResponseResult flyToRes = flyToPoint(sn, flyToPointParam);
|
if (flyToRes.getCode() == ResponseResult.CODE_SUCCESS) {
|
JSONObject jsonObject = new JSONObject();
|
jsonObject.put("targetList", targetList);
|
jsonObject.put("curIndex", curIndex);
|
jsonObject.put("payloadIndex",payloadIndex);
|
RedisOpsUtils.set("tuban:" + sn, jsonObject);
|
}
|
return flyToRes;
|
|
}
|
}
|
}
|
}
|
|
@Override
|
public ResponseResult takePhoto(String sn, String payloadIndex) throws Exception {
|
|
//获取负载控制权
|
DronePayloadParam dronePayloadParam = new DronePayloadParam();
|
dronePayloadParam.setPayloadIndex(payloadIndex);
|
|
ResponseResult seizeAuthorityRes = seizeAuthority(sn, DroneAuthorityEnum.PAYLOAD, dronePayloadParam);
|
|
//切换为相机模式
|
if (seizeAuthorityRes.getCode() != ResponseResult.CODE_SUCCESS) {
|
return seizeAuthorityRes;
|
}
|
|
PayloadCommandsParam payloadCommandsParam = new PayloadCommandsParam();
|
|
// DronePayloadParam switchParam = new DronePayloadParam();
|
// switchParam.setCameraMode(CameraModeEnum.PHOTO);
|
// switchParam.setPayloadIndex(payloadIndex);
|
|
// payloadCommandsParam.setSn(sn);
|
// payloadCommandsParam.setCmd(PayloadCommandsEnum.CAMERA_MODE_SWitCH);
|
// payloadCommandsParam.setData(switchParam);
|
//
|
// ResponseResult switchModeRes = payloadCommands(payloadCommandsParam);
|
|
// if (switchModeRes.getCode() != ResponseResult.CODE_SUCCESS){
|
// return switchModeRes;
|
// }
|
//拍照
|
payloadCommandsParam.setCmd(PayloadCommandsEnum.CAMERA_PHOTO_TAKE);
|
DronePayloadParam takePhotoParam = new DronePayloadParam();
|
takePhotoParam.setPayloadIndex(payloadIndex);
|
|
payloadCommandsParam.setData(takePhotoParam);
|
payloadCommandsParam.setSn(sn);
|
|
ResponseResult responseResult = payloadCommands(payloadCommandsParam);
|
|
return responseResult;
|
}
|
|
|
private void checkTakeoffCondition(String dockSn) {
|
Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(dockSn);
|
if (dockOpt.isEmpty() || DockModeCodeEnum.IDLE != deviceService.getDockMode(dockSn)) {
|
throw new RuntimeException("当前状态不支持起飞");
|
}
|
|
ResponseResult result = seizeAuthority(dockSn, DroneAuthorityEnum.FLIGHT, null);
|
if (ResponseResult.CODE_SUCCESS != result.getCode()) {
|
throw new IllegalArgumentException(result.getMessage());
|
}
|
|
}
|
|
@Override
|
public ResponseResult takeoffToPoint(String sn, TakeoffToPointParam param) {
|
checkTakeoffCondition(sn);
|
if (!StringUtils.hasText(param.getFlightId())){
|
param.setFlightId(UUID.randomUUID().toString());
|
}
|
ServiceReply reply = messageSenderService.publishServicesTopic(sn, DroneControlMethodEnum.TAKE_OFF_TO_POINT.getMethod(), param, param.getFlightId());
|
return ResponseResult.CODE_SUCCESS != reply.getResult() ?
|
ResponseResult.error("无人机起飞失败 " + reply.getResult())
|
: ResponseResult.success();
|
}
|
|
@ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_TAKE_OFF_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
|
public CommonTopicReceiver handleTakeoffToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
|
String dockSn = receiver.getGateway();
|
|
Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
|
if (deviceOpt.isEmpty()) {
|
log.error("机场离线");
|
return null;
|
}
|
TakeoffProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<TakeoffProgressReceiver>() {
|
});
|
|
webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
|
BizCodeEnum.TAKE_OFF_TO_POINT_PROGRESS.getCode(),
|
ResultNotifyDTO.builder().sn(dockSn)
|
.message(WaylineErrorCodeEnum.SUCCESS == eventsReceiver.getResult() ?
|
eventsReceiver.getStatus().getMessage() : eventsReceiver.getResult().getErrorMsg())
|
.result(eventsReceiver.getResult().getErrorCode())
|
.build());
|
|
return receiver;
|
}
|
|
@Override
|
public ResponseResult seizeAuthority(String sn, DroneAuthorityEnum authority, DronePayloadParam param) {
|
String method;
|
switch (authority) {
|
case FLIGHT:
|
if (deviceService.checkAuthorityFlight(sn)) {
|
return ResponseResult.success();
|
}
|
method = DroneControlMethodEnum.FLIGHT_AUTHORITY_GRAB.getMethod();
|
break;
|
case PAYLOAD:
|
if (checkPayloadAuthority(sn, param.getPayloadIndex())) {
|
return ResponseResult.success();
|
}
|
method = DroneControlMethodEnum.PAYLOAD_AUTHORITY_GRAB.getMethod();
|
break;
|
default:
|
return ResponseResult.error(CommonErrorEnum.ILLEGAL_ARGUMENT);
|
}
|
ServiceReply serviceReply = messageSenderService.publishServicesTopic(sn, method, param);
|
return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
|
ResponseResult.error(serviceReply.getResult(), "方法: " + method + " 错误码：" + serviceReply.getResult())
|
: ResponseResult.success();
|
}
|
|
private Boolean checkPayloadAuthority(String sn, String payloadIndex) {
|
Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(sn);
|
if (dockOpt.isEmpty()) {
|
throw new RuntimeException("机场离线请重启机场");
|
}
|
return devicePayloadService.checkAuthorityPayload(dockOpt.get().getChildDeviceSn(), payloadIndex);
|
}
|
|
|
@Override
|
public ResponseResult payloadCommands(PayloadCommandsParam param) throws Exception {
|
param.getCmd().getClazz()
|
.getDeclaredConstructor(DronePayloadParam.class)
|
.newInstance(param.getData())
|
.checkCondition(param.getSn());
|
|
ServiceReply serviceReply = messageSenderService.publishServicesTopic(param.getSn(), param.getCmd().getCmd(), param.getData());
|
return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
|
ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
|
: ResponseResult.success();
|
}
|
|
@Override
|
public ResponseResult payloadCommands(PayloadCommandsParam param, String bid) throws Exception {
|
param.getCmd().getClazz()
|
.getDeclaredConstructor(DronePayloadParam.class)
|
.newInstance(param.getData())
|
.checkCondition(param.getSn());
|
|
ResponseResult result = ResponseResult.success();
|
result.setTraceid(bid);
|
|
ServiceReply serviceReply = messageSenderService.publishServicesTopic(param.getSn(), param.getCmd().getCmd(), param.getData(),bid);
|
return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
|
ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
|
: result;
|
}
|
|
@Override
|
public ResponseResult requestsConfig(String sn, String method, RequestsParam param) {
|
ServiceReply serviceReply = messageSenderService.publishRequestsTopic(sn, method, param);
|
return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
|
ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
|
: ResponseResult.success();
|
}
|
|
}
|
