Add unit tests for PathConflictDetectionService to validate conflict detection logic，优化了碰撞检测逻辑
This commit is contained in:
parent
aae4877622
commit
aa4468d922
@ -1,34 +1,34 @@
|
||||
package com.qaup.collision.pathconflict.service;
|
||||
|
||||
import com.qaup.collision.pathconflict.model.entity.TransportRoute;
|
||||
import com.qaup.collision.pathconflict.repository.TransportRouteRepository;
|
||||
import com.qaup.collision.common.model.MovingObject;
|
||||
import com.qaup.collision.common.model.MovingObject.MovingObjectType;
|
||||
import com.qaup.collision.pathconflict.model.dto.ConflictAlertEvent;
|
||||
import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment;
|
||||
import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository;
|
||||
import com.qaup.collision.pathconflict.model.entity.ConflictAlertLog;
|
||||
import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository;
|
||||
import com.qaup.collision.dataprocessing.service.CoordinateSystemService;
|
||||
|
||||
import com.qaup.collision.pathconflict.model.dto.ConflictAlertEvent;
|
||||
import com.qaup.collision.pathconflict.model.entity.ConflictAlertLog;
|
||||
import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment;
|
||||
import com.qaup.collision.pathconflict.model.entity.TransportRoute;
|
||||
import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository;
|
||||
import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository;
|
||||
import com.qaup.collision.pathconflict.repository.TransportRouteRepository;
|
||||
import lombok.RequiredArgsConstructor;
|
||||
import lombok.extern.slf4j.Slf4j;
|
||||
import org.locationtech.jts.geom.*;
|
||||
import org.locationtech.jts.geom.Coordinate;
|
||||
import org.locationtech.jts.geom.Geometry;
|
||||
import org.locationtech.jts.geom.GeometryFactory;
|
||||
import org.locationtech.jts.geom.LineString;
|
||||
import org.locationtech.jts.geom.MultiPoint;
|
||||
import org.locationtech.jts.geom.Point;
|
||||
import org.locationtech.jts.geom.PrecisionModel;
|
||||
import org.locationtech.jts.operation.distance.DistanceOp;
|
||||
import org.springframework.stereotype.Service;
|
||||
import org.springframework.context.ApplicationEventPublisher;
|
||||
import org.springframework.stereotype.Service;
|
||||
|
||||
import java.util.*;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
import java.util.UUID;
|
||||
|
||||
/**
|
||||
* 路径冲突检测服务
|
||||
* 基于路径、速度和距离的冲突检测算法
|
||||
*
|
||||
* @author AI Assistant
|
||||
* @version 1.0
|
||||
* @since 2025-01-17
|
||||
*/
|
||||
@Slf4j
|
||||
@Service
|
||||
@RequiredArgsConstructor
|
||||
@ -38,74 +38,119 @@ public class PathConflictDetectionService {
|
||||
private final ObjectRouteAssignmentRepository objectRouteAssignmentRepository;
|
||||
private final ConflictAlertLogRepository conflictAlertLogRepository;
|
||||
private final ApplicationEventPublisher eventPublisher;
|
||||
private final CoordinateSystemService coordinateSystemService; // 注入 CoordinateSystemService
|
||||
private final CoordinateSystemService coordinateSystemService;
|
||||
|
||||
// 预警距离阈值（米）
|
||||
private static final double WARNING_DISTANCE_THRESHOLD = 200.0;
|
||||
// 告警距离阈值（米）
|
||||
private static final double ALERT_DISTANCE_THRESHOLD = 100.0;
|
||||
// 最大检测时间范围（秒）
|
||||
private static final int MAX_PREDICTION_TIME_SECONDS = 300; // 5分钟
|
||||
// 最小时间间隔阈值（秒）
|
||||
private static final int MAX_PREDICTION_TIME_SECONDS = 300;
|
||||
private static final double MIN_TIME_GAP_SECONDS = 30.0;
|
||||
|
||||
/**
|
||||
* 执行路径冲突检测
|
||||
*
|
||||
* @param activeObjects 当前活跃的移动对象（无人车和航空器）
|
||||
*/
|
||||
// Strong accuracy guards
|
||||
private static final double MIN_FORWARD_DISTANCE_METERS = 3.0;
|
||||
private static final double MAX_ROUTE_DEVIATION_METERS = 80.0;
|
||||
private static final double HEADING_ALIGNMENT_MIN_COS = 0.0;
|
||||
private static final double HEADING_CHECK_MIN_SPEED_KPH = 3.0;
|
||||
private static final double MIN_EFFECTIVE_SPEED_KPH = 1.0;
|
||||
|
||||
public void detectPathConflicts(List<MovingObject> activeObjects) {
|
||||
log.debug("开始路径冲突检测，活跃对象数量: {}", activeObjects.size());
|
||||
log.debug("Starting path conflict detection for {} active objects", activeObjects.size());
|
||||
|
||||
List<ConflictAlertEvent> detectedAlertEvents = new ArrayList<>();
|
||||
|
||||
// 生成所有对象组合
|
||||
for (int i = 0; i < activeObjects.size(); i++) {
|
||||
for (int j = i + 1; j < activeObjects.size(); j++) {
|
||||
MovingObject obj1 = activeObjects.get(i);
|
||||
MovingObject obj2 = activeObjects.get(j);
|
||||
|
||||
// 检测两个对象之间的潜在冲突
|
||||
Optional<ConflictAlertEvent> alertEventOptional = detectConflictBetweenObjects(obj1, obj2);
|
||||
alertEventOptional.ifPresent(detectedAlertEvents::add);
|
||||
}
|
||||
}
|
||||
|
||||
// 发布告警事件
|
||||
for (ConflictAlertEvent event : detectedAlertEvents) {
|
||||
eventPublisher.publishEvent(event);
|
||||
log.info("发布冲突告警事件: conflictId={}, alertType={}, alertLevel={}",
|
||||
log.info("Published path conflict event: conflictId={}, alertType={}, alertLevel={}",
|
||||
event.getConflictId(), event.getAlertType(), event.getAlertLevel());
|
||||
}
|
||||
|
||||
log.info("路径冲突检测完成，检测到并发布 {} 个潜在冲突告警事件", detectedAlertEvents.size());
|
||||
log.info("Path conflict detection completed, {} events published", detectedAlertEvents.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* 检测两个对象之间的路径冲突
|
||||
*/
|
||||
private Optional<ConflictAlertEvent> detectConflictBetweenObjects(MovingObject obj1, MovingObject obj2) {
|
||||
if (obj1.getObjectType() != MovingObjectType.UNMANNED_VEHICLE && obj2.getObjectType() != MovingObjectType.UNMANNED_VEHICLE) {
|
||||
log.debug("对象 {} 和 {} 都不是无人车，跳过冲突检测", obj1.getObjectName(), obj2.getObjectName());
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
try {
|
||||
// 获取对象的路径
|
||||
TransportRoute route1 = getObjectRoute(obj1);
|
||||
TransportRoute route2 = getObjectRoute(obj2);
|
||||
|
||||
if (route1 == null || route2 == null) {
|
||||
log.debug("对象 {} 或 {} 没有分配路径，跳过冲突检测", obj1.getObjectName(), obj2.getObjectName());
|
||||
if (obj1.getCurrentPosition() == null || obj2.getCurrentPosition() == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
// 计算路径交点
|
||||
List<Point> intersectionPoints = calculateRouteIntersections(route1, route2);
|
||||
TransportRoute route1 = getObjectRoute(obj1);
|
||||
TransportRoute route2 = getObjectRoute(obj2);
|
||||
|
||||
if (route1 == null || route2 == null || route1.getRouteGeometry() == null || route2.getRouteGeometry() == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
LocalRoute localRoute1 = toLocalRoute(route1);
|
||||
LocalRoute localRoute2 = toLocalRoute(route2);
|
||||
if (localRoute1 == null || localRoute2 == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
Point localObjPos1 = toLocalPoint(obj1.getCurrentPosition());
|
||||
Point localObjPos2 = toLocalPoint(obj2.getCurrentPosition());
|
||||
if (localObjPos1 == null || localObjPos2 == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
ProjectedPosition projected1 = projectToRoute(localObjPos1, localRoute1.localLine);
|
||||
ProjectedPosition projected2 = projectToRoute(localObjPos2, localRoute2.localLine);
|
||||
if (projected1 == null || projected2 == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
if (projected1.lateralDistanceMeters > MAX_ROUTE_DEVIATION_METERS || projected2.lateralDistanceMeters > MAX_ROUTE_DEVIATION_METERS) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
double speed1Kph = normalizeSpeedKph(obj1.getCurrentSpeed());
|
||||
double speed2Kph = normalizeSpeedKph(obj2.getCurrentSpeed());
|
||||
|
||||
// Ignore pairs that are effectively static.
|
||||
if (speed1Kph < MIN_EFFECTIVE_SPEED_KPH || speed2Kph < MIN_EFFECTIVE_SPEED_KPH) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
List<Point> intersectionPoints = calculateRouteIntersections(route1, route2);
|
||||
for (Point intersectionPoint : intersectionPoints) {
|
||||
// 计算每个对象到交点的距离和时间
|
||||
ConflictCalculationResult result = calculateConflictDetails(obj1, obj2, route1, route2, intersectionPoint);
|
||||
Point localIntersection = toLocalPoint(intersectionPoint);
|
||||
if (localIntersection == null) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ProjectedPosition projectedIntersection1 = projectToRoute(localIntersection, localRoute1.localLine);
|
||||
ProjectedPosition projectedIntersection2 = projectToRoute(localIntersection, localRoute2.localLine);
|
||||
if (projectedIntersection1 == null || projectedIntersection2 == null) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double forwardDistance1 = projectedIntersection1.distanceAlongRouteMeters - projected1.distanceAlongRouteMeters;
|
||||
double forwardDistance2 = projectedIntersection2.distanceAlongRouteMeters - projected2.distanceAlongRouteMeters;
|
||||
|
||||
// Only future conflicts are valid.
|
||||
if (forwardDistance1 <= MIN_FORWARD_DISTANCE_METERS || forwardDistance2 <= MIN_FORWARD_DISTANCE_METERS) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Ensure current heading points to the intersection direction when speed is meaningful.
|
||||
if (!isHeadingAligned(obj1, localObjPos1, localIntersection, speed1Kph)
|
||||
|| !isHeadingAligned(obj2, localObjPos2, localIntersection, speed2Kph)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ConflictCalculationResult result = calculateConflictDetails(
|
||||
obj1, obj2, forwardDistance1, forwardDistance2, speed1Kph, speed2Kph);
|
||||
|
||||
if (result != null && isSignificantConflict(result)) {
|
||||
String description = generateConflictDescription(obj1, obj2, route1, route2);
|
||||
@ -116,11 +161,10 @@ public class PathConflictDetectionService {
|
||||
.alertMessage(description)
|
||||
.object1Distance(result.getDistance1())
|
||||
.object2Distance(result.getDistance2())
|
||||
.minimumDistance(Math.min(result.getDistance1(), result.getDistance2())) // 简化处理，可根据需要调整
|
||||
.minimumDistance(Math.min(result.getDistance1(), result.getDistance2()))
|
||||
.build();
|
||||
|
||||
conflictAlertLogRepository.save(alertLog);
|
||||
log.info("保存冲突告警日志: {}", alertLog);
|
||||
|
||||
return Optional.of(ConflictAlertEvent.builder()
|
||||
.conflictId(Optional.of(alertLog.getId()))
|
||||
@ -140,30 +184,25 @@ public class PathConflictDetectionService {
|
||||
.build());
|
||||
}
|
||||
}
|
||||
|
||||
} catch (Exception e) {
|
||||
log.error("检测对象 {} 和 {} 之间的冲突时发生异常", obj1.getObjectName(), obj2.getObjectName(), e);
|
||||
log.error("Error detecting conflict between {} and {}", obj1.getObjectName(), obj2.getObjectName(), e);
|
||||
}
|
||||
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* 获取对象当前使用的路径
|
||||
*/
|
||||
private TransportRoute getObjectRoute(MovingObject obj) {
|
||||
Optional<ObjectRouteAssignment> assignmentOptional = objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc(
|
||||
obj.getObjectName(), // Changed from getObjectId
|
||||
Optional<ObjectRouteAssignment> assignmentOptional = objectRouteAssignmentRepository
|
||||
.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc(
|
||||
obj.getObjectName(),
|
||||
ObjectRouteAssignment.ObjectType.valueOf(obj.getObjectType().name())
|
||||
);
|
||||
|
||||
return assignmentOptional.map(assignment -> routeRepository.findById(assignment.getAssignedRouteId()).orElse(null)) // Changed findByRouteId to findById
|
||||
return assignmentOptional
|
||||
.map(assignment -> routeRepository.findById(assignment.getAssignedRouteId()).orElse(null))
|
||||
.orElse(null);
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算两条路径的交点
|
||||
*/
|
||||
private List<Point> calculateRouteIntersections(TransportRoute route1, TransportRoute route2) {
|
||||
List<Point> intersections = new ArrayList<>();
|
||||
|
||||
@ -181,114 +220,102 @@ public class PathConflictDetectionService {
|
||||
intersections.add((Point) multiPoint.getGeometryN(i));
|
||||
}
|
||||
} else if (intersection instanceof LineString) {
|
||||
// 如果是线段重叠，取线段的中点
|
||||
LineString overlapLine = (LineString) intersection;
|
||||
Point midPoint = overlapLine.getInteriorPoint();
|
||||
// 确保中间点不为空且不是POINT EMPTY
|
||||
if (midPoint != null && !midPoint.isEmpty()) {
|
||||
intersections.add(midPoint);
|
||||
} else {
|
||||
log.warn("LineString交点生成了空的中间点或无效点，跳过此交点。OverlapLine: {}", overlapLine);
|
||||
}
|
||||
}
|
||||
|
||||
} catch (Exception e) {
|
||||
log.error("计算路径交点时出错", e);
|
||||
log.error("Error calculating route intersections", e);
|
||||
}
|
||||
|
||||
return intersections;
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算冲突详细信息
|
||||
*/
|
||||
private ConflictCalculationResult calculateConflictDetails(
|
||||
MovingObject obj1, MovingObject obj2,
|
||||
TransportRoute route1, TransportRoute route2,
|
||||
Point intersectionPoint) {
|
||||
MovingObject obj1,
|
||||
MovingObject obj2,
|
||||
double distance1,
|
||||
double distance2,
|
||||
double speed1Kph,
|
||||
double speed2Kph) {
|
||||
|
||||
try {
|
||||
// 计算对象到交点的距离
|
||||
double distance1 = calculateDistanceAlongRoute(obj1.getCurrentPosition(), intersectionPoint, route1);
|
||||
double distance2 = calculateDistanceAlongRoute(obj2.getCurrentPosition(), intersectionPoint, route2);
|
||||
double speed1Ms = speed1Kph * 1000.0 / 3600.0;
|
||||
double speed2Ms = speed2Kph * 1000.0 / 3600.0;
|
||||
|
||||
// 计算到达交点的时间（基于当前速度）
|
||||
// 处理速度为null的情况，使用默认最小速度1 km/h
|
||||
double speed1 = Math.max(obj1.getCurrentSpeed() != null ? obj1.getCurrentSpeed() : 1.0, 1.0); // 避免除零，最小速度1 km/h
|
||||
double speed2 = Math.max(obj2.getCurrentSpeed() != null ? obj2.getCurrentSpeed() : 1.0, 1.0);
|
||||
int timeToConflict1 = (int) Math.round(distance1 / speed1Ms);
|
||||
int timeToConflict2 = (int) Math.round(distance2 / speed2Ms);
|
||||
|
||||
// 转换为m/s
|
||||
double speed1_ms = speed1 * 1000.0 / 3600.0;
|
||||
double speed2_ms = speed2 * 1000.0 / 3600.0;
|
||||
if (timeToConflict1 > MAX_PREDICTION_TIME_SECONDS || timeToConflict2 > MAX_PREDICTION_TIME_SECONDS) {
|
||||
return null;
|
||||
}
|
||||
|
||||
int timeToConflict1 = (int) (distance1 / speed1_ms);
|
||||
int timeToConflict2 = (int) (distance2 / speed2_ms);
|
||||
|
||||
// 计算时间差
|
||||
double timeGap = Math.abs(timeToConflict1 - timeToConflict2);
|
||||
|
||||
// 评估冲突告警级别和类型
|
||||
Optional<ConflictAlertLog.AlertLevel> alertLevelOptional = evaluateAlertLevel(distance1, obj1.getObjectType(), distance2, obj2.getObjectType(), timeGap);
|
||||
Optional<ConflictAlertLog.AlertLevel> alertLevelOptional = evaluateAlertLevel(
|
||||
distance1,
|
||||
obj1.getObjectType(),
|
||||
distance2,
|
||||
obj2.getObjectType(),
|
||||
timeGap
|
||||
);
|
||||
Optional<ConflictAlertLog.AlertType> alertTypeOptional = alertLevelOptional.isPresent()
|
||||
? evaluateAlertType(alertLevelOptional.get())
|
||||
: Optional.empty();
|
||||
|
||||
return new ConflictCalculationResult(
|
||||
distance1, distance2, timeToConflict1, timeToConflict2, timeGap,
|
||||
alertTypeOptional, alertLevelOptional,
|
||||
obj1.getObjectType(), obj2.getObjectType()); // Pass object types
|
||||
distance1,
|
||||
distance2,
|
||||
timeToConflict1,
|
||||
timeToConflict2,
|
||||
timeGap,
|
||||
alertTypeOptional,
|
||||
alertLevelOptional,
|
||||
obj1.getObjectType(),
|
||||
obj2.getObjectType());
|
||||
|
||||
} catch (Exception e) {
|
||||
log.error("计算冲突详细信息时出错", e);
|
||||
log.error("Error calculating conflict details", e);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 评估冲突的告警级别
|
||||
*/
|
||||
private Optional<ConflictAlertLog.AlertLevel> evaluateAlertLevel(
|
||||
double distance1, MovingObjectType obj1Type,
|
||||
double distance2, MovingObjectType obj2Type,
|
||||
double distance1,
|
||||
MovingObjectType obj1Type,
|
||||
double distance2,
|
||||
MovingObjectType obj2Type,
|
||||
double timeGap) {
|
||||
|
||||
boolean obj1IsUnmannedVehicle = MovingObjectType.UNMANNED_VEHICLE.equals(obj1Type);
|
||||
boolean obj2IsUnmannedVehicle = MovingObjectType.UNMANNED_VEHICLE.equals(obj2Type);
|
||||
|
||||
if (!obj1IsUnmannedVehicle && !obj2IsUnmannedVehicle) {
|
||||
// 双方都不是无人车，根据业务规则，不生成告警。
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
double distanceToEvaluate;
|
||||
|
||||
if (obj1IsUnmannedVehicle && obj2IsUnmannedVehicle) {
|
||||
// 双方都是无人车，关注两者中离交点最近的距离
|
||||
distanceToEvaluate = Math.min(distance1, distance2);
|
||||
} else if (obj1IsUnmannedVehicle) {
|
||||
// 只有 obj1 是无人车，关注 obj1 的距离。obj2 是无人车需要避让的对象。
|
||||
distanceToEvaluate = distance1;
|
||||
} else { // obj2IsUnmannedVehicle
|
||||
// 只有 obj2 是无人车，关注 obj2 的距离。obj1 是无人车需要避让的对象。
|
||||
} else {
|
||||
distanceToEvaluate = distance2;
|
||||
}
|
||||
|
||||
// 现在根据确定的 relevantDistance 评估告警级别
|
||||
if (distanceToEvaluate <= ALERT_DISTANCE_THRESHOLD && timeGap <= MIN_TIME_GAP_SECONDS) {
|
||||
return Optional.of(ConflictAlertLog.AlertLevel.CRITICAL);
|
||||
} else if (distanceToEvaluate <= WARNING_DISTANCE_THRESHOLD && timeGap <= MIN_TIME_GAP_SECONDS * 2) {
|
||||
return Optional.of(ConflictAlertLog.AlertLevel.WARNING);
|
||||
} else {
|
||||
// 不满足 CRITICAL/WARNING 阈值，不生成告警。
|
||||
return Optional.empty();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 根据告警级别评估告警类型
|
||||
*/
|
||||
private Optional<ConflictAlertLog.AlertType> evaluateAlertType(ConflictAlertLog.AlertLevel alertLevel) {
|
||||
// 处理null输入
|
||||
if (alertLevel == null) {
|
||||
return Optional.empty();
|
||||
}
|
||||
@ -300,165 +327,195 @@ public class PathConflictDetectionService {
|
||||
case EMERGENCY:
|
||||
return Optional.of(ConflictAlertLog.AlertType.CONFLICT_ALERT);
|
||||
default:
|
||||
// 应该不会到达这里，因为所有的 AlertLevel 都已经被明确处理
|
||||
return Optional.empty();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 判断是否为显著冲突
|
||||
*/
|
||||
private boolean isSignificantConflict(ConflictCalculationResult result) {
|
||||
return result.getAlertLevel().isPresent() &&
|
||||
(result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.WARNING ||
|
||||
result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.CRITICAL ||
|
||||
result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.EMERGENCY);
|
||||
(result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.WARNING
|
||||
|| result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.CRITICAL
|
||||
|| result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.EMERGENCY);
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算沿路径的距离
|
||||
*/
|
||||
private double calculateDistanceAlongRoute(Point currentPos, Point targetPos, TransportRoute route) {
|
||||
private double normalizeSpeedKph(Double speedKph) {
|
||||
if (speedKph == null || speedKph.isNaN() || speedKph.isInfinite() || speedKph < 0.0) {
|
||||
return 0.0;
|
||||
}
|
||||
return speedKph;
|
||||
}
|
||||
|
||||
private boolean isHeadingAligned(MovingObject obj, Point currentPos, Point targetPos, double speedKph) {
|
||||
if (speedKph < HEADING_CHECK_MIN_SPEED_KPH) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (obj.getCurrentHeading() == null) {
|
||||
return true;
|
||||
}
|
||||
|
||||
double vx = targetPos.getX() - currentPos.getX();
|
||||
double vy = targetPos.getY() - currentPos.getY();
|
||||
double vNorm = Math.hypot(vx, vy);
|
||||
if (vNorm < 1e-6) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Heading convention: 0 north, 90 east.
|
||||
double headingRad = Math.toRadians(obj.getCurrentHeading());
|
||||
double hx = Math.sin(headingRad);
|
||||
double hy = Math.cos(headingRad);
|
||||
double dot = (hx * vx + hy * vy) / vNorm;
|
||||
|
||||
return dot >= HEADING_ALIGNMENT_MIN_COS;
|
||||
}
|
||||
|
||||
private Point toLocalPoint(Point wgs84Point) {
|
||||
try {
|
||||
// 1. 获取原始路径几何和点
|
||||
LineString originalRouteLine = route.getRouteGeometry();
|
||||
|
||||
// 2. 将所有相关几何对象转换为局部米制坐标系
|
||||
// 2.1 转换 currentPos 和 targetPos
|
||||
double[] localCurrentCoords = coordinateSystemService.convertToLocalCoordinate(currentPos.getX(), currentPos.getY());
|
||||
double[] localTargetCoords = coordinateSystemService.convertToLocalCoordinate(targetPos.getX(), targetPos.getY());
|
||||
|
||||
GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0); // SRID 0 for Cartesian system (meters)
|
||||
Point localCurrentPos = geometryFactory.createPoint(new Coordinate(localCurrentCoords[0], localCurrentCoords[1]));
|
||||
Point localTargetPos = geometryFactory.createPoint(new Coordinate(localTargetCoords[0], localTargetCoords[1]));
|
||||
|
||||
// 2.2 转换 routeLine
|
||||
Coordinate[] originalCoords = originalRouteLine.getCoordinates();
|
||||
Coordinate[] localRouteCoords = new Coordinate[originalCoords.length];
|
||||
for (int i = 0; i < originalCoords.length; i++) {
|
||||
double[] localCoord = coordinateSystemService.convertToLocalCoordinate(originalCoords[i].x, originalCoords[i].y);
|
||||
localRouteCoords[i] = new Coordinate(localCoord[0], localCoord[1]);
|
||||
}
|
||||
LineString localRouteLine = geometryFactory.createLineString(localRouteCoords);
|
||||
|
||||
// 3. 在局部米制坐标系中执行距离计算
|
||||
// 3.1 找到当前位置在路径上的最近点
|
||||
DistanceOp distOp1 = new DistanceOp(localCurrentPos, localRouteLine);
|
||||
Coordinate[] nearestPoints1 = distOp1.nearestPoints();
|
||||
Point nearestOnRoute1 = geometryFactory.createPoint(nearestPoints1[1]);
|
||||
|
||||
// 3.2 找到目标位置在路径上的最近点
|
||||
DistanceOp distOp2 = new DistanceOp(localTargetPos, localRouteLine);
|
||||
Coordinate[] nearestPoints2 = distOp2.nearestPoints();
|
||||
Point nearestOnRoute2 = geometryFactory.createPoint(nearestPoints2[1]);
|
||||
|
||||
// 3.3 计算沿路径的距离 (现在这些距离将是米)
|
||||
double distanceToCurrentOnRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute1);
|
||||
double distanceToTargetOnRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute2);
|
||||
|
||||
return Math.abs(distanceToTargetOnRoute - distanceToCurrentOnRoute);
|
||||
|
||||
double[] local = coordinateSystemService.convertToLocalCoordinate(wgs84Point.getX(), wgs84Point.getY());
|
||||
GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0);
|
||||
return geometryFactory.createPoint(new Coordinate(local[0], local[1]));
|
||||
} catch (Exception e) {
|
||||
log.error("计算沿路径距离时出错: currentPos={}, targetPos={}",
|
||||
currentPos, targetPos, e);
|
||||
return -1.0; // 表示错误
|
||||
log.debug("Failed to convert point to local coordinates", e);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private LocalRoute toLocalRoute(TransportRoute route) {
|
||||
try {
|
||||
LineString original = route.getRouteGeometry();
|
||||
Coordinate[] originalCoords = original.getCoordinates();
|
||||
if (originalCoords.length < 2) {
|
||||
return null;
|
||||
}
|
||||
|
||||
Coordinate[] localCoords = new Coordinate[originalCoords.length];
|
||||
for (int i = 0; i < originalCoords.length; i++) {
|
||||
double[] local = coordinateSystemService.convertToLocalCoordinate(originalCoords[i].x, originalCoords[i].y);
|
||||
localCoords[i] = new Coordinate(local[0], local[1]);
|
||||
}
|
||||
|
||||
GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0);
|
||||
return new LocalRoute(route, geometryFactory.createLineString(localCoords));
|
||||
} catch (Exception e) {
|
||||
log.debug("Failed to convert route {} to local coordinates", route.getRouteName(), e);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private ProjectedPosition projectToRoute(Point localPoint, LineString localRouteLine) {
|
||||
try {
|
||||
DistanceOp distanceOp = new DistanceOp(localPoint, localRouteLine);
|
||||
Coordinate[] nearestPoints = distanceOp.nearestPoints();
|
||||
if (nearestPoints == null || nearestPoints.length < 2 || nearestPoints[1] == null) {
|
||||
return null;
|
||||
}
|
||||
|
||||
GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0);
|
||||
Point nearestOnRoute = geometryFactory.createPoint(nearestPoints[1]);
|
||||
double lateralDistance = localPoint.distance(nearestOnRoute);
|
||||
double distanceAlongRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute);
|
||||
if (distanceAlongRoute < 0) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return new ProjectedPosition(distanceAlongRoute, lateralDistance);
|
||||
} catch (Exception e) {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 生成冲突ID
|
||||
*/
|
||||
private String generateConflictId(String obj1Name, String obj2Name) {
|
||||
// 确保ID的生成是确定性的，与对象的顺序无关
|
||||
String[] names = {obj1Name, obj2Name};
|
||||
Arrays.sort(names);
|
||||
return "CONFLICT-" + names[0] + "-" + names[1] + "-" + UUID.randomUUID().toString().substring(0, 8);
|
||||
}
|
||||
|
||||
/**
|
||||
* 生成冲突描述
|
||||
*/
|
||||
private String generateConflictDescription(
|
||||
MovingObject obj1, MovingObject obj2,
|
||||
TransportRoute route1, TransportRoute route2) {
|
||||
return String.format("对象 %s (%s) 在路径上与对象 %s (%s) 可能发生冲突。",
|
||||
MovingObject obj1,
|
||||
MovingObject obj2,
|
||||
TransportRoute route1,
|
||||
TransportRoute route2) {
|
||||
return String.format("Objects %s (%s) and %s (%s) may conflict on assigned routes",
|
||||
obj1.getObjectName(), obj1.getObjectType(),
|
||||
obj2.getObjectName(), obj2.getObjectType());
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算沿LineString从起点到给定点的距离。
|
||||
* 假定point已经投影到line上。
|
||||
* 如果点不在line上，结果可能不准确。
|
||||
*/
|
||||
private double getLinearLengthToPoint(LineString line, Point point) {
|
||||
double length = 0.0;
|
||||
Coordinate[] coords = line.getCoordinates();
|
||||
|
||||
for (int i = 0; i < coords.length - 1; i++) {
|
||||
Point p1 = line.getFactory().createPoint(coords[i]);
|
||||
Point p2 = line.getFactory().createPoint(coords[i+1]);
|
||||
Point p2 = line.getFactory().createPoint(coords[i + 1]);
|
||||
|
||||
// 如果点在线段 (p1, p2) 上，则计算到p1的距离，然后加上点到p1的距离
|
||||
if (point.equals(p1)) {
|
||||
return length;
|
||||
}
|
||||
|
||||
double segmentLength = p1.distance(p2);
|
||||
// 检查点是否在当前线段上 (使用一个小的容差)
|
||||
if (point.distance(p1) + point.distance(p2) - segmentLength < 1e-6) { // 容差检查
|
||||
if (point.distance(p1) + point.distance(p2) - segmentLength < 1e-6) {
|
||||
return length + p1.distance(point);
|
||||
}
|
||||
length += segmentLength;
|
||||
}
|
||||
// 如果点是最后一个点
|
||||
|
||||
if (point.equals(line.getEndPoint())) {
|
||||
return line.getLength();
|
||||
}
|
||||
// 如果没有找到匹配，返回-1或者抛异常，这里返回线长表示无法准确计算
|
||||
return -1.0; // 表示未找到精确位置或计算错误
|
||||
|
||||
return -1.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 冲突计算结果内部类
|
||||
*/
|
||||
private record LocalRoute(TransportRoute route, LineString localLine) {}
|
||||
|
||||
private record ProjectedPosition(double distanceAlongRouteMeters, double lateralDistanceMeters) {}
|
||||
|
||||
private static class ConflictCalculationResult {
|
||||
private final double distance1;
|
||||
private final double distance2;
|
||||
private final int timeToConflict1;
|
||||
private final int timeToConflict2;
|
||||
private final double timeGap;
|
||||
private final Optional<ConflictAlertLog.AlertType> alertType; // Changed to Optional
|
||||
private final Optional<ConflictAlertLog.AlertLevel> alertLevel; // Changed to Optional
|
||||
private final MovingObjectType object1Type; // New
|
||||
private final MovingObjectType object2Type; // New
|
||||
private final Optional<ConflictAlertLog.AlertType> alertType;
|
||||
private final Optional<ConflictAlertLog.AlertLevel> alertLevel;
|
||||
private final MovingObjectType object1Type;
|
||||
private final MovingObjectType object2Type;
|
||||
|
||||
public ConflictCalculationResult(double distance1, double distance2,
|
||||
int timeToConflict1, int timeToConflict2,
|
||||
double timeGap,
|
||||
Optional<ConflictAlertLog.AlertType> alertType, // Changed to Optional
|
||||
Optional<ConflictAlertLog.AlertLevel> alertLevel, // Changed to Optional
|
||||
MovingObjectType object1Type, // New
|
||||
MovingObjectType object2Type) { // New
|
||||
Optional<ConflictAlertLog.AlertType> alertType,
|
||||
Optional<ConflictAlertLog.AlertLevel> alertLevel,
|
||||
MovingObjectType object1Type,
|
||||
MovingObjectType object2Type) {
|
||||
this.distance1 = distance1;
|
||||
this.distance2 = distance2;
|
||||
this.timeToConflict1 = timeToConflict1;
|
||||
this.timeToConflict2 = timeToConflict2;
|
||||
this.timeGap = timeGap;
|
||||
this.alertType = alertType; // Changed
|
||||
this.alertLevel = alertLevel; // Changed
|
||||
this.object1Type = object1Type; // New
|
||||
this.object2Type = object2Type; // New
|
||||
this.alertType = alertType;
|
||||
this.alertLevel = alertLevel;
|
||||
this.object1Type = object1Type;
|
||||
this.object2Type = object2Type;
|
||||
}
|
||||
|
||||
public double getDistance1() { return distance1; }
|
||||
|
||||
public double getDistance2() { return distance2; }
|
||||
|
||||
public int getTimeToConflict1() { return timeToConflict1; }
|
||||
|
||||
public int getTimeToConflict2() { return timeToConflict2; }
|
||||
|
||||
public double getTimeGap() { return timeGap; }
|
||||
public Optional<ConflictAlertLog.AlertType> getAlertType() { return alertType; } // Changed
|
||||
public Optional<ConflictAlertLog.AlertLevel> getAlertLevel() { return alertLevel; } // Changed
|
||||
public MovingObjectType getObject1Type() { return object1Type; } // New
|
||||
public MovingObjectType getObject2Type() { return object2Type; } // New
|
||||
|
||||
public Optional<ConflictAlertLog.AlertType> getAlertType() { return alertType; }
|
||||
|
||||
public Optional<ConflictAlertLog.AlertLevel> getAlertLevel() { return alertLevel; }
|
||||
|
||||
public MovingObjectType getObject1Type() { return object1Type; }
|
||||
|
||||
public MovingObjectType getObject2Type() { return object2Type; }
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,134 @@
|
||||
package com.qaup.collision.pathconflict.service;
|
||||
|
||||
import com.qaup.collision.common.model.MovingObject;
|
||||
import com.qaup.collision.common.model.MovingObject.MovingObjectType;
|
||||
import com.qaup.collision.dataprocessing.service.CoordinateSystemService;
|
||||
import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment;
|
||||
import com.qaup.collision.pathconflict.model.entity.TransportRoute;
|
||||
import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository;
|
||||
import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository;
|
||||
import com.qaup.collision.pathconflict.repository.TransportRouteRepository;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.extension.ExtendWith;
|
||||
import org.locationtech.jts.geom.Coordinate;
|
||||
import org.locationtech.jts.geom.GeometryFactory;
|
||||
import org.locationtech.jts.geom.LineString;
|
||||
import org.locationtech.jts.geom.Point;
|
||||
import org.mockito.InjectMocks;
|
||||
import org.mockito.Mock;
|
||||
import org.mockito.junit.jupiter.MockitoExtension;
|
||||
import org.springframework.context.ApplicationEventPublisher;
|
||||
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import static org.mockito.ArgumentMatchers.any;
|
||||
import static org.mockito.ArgumentMatchers.anyDouble;
|
||||
import static org.mockito.Mockito.never;
|
||||
import static org.mockito.Mockito.verify;
|
||||
import static org.mockito.Mockito.when;
|
||||
|
||||
@ExtendWith(MockitoExtension.class)
|
||||
class PathConflictDetectionDirectionalTest {
|
||||
|
||||
@Mock
|
||||
private ObjectRouteAssignmentRepository objectRouteAssignmentRepository;
|
||||
|
||||
@Mock
|
||||
private TransportRouteRepository routeRepository;
|
||||
|
||||
@Mock
|
||||
private ConflictAlertLogRepository conflictAlertLogRepository;
|
||||
|
||||
@Mock
|
||||
private ApplicationEventPublisher eventPublisher;
|
||||
|
||||
@Mock
|
||||
private CoordinateSystemService coordinateSystemService;
|
||||
|
||||
@InjectMocks
|
||||
private PathConflictDetectionService pathConflictDetectionService;
|
||||
|
||||
@Test
|
||||
void shouldNotAlertWhenIntersectionIsBehindMovingDirection() throws Exception {
|
||||
GeometryFactory geometryFactory = new GeometryFactory();
|
||||
|
||||
// Route 1: horizontal line, intersection at (0,0), object currently east of intersection.
|
||||
LineString routeLine1 = geometryFactory.createLineString(new Coordinate[]{
|
||||
new Coordinate(-1.0, 0.0),
|
||||
new Coordinate(1.0, 0.0)
|
||||
});
|
||||
|
||||
// Route 2: vertical line crossing route 1 at (0,0).
|
||||
LineString routeLine2 = geometryFactory.createLineString(new Coordinate[]{
|
||||
new Coordinate(0.0, -1.0),
|
||||
new Coordinate(0.0, 1.0)
|
||||
});
|
||||
|
||||
Point obj1Pos = geometryFactory.createPoint(new Coordinate(0.5, 0.0));
|
||||
Point obj2Pos = geometryFactory.createPoint(new Coordinate(0.0, -0.5));
|
||||
|
||||
MovingObject obj1 = MovingObject.builder()
|
||||
.objectId("UV-1")
|
||||
.objectName("UV-1")
|
||||
.objectType(MovingObjectType.UNMANNED_VEHICLE)
|
||||
.currentPosition(obj1Pos)
|
||||
.currentSpeed(20.0)
|
||||
.currentHeading(90.0) // East, away from intersection at x=0
|
||||
.altitude(0.0)
|
||||
.build();
|
||||
|
||||
MovingObject obj2 = MovingObject.builder()
|
||||
.objectId("AC-1")
|
||||
.objectName("AC-1")
|
||||
.objectType(MovingObjectType.AIRCRAFT)
|
||||
.currentPosition(obj2Pos)
|
||||
.currentSpeed(20.0)
|
||||
.currentHeading(0.0)
|
||||
.altitude(0.0)
|
||||
.build();
|
||||
|
||||
ObjectRouteAssignment assignment1 = ObjectRouteAssignment.builder()
|
||||
.objectName("UV-1")
|
||||
.objectType(ObjectRouteAssignment.ObjectType.UNMANNED_VEHICLE)
|
||||
.assignedRouteId(1L)
|
||||
.build();
|
||||
|
||||
ObjectRouteAssignment assignment2 = ObjectRouteAssignment.builder()
|
||||
.objectName("AC-1")
|
||||
.objectType(ObjectRouteAssignment.ObjectType.AIRCRAFT)
|
||||
.assignedRouteId(2L)
|
||||
.build();
|
||||
|
||||
TransportRoute route1 = TransportRoute.builder()
|
||||
.id(1L)
|
||||
.routeName("R1")
|
||||
.routeType(TransportRoute.RouteType.UNMANNED_VEHICLE)
|
||||
.routeGeometry(routeLine1)
|
||||
.build();
|
||||
|
||||
TransportRoute route2 = TransportRoute.builder()
|
||||
.id(2L)
|
||||
.routeName("R2")
|
||||
.routeType(TransportRoute.RouteType.AIRCRAFT)
|
||||
.routeGeometry(routeLine2)
|
||||
.build();
|
||||
|
||||
when(objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc("UV-1", ObjectRouteAssignment.ObjectType.UNMANNED_VEHICLE))
|
||||
.thenReturn(Optional.of(assignment1));
|
||||
when(objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc("AC-1", ObjectRouteAssignment.ObjectType.AIRCRAFT))
|
||||
.thenReturn(Optional.of(assignment2));
|
||||
|
||||
when(routeRepository.findById(1L)).thenReturn(Optional.of(route1));
|
||||
when(routeRepository.findById(2L)).thenReturn(Optional.of(route2));
|
||||
|
||||
// Use identity transform for deterministic test geometry.
|
||||
when(coordinateSystemService.convertToLocalCoordinate(anyDouble(), anyDouble()))
|
||||
.thenAnswer(invocation -> new double[]{invocation.getArgument(0), invocation.getArgument(1)});
|
||||
|
||||
pathConflictDetectionService.detectPathConflicts(List.of(obj1, obj2));
|
||||
|
||||
verify(conflictAlertLogRepository, never()).save(any());
|
||||
verify(eventPublisher, never()).publishEvent(any());
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user