root/APT15E_server
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

opt:1.优化功能。2.增加标定功能。

Browse Source
This commit is contained in:
liejiu946
2026-02-06 18:07:34 +08:00
parent a313981785
commit 767703920a
17 changed files with 632 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/org/nl/apt15e/apt/anomalyInfo/controller/AnomalyInfoController.java
View File

@@ -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<Object> queryErrorDataByCode(@RequestParam("code") String code) {
return new ResponseEntity<>(anomalyInfoService.queryErrorDataByCode(code), HttpStatus.OK);

58
src/main/java/org/nl/apt15e/apt/calibration/controller/CalibrationController.java Normal file
View File

@@ -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<Object> getCalibrationConfigInfo() {
return new ResponseEntity<>(calibrationService.getCalibrationConfigInfo(), HttpStatus.OK);
}
@PostMapping("/calibrationLaser")
@Log("一键标定激光")
public ResponseEntity<Object> calibrationLaser(@RequestBody CalibrationLaserParam param){
return new ResponseEntity<>(calibrationService.calibrationLaser(param), HttpStatus.OK);
}
@PostMapping("/startCalibrationCamera")
@Log("开始标定顶部相机")
public ResponseEntity<Object> startCalibrationCamera(@RequestParam String location){
return new ResponseEntity<>(calibrationService.startCalibrationCamera(location), HttpStatus.OK);
}
@PostMapping("/endCalibrationCamera")
@Log("结束标定顶部相机")
public ResponseEntity<Object> endCalibrationCamera(@RequestParam String location,@RequestParam String params){
return new ResponseEntity<>(calibrationService.endCalibrationCamera(location,params), HttpStatus.OK);
}
@PostMapping("/calibrationDepthcamera")
@Log("一键标定相机")
public ResponseEntity<Object> calibrationDepthcamera(@RequestParam String location){
return new ResponseEntity<>(calibrationService.calibrationDepthcamera(location), HttpStatus.OK);
}
@GetMapping("/getCalibrationByTaskId")
@Log("根据task_id查询对应的标定结果")
public ResponseEntity<Object> getCalibrationByTaskId(@RequestParam String task_id){
return new ResponseEntity<>(calibrationService.getCalibrationByTaskId(task_id), HttpStatus.OK);
}
}

29
src/main/java/org/nl/apt15e/apt/calibration/enums/CalibrationResult.java Normal file
View File

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

101
src/main/java/org/nl/apt15e/apt/calibration/enums/LaserAndCameraType.java Normal file
View File

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

21
src/main/java/org/nl/apt15e/apt/calibration/param/CalibrationLaserParam.java Normal file
View File

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

53
src/main/java/org/nl/apt15e/apt/calibration/service/CalibrationService.java Normal file
View File

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

276
src/main/java/org/nl/apt15e/apt/calibration/service/impl/CalibrationServiceImpl.java Normal file
View File

@@ -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<String,String> 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;
}
}

10
src/main/java/org/nl/apt15e/apt/ext/service/impl/RcsToAptServiceImpl.java
View File

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

5
src/main/java/org/nl/apt15e/apt/task/service/dao/Task.java
View File

@@ -39,6 +39,11 @@ public class Task implements Serializable {
*/
private String task_seq;
/**
* 任务顺序当前索引(从1开始)
*/
private int task_seq_index;
/**
* 任务状态
*/

1
src/main/java/org/nl/apt15e/apt/taskOperate/service/impl/TaskManageServiceImpl.java
View File

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

6
src/main/java/org/nl/apt15e/apt/teaching/rest/TeachingController.java
View File

@@ -73,6 +73,12 @@ public class TeachingController {
return new ResponseEntity<>(teachingService.restart(), HttpStatus.OK);
}
@PostMapping("/startingPointRelocate")
// @Log("起点位置重定位")
private ResponseEntity<Object> startingPointRelocate() {
return new ResponseEntity<>(teachingService.startingPointRelocate(), HttpStatus.OK);
}
@PostMapping("/relocate")
private ResponseEntity<Object> relocate(@RequestParam("x") Double x, @RequestParam("y") Double y, @RequestParam("angle") Double angle) {
return new ResponseEntity<>(teachingService.relocate(x, y, angle), HttpStatus.OK);

5
src/main/java/org/nl/apt15e/apt/teaching/service/TeachingService.java
View File

@@ -81,6 +81,11 @@ public interface TeachingService {
*/
Map<String, Object> restart();
/**
* 起点位置重定位
*/
Map<String, Object> startingPointRelocate();
/**
* 一键部署地图
* @return

45
src/main/java/org/nl/apt15e/apt/teaching/service/impl/TeachingServiceImpl.java
View File

@@ -386,6 +386,18 @@ public class TeachingServiceImpl implements TeachingService {
throw new BadRequestException(LangProcess.msg("error_restart"));
}
@Override
public Map<String, Object> 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<String, Object> 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;

5
src/main/java/org/nl/apt15e/apt/vehicle/dao/VehicleInfo.java
View File

@@ -89,6 +89,11 @@ public class VehicleInfo implements Serializable {
*/
private String task_seq;
/**
* 车辆任务链中当前索引
*/
private int task_seq_index;
/**
* 车辆任务链中当前点位
*/

2
src/main/java/org/nl/apt15e/apt/vehicle/service/impl/VehicleInfoServiceImpl.java
View File

@@ -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<String, Object> vehicleInfoMap = new HashMap<>();

49
src/main/java/org/nl/apt15e/config/thread/ProtobufWebSocketHandler.java
View File

@@ -98,23 +98,23 @@ public class ProtobufWebSocketHandler extends BinaryWebSocketHandler {
Robottype.LaserData laser = datagram.getLaserScan();
// 9顶部激光
if (laser.getLocation() == 9){
Set<PointCloudDataDto> globalData = new HashSet<>();
// Set<PointCloudDataDto> globalData = new HashSet<>();
Set<PointCloudDataDto> 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<PointCloudDataDto> 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;

1
src/main/resources/config/application.yml
View File

@@ -50,6 +50,7 @@ security:
- /webSocket/**
- /file/**
- /routeInfo/**
- /calibration/**
- /station/**
# 静态资源
- /*.html