From 767703920ada4b092061a695e9e6d321db732e23 Mon Sep 17 00:00:00 2001 From: liejiu946 Date: Fri, 6 Feb 2026 18:07:34 +0800 Subject: [PATCH] =?UTF-8?q?opt:1.=E4=BC=98=E5=8C=96=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E3=80=822.=E5=A2=9E=E5=8A=A0=E6=A0=87=E5=AE=9A=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../controller/AnomalyInfoController.java | 2 + .../controller/CalibrationController.java | 58 ++++ .../calibration/enums/CalibrationResult.java | 29 ++ .../calibration/enums/LaserAndCameraType.java | 101 +++++++ .../param/CalibrationLaserParam.java | 21 ++ .../service/CalibrationService.java | 53 ++++ .../service/impl/CalibrationServiceImpl.java | 276 ++++++++++++++++++ .../ext/service/impl/RcsToAptServiceImpl.java | 10 +- .../nl/apt15e/apt/task/service/dao/Task.java | 5 + .../service/impl/TaskManageServiceImpl.java | 1 + .../apt/teaching/rest/TeachingController.java | 6 + .../apt/teaching/service/TeachingService.java | 5 + .../service/impl/TeachingServiceImpl.java | 45 ++- .../apt15e/apt/vehicle/dao/VehicleInfo.java | 5 + .../service/impl/VehicleInfoServiceImpl.java | 2 + .../thread/ProtobufWebSocketHandler.java | 49 +++- src/main/resources/config/application.yml | 1 + 17 files changed, 632 insertions(+), 37 deletions(-) create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/controller/CalibrationController.java create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/enums/CalibrationResult.java create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/enums/LaserAndCameraType.java create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/param/CalibrationLaserParam.java create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/service/CalibrationService.java create mode 100644 src/main/java/org/nl/apt15e/apt/calibration/service/impl/CalibrationServiceImpl.java diff --git a/src/main/java/org/nl/apt15e/apt/anomalyInfo/controller/AnomalyInfoController.java b/src/main/java/org/nl/apt15e/apt/anomalyInfo/controller/AnomalyInfoController.java index 2d27091..a64214d 100644 --- a/src/main/java/org/nl/apt15e/apt/anomalyInfo/controller/AnomalyInfoController.java +++ b/src/main/java/org/nl/apt15e/apt/anomalyInfo/controller/AnomalyInfoController.java @@ -1,5 +1,6 @@ package org.nl.apt15e.apt.anomalyInfo.controller; +import cn.dev33.satoken.annotation.SaIgnore; import com.alibaba.excel.EasyExcel; import lombok.Value; import org.nl.apt15e.apt.anomalyInfo.dao.ErrorHandling; @@ -50,6 +51,7 @@ public class AnomalyInfoController { @Resource private ProcessZip processZip; + @SaIgnore @PostMapping("/queryErrorDataByCode") public ResponseEntity queryErrorDataByCode(@RequestParam("code") String code) { return new ResponseEntity<>(anomalyInfoService.queryErrorDataByCode(code), HttpStatus.OK); diff --git a/src/main/java/org/nl/apt15e/apt/calibration/controller/CalibrationController.java b/src/main/java/org/nl/apt15e/apt/calibration/controller/CalibrationController.java new file mode 100644 index 0000000..ac1a13d --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/controller/CalibrationController.java @@ -0,0 +1,58 @@ +package org.nl.apt15e.apt.calibration.controller; + +import org.nl.apt15e.apt.calibration.param.CalibrationLaserParam; +import org.nl.apt15e.apt.calibration.service.CalibrationService; +import org.nl.apt15e.common.logging.annotation.Log; +import org.springframework.http.HttpStatus; +import org.springframework.http.ResponseEntity; +import org.springframework.web.bind.annotation.*; + +import javax.annotation.Resource; + +/** + * @author dsh + * 2026/1/6 + */ +@RestController +@RequestMapping("/calibration") +public class CalibrationController { + + @Resource + private CalibrationService calibrationService; + + @GetMapping("/getCalibrationConfigInfo") + @Log("获取车辆相机和激光配置信息") + public ResponseEntity getCalibrationConfigInfo() { + return new ResponseEntity<>(calibrationService.getCalibrationConfigInfo(), HttpStatus.OK); + } + + @PostMapping("/calibrationLaser") + @Log("一键标定激光") + public ResponseEntity calibrationLaser(@RequestBody CalibrationLaserParam param){ + return new ResponseEntity<>(calibrationService.calibrationLaser(param), HttpStatus.OK); + } + + @PostMapping("/startCalibrationCamera") + @Log("开始标定顶部相机") + public ResponseEntity startCalibrationCamera(@RequestParam String location){ + return new ResponseEntity<>(calibrationService.startCalibrationCamera(location), HttpStatus.OK); + } + + @PostMapping("/endCalibrationCamera") + @Log("结束标定顶部相机") + public ResponseEntity endCalibrationCamera(@RequestParam String location,@RequestParam String params){ + return new ResponseEntity<>(calibrationService.endCalibrationCamera(location,params), HttpStatus.OK); + } + + @PostMapping("/calibrationDepthcamera") + @Log("一键标定相机") + public ResponseEntity calibrationDepthcamera(@RequestParam String location){ + return new ResponseEntity<>(calibrationService.calibrationDepthcamera(location), HttpStatus.OK); + } + + @GetMapping("/getCalibrationByTaskId") + @Log("根据task_id查询对应的标定结果") + public ResponseEntity getCalibrationByTaskId(@RequestParam String task_id){ + return new ResponseEntity<>(calibrationService.getCalibrationByTaskId(task_id), HttpStatus.OK); + } +} diff --git a/src/main/java/org/nl/apt15e/apt/calibration/enums/CalibrationResult.java b/src/main/java/org/nl/apt15e/apt/calibration/enums/CalibrationResult.java new file mode 100644 index 0000000..dce1023 --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/enums/CalibrationResult.java @@ -0,0 +1,29 @@ +package org.nl.apt15e.apt.calibration.enums; + +import lombok.Getter; + +/** + * @author dsh + * 2026/1/7 + */ +@Getter +public enum CalibrationResult { + + COLLECTING("1","采集数据中"), + FAILED("2","标定失败"), + RESULT_ABNORMAL("3","标定结果异常"), + PROCEDURE_ABNORMAL("999","标定程序异常"), + SUCCESSFUL("0","标定成功"); + + + + + private String code; + + private String name; + + CalibrationResult(String code, String name) { + this.code = code; + this.name = name; + } +} diff --git a/src/main/java/org/nl/apt15e/apt/calibration/enums/LaserAndCameraType.java b/src/main/java/org/nl/apt15e/apt/calibration/enums/LaserAndCameraType.java new file mode 100644 index 0000000..cc266cf --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/enums/LaserAndCameraType.java @@ -0,0 +1,101 @@ +package org.nl.apt15e.apt.calibration.enums; + +import lombok.Getter; + +/** + * @author dsh + * 2026/1/6 + */ +@Getter +public enum LaserAndCameraType { + + /** + * 前 + */ + FRONT("1", "前", "前","Front"), + /** + * 后 + */ + REAR("2", "后", "后","Rear"), + /** + * 左 + */ + LEFT("3", "左", "左","Left"), + /** + * 左前 + */ + FRONT_LEFT("4", "左前", "左前","Front left"), + /** + * 左后 + */ + LEFT_BACK("5", "左后", "左后","Left rear"), + /** + * 右 + */ + RIGHT("6", "右", "右","Right"), + /** + * 右前 + */ + RIGHT_FRONT("7", "右前", "右前","Right front"), + /** + * 右后 + */ + RIGHT_BACK("8", "右后", "右后","Right rear"), + + /** + * 顶部前 + */ + TOP_FRONT("9", "顶部前", "顶部前","Top front"), + /** + * 顶部后 + */ + TOP_REAR("10", "顶部后", "顶部后","Top rear"), + /** + * 顶部左 + */ + TOP_LEFT("11", "顶部左", "顶部左","Top left"), + /** + * 顶部左前 + */ + TOP_FRONT_LEFT("12", "顶部左前", "顶部左前","Top front left"), + /** + * 顶部左后 + */ + TOP_LEFT_BACK("13", "顶部左后", "顶部左后","Top left rear"), + /** + * 顶部右 + */ + TOP_RIGHT("14", "顶部右", "顶部右","Top right"), + /** + * 顶部右前 + */ + TOP_RIGHT_FRONT("15", "顶部右前", "顶部右前","Top right front"), + /** + * 顶部右后 + */ + TOP_RIGHT_BACK("16", "顶部右后", "顶部右后","Top right rear"); + + private String location; + + private String name; + + private String zh_name; + + private String en_name; + + LaserAndCameraType(String location, String name, String zh_name, String en_name) { + this.location = location; + this.name = name; + this.zh_name = zh_name; + this.en_name = en_name; + } + + public static LaserAndCameraType getByLocation(String code) { + for (LaserAndCameraType e : LaserAndCameraType.values()) { + if (e.location.equals(code)) { + return e; + } + } + return null; + } +} diff --git a/src/main/java/org/nl/apt15e/apt/calibration/param/CalibrationLaserParam.java b/src/main/java/org/nl/apt15e/apt/calibration/param/CalibrationLaserParam.java new file mode 100644 index 0000000..0a96b6e --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/param/CalibrationLaserParam.java @@ -0,0 +1,21 @@ +package org.nl.apt15e.apt.calibration.param; + +import lombok.Data; + +/** + * @author dsh + * 2026/1/7 + */ +@Data +public class CalibrationLaserParam { + + /** + * 设备ID + */ + private String location; + + /** + * 指令 + */ + private int cmd; +} diff --git a/src/main/java/org/nl/apt15e/apt/calibration/service/CalibrationService.java b/src/main/java/org/nl/apt15e/apt/calibration/service/CalibrationService.java new file mode 100644 index 0000000..1f5e752 --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/service/CalibrationService.java @@ -0,0 +1,53 @@ +package org.nl.apt15e.apt.calibration.service; + + +import org.nl.apt15e.apt.calibration.param.CalibrationLaserParam; +import org.nl.apt15e.apt.dto.WebResponse; + +/** + * @author dsh + * 2026/1/6 + */ +public interface CalibrationService { + + /** + * 获取车辆相机和激光配置信息 + * @return + */ + WebResponse getCalibrationConfigInfo(); + + /** + * 标定激光 + * @param param + * @return + */ + WebResponse calibrationLaser(CalibrationLaserParam param); + + /** + * 开始标定顶部相机 + * @param location + * @return + */ + WebResponse startCalibrationCamera(String location); + + /** + * 结束标定顶部相机 + * @param location + * @return + */ + WebResponse endCalibrationCamera(String location,String params); + + /** + * 标定深度相机 + * @param location + * @return + */ + WebResponse calibrationDepthcamera(String location); + + /** + * 根据task_id查询对应的标定结果 + * @param task_id + * @return + */ + WebResponse getCalibrationByTaskId(String task_id); +} diff --git a/src/main/java/org/nl/apt15e/apt/calibration/service/impl/CalibrationServiceImpl.java b/src/main/java/org/nl/apt15e/apt/calibration/service/impl/CalibrationServiceImpl.java new file mode 100644 index 0000000..394b233 --- /dev/null +++ b/src/main/java/org/nl/apt15e/apt/calibration/service/impl/CalibrationServiceImpl.java @@ -0,0 +1,276 @@ +package org.nl.apt15e.apt.calibration.service.impl; + +import cn.hutool.core.date.DateUtil; +import cn.hutool.core.util.ObjectUtil; +import cn.hutool.core.util.StrUtil; +import cn.hutool.http.HttpResponse; +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.JSONArray; +import com.alibaba.fastjson.JSONObject; +import lombok.extern.slf4j.Slf4j; +import org.nl.apt15e.apt.calibration.enums.CalibrationResult; +import org.nl.apt15e.apt.calibration.enums.LaserAndCameraType; +import org.nl.apt15e.apt.calibration.param.CalibrationLaserParam; +import org.nl.apt15e.apt.calibration.service.CalibrationService; +import org.nl.apt15e.apt.dto.WebResponse; +import org.nl.apt15e.common.BadRequestException; +import org.nl.apt15e.config.language.LangProcess; +import org.nl.apt15e.util.HTTPUtil; +import org.nl.apt15e.util.URLConstant; +import org.springframework.stereotype.Service; + +import java.util.Date; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + +/** + * @author dsh + * 2026/1/6 + */ +@Slf4j +@Service +public class CalibrationServiceImpl implements CalibrationService { + + public static Map currentCalibration = new ConcurrentHashMap<>(); + + @Override + public WebResponse getCalibrationConfigInfo() { + HttpResponse response = null; + try { + response = HTTPUtil.post(URLConstant.VEHICLE_IP_PORT,"/tool/rob/getInstallConfig", new JSONObject()); + } catch (Exception e) { + log.info("访问车辆相机和激光配置接口报错:{}",e.getMessage()); + throw new BadRequestException(LangProcess.msg("failed")); + } + // 检查响应状态码 + if (response.isOk() && response.body() != null) { + // 获取响应体内容 + JSONObject body = JSON.parseObject(response.body()); + log.info("获取车辆相机和激光配置:{}",body); + return WebResponse.requestParamOk(this.analyzeLaserAndCamera(body)); + } + log.info("获取车辆相机和激光配置失败"); + throw new BadRequestException(LangProcess.msg("failed")); + } + + @Override + public WebResponse calibrationLaser(CalibrationLaserParam param) { + if (ObjectUtil.isEmpty(param)){ + throw new BadRequestException(LangProcess.msg("param_is_null")); + } + JSONObject params = new JSONObject(); + String task_id = DateUtil.format(new Date(), "yyyyMMddHHmmssSSS"); + String attach = "{"+ + "'init_pose':{'pose':{'x':0,'y':0,'angle':0}},'params':'','sub_command':"+param.getLocation()+",'target_pose':{'pose':{'x':0,'y':0,'angle':0}},"+ + "'task_id':'"+task_id+"',"+ + "'need_response':true"+ + "}"; + params.put("cmd", param.getCmd()); + params.put("attach", JSON.parseObject(attach)); + HttpResponse response = null; + try { + response = HTTPUtil.post(URLConstant.VEHICLE_IP_PORT,"/tool/rob/sendCMD", params); + } catch (Exception e) { + log.info("访问车体激光标定接口报错:{}",e.getMessage()); + throw new BadRequestException(LangProcess.msg("failed")); + } + // 检查响应状态码 + if (response.isOk() && response.body() != null) { + // 获取响应体内容 + JSONObject body = JSON.parseObject(response.body()); + log.info("激光标定:{}",body); + if ("200".equals(body.getString("code"))){ + currentCalibration.put(task_id, CalibrationResult.COLLECTING.getCode()); + JSONObject result = new JSONObject(); + result.put("task_id", task_id); + return WebResponse.requestParamOk(result); + } + } + log.info("激光标定失败"); + throw new BadRequestException(LangProcess.msg("failed")); + } + + @Override + public WebResponse endCalibrationCamera(String location,String param) { + if (StrUtil.isBlank(location)){ + throw new BadRequestException(LangProcess.msg("param_is_null")); + } + JSONObject params = new JSONObject(); + String task_id = DateUtil.format(new Date(), "yyyyMMddHHmmssSSS"); + String attach = "{"+ + "'init_pose':{'pose':{'x':0,'y':0,'angle':0}},'params':'"+param+"','sub_command':"+location+",'target_pose':{'pose':{'x':0,'y':0,'angle':0}},"+ + "'task_id':"+task_id+","+ + "'need_response':true"+ + "}"; + params.put("cmd", 1822); + params.put("attach", JSON.parseObject(attach)); + HttpResponse response = null; + try { + response = HTTPUtil.post(URLConstant.VEHICLE_IP_PORT,"/tool/rob/sendCMD", params); + } catch (Exception e) { + log.info("访问车体顶部相机结束标定接口报错:{}",e.getMessage()); + throw new BadRequestException(LangProcess.msg("failed")); + } + // 检查响应状态码 + if (response.isOk() && response.body() != null) { + // 获取响应体内容 + JSONObject body = JSON.parseObject(response.body()); + log.info("结束顶部相机标定:{}",body); + if ("200".equals(body.getString("code"))){ + currentCalibration.put(task_id, CalibrationResult.COLLECTING.getCode()); + JSONObject result = new JSONObject(); + result.put("task_id", task_id); + return WebResponse.requestParamOk(result); + } + } + log.info("结束顶部相机标定失败"); + throw new BadRequestException(LangProcess.msg("failed")); + } + + @Override + public WebResponse startCalibrationCamera(String location) { + if (StrUtil.isBlank(location)){ + throw new BadRequestException(LangProcess.msg("param_is_null")); + } + JSONObject params = new JSONObject(); + String task_id = DateUtil.format(new Date(), "yyyyMMddHHmmssSSS"); + String attach = "{"+ + "'init_pose':{'pose':{'x':0,'y':0,'angle':0}},'params':'','sub_command':"+location+",'target_pose':{'pose':{'x':0,'y':0,'angle':0}},"+ + "'task_id':"+task_id+","+ + "'need_response':true"+ + "}"; + params.put("cmd", 1821); + params.put("attach", JSON.parseObject(attach)); + HttpResponse response = null; + try { + response = HTTPUtil.post(URLConstant.VEHICLE_IP_PORT,"/tool/rob/sendCMD", params); + } catch (Exception e) { + log.info("访问车体顶部相机开始标定接口报错:{}",e.getMessage()); + throw new BadRequestException(LangProcess.msg("failed")); + } + // 检查响应状态码 + if (response.isOk() && response.body() != null) { + // 获取响应体内容 + JSONObject body = JSON.parseObject(response.body()); + log.info("开始顶部相机标定:{}",body); + if ("200".equals(body.getString("code"))){ + return WebResponse.requestOk(); + } + } + log.info("开始顶部相机标定失败"); + throw new BadRequestException(LangProcess.msg("failed")); + } + + @Override + public WebResponse calibrationDepthcamera(String location) { + if (StrUtil.isBlank(location)){ + throw new BadRequestException(LangProcess.msg("param_is_null")); + } + JSONObject params = new JSONObject(); + String task_id = DateUtil.format(new Date(), "yyyyMMddHHmmssSSS"); + String attach = "{"+ + "'init_pose':{'pose':{'x':0,'y':0,'angle':0}},'params':'','sub_command':"+location+",'target_pose':{'pose':{'x':0,'y':0,'angle':0}},"+ + "'task_id':"+task_id+","+ + "'need_response':true"+ + "}"; + params.put("cmd", 1815); + params.put("attach", JSON.parseObject(attach)); + HttpResponse response = null; + try { + response = HTTPUtil.post(URLConstant.VEHICLE_IP_PORT,"/tool/rob/sendCMD", params); + } catch (Exception e) { + log.info("访问车体深度相机标定接口报错:{}",e.getMessage()); + throw new BadRequestException(LangProcess.msg("failed")); + } + // 检查响应状态码 + if (response.isOk() && response.body() != null) { + // 获取响应体内容 + JSONObject body = JSON.parseObject(response.body()); + log.info("深度相机标定:{}",body); + if ("200".equals(body.getString("code"))){ + currentCalibration.put(task_id, CalibrationResult.COLLECTING.getCode()); + JSONObject result = new JSONObject(); + result.put("task_id", task_id); + return WebResponse.requestParamOk(result); + } + } + log.info("深度相机标定失败"); + throw new BadRequestException(LangProcess.msg("failed")); + } + + @Override + public WebResponse getCalibrationByTaskId(String task_id) { + if (StrUtil.isBlank(task_id)){ + throw new BadRequestException(LangProcess.msg("param_is_null")); + } + return WebResponse.requestParamOk(currentCalibration.get(task_id)); + } + + public JSONObject analyzeLaserAndCamera(JSONObject body) { + //激光 + JSONObject Laser = body.getJSONObject("Laser"); + //相机 + JSONObject Camera = body.getJSONObject("camera"); + //深度相机 + JSONObject Depthcamera = body.getJSONObject("depthcamera"); + + JSONObject result = new JSONObject(); + JSONArray LaserArray = new JSONArray(); + JSONArray CameraArray = new JSONArray(); + JSONArray DepthcameraArray = new JSONArray(); + if (Laser != null){ + for (String key : Laser.keySet()) { + JSONObject laser = Laser.getJSONObject(key); + String location = laser.getString("location"); + JSONObject value = new JSONObject(); + value.put("location", location); + LaserAndCameraType settingCodeEnum = LaserAndCameraType.getByLocation(location); + if (settingCodeEnum != null) { + value.put("name", settingCodeEnum.getName()); + value.put("zh_name", settingCodeEnum.getZh_name()); + value.put("en_name", settingCodeEnum.getEn_name()); + } + LaserArray.add(value); + } + } + result.put("Laser", LaserArray); + if (Camera != null){ + for (String key : Camera.keySet()) { + JSONObject camera = Camera.getJSONObject(key); + String location = camera.getString("location"); + JSONObject value = new JSONObject(); + value.put("location", location); + LaserAndCameraType settingCodeEnum = LaserAndCameraType.getByLocation(location); + if (settingCodeEnum != null) { + value.put("name", settingCodeEnum.getName()); + value.put("zh_name", settingCodeEnum.getZh_name()); + value.put("en_name", settingCodeEnum.getEn_name()); + } + CameraArray.add(value); + } + } + + result.put("camera", CameraArray); + if (Depthcamera != null){ + for (String key : Depthcamera.keySet()) { + JSONObject depthcamera = Depthcamera.getJSONObject(key); + String location = depthcamera.getString("location"); + JSONObject value = new JSONObject(); + value.put("location", location); + // 深度相机的location是负数，需要做转换。 + int new_location = Integer.parseInt(location); + LaserAndCameraType settingCodeEnum = LaserAndCameraType.getByLocation(String.valueOf(Math.abs(new_location))); + if (settingCodeEnum != null) { + value.put("name", settingCodeEnum.getName()); + value.put("zh_name", settingCodeEnum.getZh_name()); + value.put("en_name", settingCodeEnum.getEn_name()); + } + DepthcameraArray.add(value); + } + } + + result.put("depthcamera", DepthcameraArray); + return result; + } + +} diff --git a/src/main/java/org/nl/apt15e/apt/ext/service/impl/RcsToAptServiceImpl.java b/src/main/java/org/nl/apt15e/apt/ext/service/impl/RcsToAptServiceImpl.java index 1e0d25f..ff0bf76 100644 --- a/src/main/java/org/nl/apt15e/apt/ext/service/impl/RcsToAptServiceImpl.java +++ b/src/main/java/org/nl/apt15e/apt/ext/service/impl/RcsToAptServiceImpl.java @@ -64,11 +64,6 @@ public class RcsToAptServiceImpl implements RcsToAptService { // 更新任务状态 String taskChainPoStatus = rcsToAptTaskChainPoDto.getStatus().toString(); -// if (taskPoStatus.equals(RcsTaskStatus.SON_EXECUTING.getCode()) || taskPoStatus.equals(RcsTaskStatus.SON_FINISHED.getCode())) { -// task.setTask_status(TaskStatus.EXECUTING.getCode()); -// } else if (taskChainPoStatus.equals(RcsTaskStatus.TASK_FINISHED.getCode())) { -// task.setTask_status(TaskStatus.FINISHED.getCode()); -// } if (RcsTaskChainStatus.FINISHED.getCode().equals(taskChainPoStatus)) { task.setTask_status(TaskStatus.FINISHED.getCode()); } else if (RcsTaskChainStatus.EXECUTING.getCode().equals(taskChainPoStatus)) { @@ -82,6 +77,11 @@ public class RcsToAptServiceImpl implements RcsToAptService { if (ObjectUtil.isNotEmpty(endPointCode)) { Station staDao = stationService.getOne(new LambdaQueryWrapper<>(Station.class) .eq(Station::getStation_code, endPointCode)); + int oldSeqIndex = task.getTask_seq_index(); + // 判断是否重复站点,不重复时才增加index + if (!staDao.getStation_name().equals(task.getTask_point())){ + task.setTask_seq_index(oldSeqIndex+1); + } task.setTask_point(staDao.getStation_name()); } } diff --git a/src/main/java/org/nl/apt15e/apt/task/service/dao/Task.java b/src/main/java/org/nl/apt15e/apt/task/service/dao/Task.java index bb97dd7..6506b7a 100644 --- a/src/main/java/org/nl/apt15e/apt/task/service/dao/Task.java +++ b/src/main/java/org/nl/apt15e/apt/task/service/dao/Task.java @@ -39,6 +39,11 @@ public class Task implements Serializable { */ private String task_seq; + /** + * 任务顺序当前索引(从1开始) + */ + private int task_seq_index; + /** * 任务状态 */ diff --git a/src/main/java/org/nl/apt15e/apt/taskOperate/service/impl/TaskManageServiceImpl.java b/src/main/java/org/nl/apt15e/apt/taskOperate/service/impl/TaskManageServiceImpl.java index 270e821..1abd502 100644 --- a/src/main/java/org/nl/apt15e/apt/taskOperate/service/impl/TaskManageServiceImpl.java +++ b/src/main/java/org/nl/apt15e/apt/taskOperate/service/impl/TaskManageServiceImpl.java @@ -134,6 +134,7 @@ public class TaskManageServiceImpl implements TaskManageService { .map(Station::getStation_name) .collect(Collectors.joining(",")) ); + task.setTask_seq_index(0); task.setTask_status(TaskStatus.CREATE.getCode()); task.setCreate_time(DateUtil.now()); iTaskService.save(task); diff --git a/src/main/java/org/nl/apt15e/apt/teaching/rest/TeachingController.java b/src/main/java/org/nl/apt15e/apt/teaching/rest/TeachingController.java index da62f4e..7d4652e 100644 --- a/src/main/java/org/nl/apt15e/apt/teaching/rest/TeachingController.java +++ b/src/main/java/org/nl/apt15e/apt/teaching/rest/TeachingController.java @@ -73,6 +73,12 @@ public class TeachingController { return new ResponseEntity<>(teachingService.restart(), HttpStatus.OK); } + @PostMapping("/startingPointRelocate") +// @Log("起点位置重定位") + private ResponseEntity startingPointRelocate() { + return new ResponseEntity<>(teachingService.startingPointRelocate(), HttpStatus.OK); + } + @PostMapping("/relocate") private ResponseEntity relocate(@RequestParam("x") Double x, @RequestParam("y") Double y, @RequestParam("angle") Double angle) { return new ResponseEntity<>(teachingService.relocate(x, y, angle), HttpStatus.OK); diff --git a/src/main/java/org/nl/apt15e/apt/teaching/service/TeachingService.java b/src/main/java/org/nl/apt15e/apt/teaching/service/TeachingService.java index 8b79cac..38e0074 100644 --- a/src/main/java/org/nl/apt15e/apt/teaching/service/TeachingService.java +++ b/src/main/java/org/nl/apt15e/apt/teaching/service/TeachingService.java @@ -81,6 +81,11 @@ public interface TeachingService { */ Map restart(); + /** + * 起点位置重定位 + */ + Map startingPointRelocate(); + /** * 一键部署地图 * @return diff --git a/src/main/java/org/nl/apt15e/apt/teaching/service/impl/TeachingServiceImpl.java b/src/main/java/org/nl/apt15e/apt/teaching/service/impl/TeachingServiceImpl.java index 0b992c7..3299084 100644 --- a/src/main/java/org/nl/apt15e/apt/teaching/service/impl/TeachingServiceImpl.java +++ b/src/main/java/org/nl/apt15e/apt/teaching/service/impl/TeachingServiceImpl.java @@ -386,6 +386,18 @@ public class TeachingServiceImpl implements TeachingService { throw new BadRequestException(LangProcess.msg("error_restart")); } + @Override + public Map startingPointRelocate() { + JSONObject response = new JSONObject(); + Station station = stationService.getOne(new LambdaQueryWrapper<>(Station.class) + .eq(Station::getStation_code,"A") + ); + this.relocate(station.getX(),station.getY(),station.getAngle()); + response.put("code", 200); + response.put("message", LangProcess.msg("successful")); + return response; + } + @Override public Map oneClickDeployment(String mapName) { if (StrUtil.isBlank(mapName)){ @@ -402,23 +414,24 @@ public class TeachingServiceImpl implements TeachingService { this.changeCurrentRunMap(mapName); // 解析地图包并同步到调度 this.synchronizeMap(mapName,zipFile); - // 重启本体程序 - this.restart(); - // 重定位，需要等待一会 - while (!"1".equals(VehicleInfoServiceImpl.vehicleInfo.getReady())){ - log.info("建图 重启本体程序中,还未接收到本体重启信号"); - try { - Thread.sleep(1000); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } - log.info("接收到本体重启信号，发送重定位指令"); - Station station = stationService.getOne(new LambdaQueryWrapper<>(Station.class) - .eq(Station::getStation_code,"A") - ); - this.relocate(station.getX(),station.getY(),station.getAngle()); + // 重启本体程序 +// this.restart(); + +// 重定位，需要等待一会 +// while (!"1".equals(VehicleInfoServiceImpl.vehicleInfo.getReady())){ +// log.info("建图 重启本体程序中,还未接收到本体重启信号"); +// try { +// Thread.sleep(1000); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } +// } +// log.info("接收到本体重启信号，发送重定位指令"); +// Station station = stationService.getOne(new LambdaQueryWrapper<>(Station.class) +// .eq(Station::getStation_code,"A") +// ); +// this.relocate(station.getX(),station.getY(),station.getAngle()); response.put("code", 200); response.put("message", LangProcess.msg("successful")); return response; diff --git a/src/main/java/org/nl/apt15e/apt/vehicle/dao/VehicleInfo.java b/src/main/java/org/nl/apt15e/apt/vehicle/dao/VehicleInfo.java index 5e573f3..c892154 100644 --- a/src/main/java/org/nl/apt15e/apt/vehicle/dao/VehicleInfo.java +++ b/src/main/java/org/nl/apt15e/apt/vehicle/dao/VehicleInfo.java @@ -89,6 +89,11 @@ public class VehicleInfo implements Serializable { */ private String task_seq; + /** + * 车辆任务链中当前索引 + */ + private int task_seq_index; + /** * 车辆任务链中当前点位 */ diff --git a/src/main/java/org/nl/apt15e/apt/vehicle/service/impl/VehicleInfoServiceImpl.java b/src/main/java/org/nl/apt15e/apt/vehicle/service/impl/VehicleInfoServiceImpl.java index 919b082..bb69c56 100644 --- a/src/main/java/org/nl/apt15e/apt/vehicle/service/impl/VehicleInfoServiceImpl.java +++ b/src/main/java/org/nl/apt15e/apt/vehicle/service/impl/VehicleInfoServiceImpl.java @@ -274,9 +274,11 @@ public class VehicleInfoServiceImpl implements VehicleInfoService { if (ObjectUtil.isNotEmpty(task)){ vehicleInfo.setTask_seq(task.getTask_seq()); vehicleInfo.setTask_point(task.getTask_point()); + vehicleInfo.setTask_seq_index(task.getTask_seq_index()); }else { vehicleInfo.setTask_seq(""); vehicleInfo.setTask_point(""); + vehicleInfo.setTask_seq_index(0); } webSocketSet.forEach(c -> { Map vehicleInfoMap = new HashMap<>(); diff --git a/src/main/java/org/nl/apt15e/config/thread/ProtobufWebSocketHandler.java b/src/main/java/org/nl/apt15e/config/thread/ProtobufWebSocketHandler.java index f726309..9a566c2 100644 --- a/src/main/java/org/nl/apt15e/config/thread/ProtobufWebSocketHandler.java +++ b/src/main/java/org/nl/apt15e/config/thread/ProtobufWebSocketHandler.java @@ -98,23 +98,23 @@ public class ProtobufWebSocketHandler extends BinaryWebSocketHandler { Robottype.LaserData laser = datagram.getLaserScan(); // 9顶部激光 if (laser.getLocation() == 9){ - Set globalData = new HashSet<>(); +// Set globalData = new HashSet<>(); Set currentData = new HashSet<>(); for (Robottype.Point point: laser.getScanList()){ - double cosYaw = cos(VehicleInfoServiceImpl.vehicleInfo.getTheta()); - double sinYaw = sin(VehicleInfoServiceImpl.vehicleInfo.getTheta()); - double x_global = VehicleInfoServiceImpl.vehicleInfo.getX() + cosYaw * point.getX() - sinYaw * point.getY(); - DecimalFormat x_globalDF = new DecimalFormat("#.#"); - String x_result = x_globalDF.format(x_global); - double x_globalNum = Double.parseDouble(x_result); - double y_global = VehicleInfoServiceImpl.vehicleInfo.getY() + sinYaw * point.getX() + cosYaw * point.getY(); - DecimalFormat y_globalDF = new DecimalFormat("#.#"); - String y_result = y_globalDF.format(y_global); - double y_globalNum = Double.parseDouble(y_result); - PointCloudDataDto globalPointCloudDataDto = new PointCloudDataDto(); - globalPointCloudDataDto.setX(x_globalNum); - globalPointCloudDataDto.setY(y_globalNum); - globalData.add(globalPointCloudDataDto); +// double cosYaw = cos(VehicleInfoServiceImpl.vehicleInfo.getTheta()); +// double sinYaw = sin(VehicleInfoServiceImpl.vehicleInfo.getTheta()); +// double x_global = VehicleInfoServiceImpl.vehicleInfo.getX() + cosYaw * point.getX() - sinYaw * point.getY(); +// DecimalFormat x_globalDF = new DecimalFormat("#.#"); +// String x_result = x_globalDF.format(x_global); +// double x_globalNum = Double.parseDouble(x_result); +// double y_global = VehicleInfoServiceImpl.vehicleInfo.getY() + sinYaw * point.getX() + cosYaw * point.getY(); +// DecimalFormat y_globalDF = new DecimalFormat("#.#"); +// String y_result = y_globalDF.format(y_global); +// double y_globalNum = Double.parseDouble(y_result); +// PointCloudDataDto globalPointCloudDataDto = new PointCloudDataDto(); +// globalPointCloudDataDto.setX(x_globalNum); +// globalPointCloudDataDto.setY(y_globalNum); +// globalData.add(globalPointCloudDataDto); double x_current = point.getX(); DecimalFormat x_currentDF = new DecimalFormat("#.#"); @@ -129,8 +129,25 @@ public class ProtobufWebSocketHandler extends BinaryWebSocketHandler { currentPointCloudDataDto.setY(y_currentNum); currentData.add(currentPointCloudDataDto); } - VehicleInfoServiceImpl.globalPointCloudData = globalData; +// VehicleInfoServiceImpl.globalPointCloudData = globalData; VehicleInfoServiceImpl.currentPointCloudData = currentData; + }else if (laser.getLocation() == 100){ + Set globalData = new HashSet<>(); + for (Robottype.Point point: laser.getScanList()){ + double x_global = point.getX(); + DecimalFormat x_globalDF = new DecimalFormat("#.#"); + String x_GlobalResult = x_globalDF.format(x_global); + double x_globalNum = Double.parseDouble(x_GlobalResult); + double y_global = point.getY(); + DecimalFormat y_globalDF = new DecimalFormat("#.#"); + String y_result = y_globalDF.format(y_global); + double y_globalNum = Double.parseDouble(y_result); + PointCloudDataDto currentPointCloudDataDto = new PointCloudDataDto(); + currentPointCloudDataDto.setX(x_globalNum); + currentPointCloudDataDto.setY(y_globalNum); + globalData.add(currentPointCloudDataDto); + VehicleInfoServiceImpl.globalPointCloudData = globalData; + } } // System.out.println("Received laser_scan: " + list); break; diff --git a/src/main/resources/config/application.yml b/src/main/resources/config/application.yml index 0fe0ea6..9519c19 100644 --- a/src/main/resources/config/application.yml +++ b/src/main/resources/config/application.yml @@ -50,6 +50,7 @@ security: - /webSocket/** - /file/** - /routeInfo/** + - /calibration/** - /station/** # 静态资源 - /*.html