新增有障碍物的路径规划算法和优化没有障碍物的路径算法

张世豪

6 天以前 1cf1ecbc75c6d14b40efb3161e7db0b8b64f7de2

 src/zhuye/MapRenderer.java

@@ -18,6 +18,7 @@
import java.util.List;
import java.util.Locale;
import java.util.Set;
import set.Setsys;
import gecaoji.Device;
import gecaoji.Gecaoji;
import gecaoji.GecaojiMeg;
@@ -35,13 +36,15 @@
 */
public class MapRenderer {
    // 视图变换参数
    private double scale = 1.0;
    private static final double DEFAULT_SCALE = 20.0; // 默认缩放比例
    private double scale = DEFAULT_SCALE;
    private double translateX = 0.0;
    private double translateY = 0.0;
    private Point lastDragPoint;
    private static final double MIN_SCALE = 0.05d;
    private static final double MAX_SCALE = 50.0d;
    private static final double SCALE_EPSILON = 1e-6d;
    private static final String MAP_SCALE_PROPERTY = "mapScale"; // 属性文件中的键名
    
    // 主题颜色
    private final Color THEME_COLOR = new Color(46, 139, 87);
@@ -111,6 +114,64 @@
        this.mowerUpdateTimer = createMowerTimer();
        this.mowerInfoManager = new GecaojiMeg(visualizationPanel, mower);
        setupMouseListeners();
        // 从配置文件读取上次保存的缩放比例和视图中心坐标
        loadViewSettingsFromProperties();
    }
    
    /**
     * 从配置文件读取缩放比例和视图中心坐标
     */
    private void loadViewSettingsFromProperties() {
        // 加载缩放比例
        String scaleValue = Setsys.getPropertyValue(MAP_SCALE_PROPERTY);
        if (scaleValue != null && !scaleValue.trim().isEmpty()) {
            try {
                double savedScale = Double.parseDouble(scaleValue.trim());
                // 验证缩放比例是否在有效范围内
                if (savedScale >= MIN_SCALE && savedScale <= MAX_SCALE) {
                    scale = savedScale;
                } else {
                    scale = DEFAULT_SCALE;
                }
            } catch (NumberFormatException e) {
                // 如果解析失败，使用默认值
                scale = DEFAULT_SCALE;
            }
        } else {
            // 如果没有保存的值，使用默认值
            scale = DEFAULT_SCALE;
        }
        
        // 加载视图中心坐标
        String viewCenterXValue = Setsys.getPropertyValue("viewCenterX");
        String viewCenterYValue = Setsys.getPropertyValue("viewCenterY");
        if (viewCenterXValue != null && !viewCenterXValue.trim().isEmpty()) {
            try {
                translateX = Double.parseDouble(viewCenterXValue.trim());
            } catch (NumberFormatException e) {
                translateX = 0.0;
            }
        } else {
            translateX = 0.0;
        }
        if (viewCenterYValue != null && !viewCenterYValue.trim().isEmpty()) {
            try {
                translateY = Double.parseDouble(viewCenterYValue.trim());
            } catch (NumberFormatException e) {
                translateY = 0.0;
            }
        } else {
            translateY = 0.0;
        }
    }
    
    /**
     * 保存缩放比例到配置文件
     */
    private void saveScaleToProperties() {
        Setsys setsys = new Setsys();
        // 保留2位小数
        setsys.updateProperty(MAP_SCALE_PROPERTY, String.format("%.2f", scale));
    }
    
    /**
@@ -218,6 +279,8 @@
        translateX += (newWorldX - worldX);
        translateY += (newWorldY - worldY);

        // 保存缩放比例到配置文件
        saveScaleToProperties();
        visualizationPanel.repaint();
    }

@@ -253,9 +316,11 @@
     * 重置视图
     */
    public void resetView() {
        scale = 1.0;
        scale = DEFAULT_SCALE;
        translateX = 0.0;
        translateY = 0.0;
        // 保存缩放比例到配置文件
        saveScaleToProperties();
        visualizationPanel.repaint();
    }
    
@@ -281,14 +346,11 @@
        boolean hasPlannedPath = currentPlannedPath != null && currentPlannedPath.size() >= 2;
        boolean hasObstacles = currentObstacles != null && !currentObstacles.isEmpty();

        // 绘制地块边界（底层）
        if (hasBoundary) {
            drawCurrentBoundary(g2d);
        }

        if (hasObstacles) {
            Obstacledraw.drawObstacles(g2d, currentObstacles, scale, selectedObstacleName);
        }

        yulanzhangaiwu.renderPreview(g2d, scale);

        if (!circleSampleMarkers.isEmpty()) {
@@ -301,10 +363,16 @@

    adddikuaiyulan.drawPreview(g2d, handheldBoundaryPreview, scale, handheldBoundaryPreviewActive, boundaryPreviewMarkerScale);

        // 绘制导航路径（中层）
        if (hasPlannedPath) {
            drawCurrentPlannedPath(g2d);
        }

        // 绘制障碍物（顶层，显示在地块和导航路径上方）
        if (hasObstacles) {
            Obstacledraw.drawObstacles(g2d, currentObstacles, scale, selectedObstacleName);
        }

        if (boundaryPointsVisible && hasBoundary) {
            double markerScale = boundaryPointSizeScale * (previewSizingEnabled ? PREVIEW_BOUNDARY_MARKER_SCALE : 1.0d);
            pointandnumber.drawBoundaryPoints(
@@ -482,7 +550,8 @@
        if (device == null) {
            return;
        }
        if (!isHighPrecisionFix(device.getPositioningStatus())) {
        // 使用更宽松的定位状态判断，允许状态1和4显示拖尾
        if (!isValidFixForTrail(device.getPositioningStatus())) {
            return;
        }

@@ -504,6 +573,56 @@
        idleMowerTrail.addLast(new tuowei.TrailSample(now, new Point2D.Double(position.x, position.y)));
        pruneIdleMowerTrail(now);
    }
    
    /**
     * 强制更新拖尾（用于收到$GNGGA数据时立即更新）
     * 这个方法会刷新mower位置并立即添加到拖尾
     */
    public void forceUpdateIdleMowerTrail() {
        long now = System.currentTimeMillis();
        pruneIdleMowerTrail(now);

        if (idleTrailSuppressed || realtimeTrackRecording) {
            if (!idleMowerTrail.isEmpty()) {
                clearIdleMowerTrail();
            }
            return;
        }

        Device device = Device.getGecaoji();
        if (device == null) {
            return;
        }
        // 使用更宽松的定位状态判断，允许状态1和4显示拖尾
        if (!isValidFixForTrail(device.getPositioningStatus())) {
            return;
        }

        // 刷新mower位置，使用最新的Device数据
        mower.refreshFromDevice();
        Point2D.Double position = mower.getPosition();
        if (position == null || !Double.isFinite(position.x) || !Double.isFinite(position.y)) {
            return;
        }

        tuowei.TrailSample lastSample = idleMowerTrail.peekLast();
        if (lastSample != null) {
            Point2D.Double lastPoint = lastSample.getPoint();
            double dx = position.x - lastPoint.x;
            double dy = position.y - lastPoint.y;
            if (Math.hypot(dx, dy) < IDLE_TRAIL_SAMPLE_DISTANCE_METERS) {
                return;
            }
        }

        idleMowerTrail.addLast(new tuowei.TrailSample(now, new Point2D.Double(position.x, position.y)));
        pruneIdleMowerTrail(now);
        
        // 立即重绘，确保拖尾及时显示
        if (visualizationPanel != null) {
            visualizationPanel.repaint();
        }
    }

    private void pruneIdleMowerTrail(long now) {
        if (idleMowerTrail.isEmpty()) {
@@ -1233,6 +1352,32 @@
            return false;
        }
    }
    
    /**
     * 判断定位状态是否有效，可用于显示拖尾
     * 接受状态1（单点定位）、2（码差分）、3（无效PPS）、4（固定解）、5（浮点解）
     */
    private boolean isValidFixForTrail(String fixQuality) {
        if (fixQuality == null) {
            return false;
        }
        String trimmed = fixQuality.trim();
        if (trimmed.isEmpty()) {
            return false;
        }
        // 接受状态1,2,3,4,5（只要不是0或无效状态）
        if ("1".equals(trimmed) || "2".equals(trimmed) || "3".equals(trimmed) || 
            "4".equals(trimmed) || "5".equals(trimmed)) {
            return true;
        }
        try {
            double value = Double.parseDouble(trimmed);
            // 接受1.0, 2.0, 3.0, 4.0, 5.0（只要不是0）
            return value >= 1.0 && value <= 5.0;
        } catch (NumberFormatException ex) {
            return false;
        }
    }

    private boolean isPointInsideActiveBoundary(Point2D.Double point) {
        if (point == null || !Double.isFinite(point.x) || !Double.isFinite(point.y)) {
@@ -1339,7 +1484,15 @@
     * 设置视图变换参数（用于程序化控制）
     */
    public void setViewTransform(double scale, double translateX, double translateY) {
        this.scale = scale;
        // 限制缩放范围
        scale = Math.max(MIN_SCALE, Math.min(scale, MAX_SCALE));
        // 如果缩放比例改变了，保存到配置文件
        if (Math.abs(this.scale - scale) > SCALE_EPSILON) {
            this.scale = scale;
            saveScaleToProperties();
        } else {
            this.scale = scale;
        }
        this.translateX = translateX;
        this.translateY = translateY;
        visualizationPanel.repaint();