package com.zy.acs.manager.core.service;
|
|
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
|
import com.zy.acs.common.constant.RedisConstant;
|
import com.zy.acs.common.utils.GsonUtils;
|
import com.zy.acs.common.utils.RedisSupport;
|
import com.zy.acs.framework.common.Cools;
|
import com.zy.acs.framework.common.SnowflakeIdWorker;
|
import com.zy.acs.manager.core.domain.Lane;
|
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
|
import com.zy.acs.manager.manager.entity.Code;
|
import com.zy.acs.manager.manager.enums.StatusType;
|
import com.zy.acs.manager.manager.service.CodeService;
|
import com.zy.acs.manager.manager.service.RouteService;
|
import com.zy.acs.manager.system.service.ConfigService;
|
import lombok.extern.slf4j.Slf4j;
|
import org.apache.commons.lang.time.StopWatch;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Service;
|
|
import javax.annotation.PostConstruct;
|
import java.security.MessageDigest;
|
import java.security.NoSuchAlgorithmException;
|
import java.util.*;
|
import java.util.stream.Collectors;
|
|
/**
|
* Created by vincent on 10/25/2024
|
*/
|
@Slf4j
|
@Service
|
public class LaneService {
|
|
private final RedisSupport redis = RedisSupport.defaultRedisSupport;
|
|
private List<Lane> lanes = new ArrayList<>();
|
|
private final Map<String, List<String>> adjacencyCodeMap = new HashMap<>();
|
|
private boolean initialized = Boolean.FALSE;
|
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private RouteService routeService;
|
@Autowired
|
private SnowflakeIdWorker snowflakeIdWorker;
|
@Autowired
|
private ConfigService configService;
|
|
public Boolean isInitialized() {
|
return this.initialized;
|
}
|
|
public Lane search(String codeData) {
|
if (Cools.isEmpty(codeData) || !this.initialized) {
|
return null;
|
}
|
for (Lane lane : this.lanes) {
|
if (lane.getCodes().contains(codeData)) {
|
return lane;
|
}
|
}
|
return null;
|
}
|
|
@PostConstruct
|
public void init() {
|
Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
|
String laneDataStr = redis.getValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev));
|
if (!Cools.isEmpty(laneDataStr)) {
|
this.lanes = GsonUtils.fromJsonToList(laneDataStr, Lane.class);
|
} else {
|
|
StopWatch stopWatch = new StopWatch();
|
stopWatch.start();
|
|
this.initLaneData();
|
|
stopWatch.stop();
|
log.info("the rcs system calculated lane data which has spend {} millisecond......", stopWatch.getTime());
|
|
redis.setValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev), GsonUtils.toJson(this.lanes));
|
}
|
|
// System.out.println(GsonUtils.toJson(this.lanes));
|
}
|
|
private void initLaneData() {
|
log.info("the rcs system is starting to initialize lane data...");
|
|
List<Code> codeList = codeService.list(new LambdaQueryWrapper<Code>().eq(Code::getStatus, StatusType.ENABLE.val));
|
|
this.fillAdjacencyCodeMap(codeList);
|
|
this.generateLane(codeList.stream().map(Code::getData).collect(Collectors.toList()));
|
|
this.mergeDeadEndLane();
|
|
this.deleteInteractionPoint();
|
|
this.filterLanesWithFewPoints();
|
|
this.generateLaneHash();
|
|
this.initialized = Boolean.TRUE;
|
log.info("the lane data initialization has been completed in rcs system.");
|
}
|
|
private void fillAdjacencyCodeMap(List<Code> codeList) {
|
for (Code code : codeList) {
|
List<Long> adjacencyNode = routeService.getAdjacencyNode(code.getId());
|
this.adjacencyCodeMap.put(code.getData(), adjacencyNode.stream().map(node -> (
|
codeService.getById(node).getData()
|
)).collect(Collectors.toList()));
|
}
|
}
|
|
private void generateLane(List<String> codeDataList) {
|
Set<String> visited = new HashSet<>();
|
|
for (String codeData : codeDataList) {
|
if (this.adjacencyCodeMap.get(codeData).size() != 2) {
|
List<String> neighbors = this.adjacencyCodeMap.get(codeData);
|
for (String neighbor : neighbors) {
|
if (this.adjacencyCodeMap.get(neighbor).size() == 2 && !visited.contains(neighbor)) {
|
Lane lane = new Lane(String.valueOf(snowflakeIdWorker.nextId()).substring(3));
|
lane.getCodes().add(codeData); // 包含起点
|
this.dfsCalcIncludingEnd(codeData, neighbor, lane, visited);
|
this.lanes.add(lane);
|
}
|
}
|
}
|
}
|
|
for (String codeData : codeDataList) {
|
if (this.adjacencyCodeMap.get(codeData).size() == 2 && !visited.contains(codeData)) {
|
// 检查是否为环路的一部分
|
Lane lane = new Lane(String.valueOf(snowflakeIdWorker.nextId()).substring(3));
|
this.dfsCalcForLoop(codeData, null, lane, visited);
|
this.lanes.add(lane);
|
}
|
}
|
|
|
}
|
|
private void dfsCalcIncludingEnd(String start, String current, Lane lane, Set<String> visited) {
|
lane.getCodes().add(current);
|
visited.add(current);
|
|
List<String> neighbors = this.adjacencyCodeMap.get(current);
|
if (neighbors == null || neighbors.isEmpty()) {
|
return;
|
}
|
|
for (String neighbor : neighbors) {
|
if (neighbor.equals(start)) {
|
continue;
|
}
|
|
if (!visited.contains(neighbor)) {
|
int degree = this.adjacencyCodeMap.get(neighbor).size();
|
if (degree == 2) {
|
if (this.isSameDirection(current, neighbor, start)) {
|
this.dfsCalcIncludingEnd(current, neighbor, lane, visited);
|
}
|
} else {
|
// 终点或拐弯点，包含并停止
|
lane.getCodes().add(neighbor);
|
visited.add(neighbor);
|
}
|
}
|
}
|
}
|
|
private void dfsCalcForLoop(String current, String parent, Lane lane, Set<String> visited) {
|
lane.getCodes().add(current);
|
visited.add(current);
|
|
List<String> neighbors = this.adjacencyCodeMap.get(current);
|
if (neighbors == null || neighbors.isEmpty()) {
|
return;
|
}
|
|
for (String neighbor : neighbors) {
|
if (neighbor.equals(parent)) {
|
continue;
|
}
|
|
if (!visited.contains(neighbor)) {
|
int degree = this.adjacencyCodeMap.get(neighbor).size();
|
if (degree == 2) {
|
if (this.isSameDirection(current, neighbor, parent)) {
|
this.dfsCalcForLoop(current, neighbor, lane, visited);
|
}
|
} else {
|
lane.getCodes().add(neighbor);
|
visited.add(neighbor);
|
}
|
}
|
}
|
}
|
|
private boolean isSameDirection(String current, String neighbor, String parent) {
|
if (parent == null) {
|
return true;
|
}
|
|
Code parentCode = codeService.selectByData(parent);
|
Code currentCode = codeService.selectByData(current);
|
Code neighborCode = codeService.selectByData(neighbor);
|
|
double direction1 = this.calculateDirection(parentCode, currentCode);
|
double direction2 = this.calculateDirection(currentCode, neighborCode);
|
double angleDifference = Math.abs(direction1 - direction2);
|
|
// 规范化角度差
|
angleDifference = Math.min(angleDifference, 2 * Math.PI - angleDifference);
|
|
// 设置角度差阈值为3度
|
return angleDifference < Math.toRadians(3);
|
}
|
|
/**
|
* 计算两个点之间的方向角
|
*/
|
private double calculateDirection(Code from, Code to) {
|
double deltaX = to.getX() - from.getX();
|
double deltaY = to.getY() - from.getY();
|
return Math.atan2(deltaY, deltaX);
|
}
|
|
/**
|
* 1.explore a dead end lane and checkout end point that is not dead point
|
* 2.if this point has two near points and one of those points which not in current lane was also have two near points too
|
* 3.then merge above two lane because they can connect each other
|
*/
|
private void mergeDeadEndLane() {
|
Iterator<Lane> iterator = this.lanes.iterator();
|
while (iterator.hasNext()) {
|
Lane lane = iterator.next();
|
String[] endPoints = lane.queryEndPoints();
|
if (null == endPoints) {
|
continue;
|
}
|
String startPoint = endPoints[0];
|
String endPoint = endPoints[1];
|
|
boolean isDeadEndLane = false;
|
String deadEndPoint = null;
|
Integer anotherPointNearsCount = null;
|
if (this.adjacencyCodeMap.get(startPoint).size() == 1) {
|
isDeadEndLane = true;
|
deadEndPoint = startPoint;
|
anotherPointNearsCount = this.adjacencyCodeMap.get(endPoint).size();
|
}
|
if (this.adjacencyCodeMap.get(endPoint).size() == 1) {
|
isDeadEndLane = true;
|
deadEndPoint = endPoint;
|
anotherPointNearsCount = this.adjacencyCodeMap.get(startPoint).size();
|
}
|
|
if (!isDeadEndLane) {
|
continue;
|
}
|
|
if (anotherPointNearsCount == 2) {
|
String anotherPoint = deadEndPoint.equals(startPoint) ? endPoint : startPoint;
|
List<String> anotherPointNears = this.adjacencyCodeMap.get(anotherPoint);
|
for (String anotherPointNear : anotherPointNears) {
|
if (!lane.getCodes().contains(anotherPointNear) && this.adjacencyCodeMap.get(anotherPointNear).size() == 2) {
|
|
for (Lane lane0 : lanes) {
|
if (lane0.getCodes().contains(anotherPointNear)) {
|
|
lane0.getCodes().addAll(lane.getCodes());
|
iterator.remove();
|
|
lane0.sortUsingDfs(this.adjacencyCodeMap);
|
|
}
|
}
|
|
}
|
}
|
}
|
|
}
|
}
|
|
private void deleteInteractionPoint() {
|
for (Lane lane : this.lanes) {
|
lane.removeInteraction(this.adjacencyCodeMap);
|
}
|
}
|
|
private void filterLanesWithFewPoints() {
|
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
|
this.lanes.removeIf(next -> next.getCodes().size() <= Optional.ofNullable(maxAgvCountInLane).orElse(2));
|
}
|
|
|
private void generateLaneHash() {
|
for (Lane lane : this.lanes) {
|
String hashCode = generateDigest(lane.getCodes());
|
lane.setHashCode(hashCode);
|
}
|
}
|
|
public static String generateDigest(List<String> list) {
|
MessageDigest md = null;
|
try {
|
md = MessageDigest.getInstance("SHA-256");
|
} catch (NoSuchAlgorithmException e) {
|
log.error("generateDigest", e);
|
throw new RuntimeException(e);
|
}
|
String combined = list.stream().sorted().collect(Collectors.joining(","));
|
byte[] hashBytes = md.digest(combined.getBytes());
|
|
StringBuilder sb = new StringBuilder();
|
for (byte b : hashBytes) {
|
sb.append(String.format("%02x", b));
|
}
|
return sb.toString();
|
}
|
|
}
|
