package com.algo.util;
|
|
import com.fasterxml.jackson.databind.ObjectMapper;
|
|
import java.io.*;
|
import java.util.ArrayList;
|
import java.util.HashMap;
|
import java.util.HashSet;
|
import java.util.List;
|
import java.util.Map;
|
import java.util.Set;
|
|
/**
|
* JSON文件读取
|
*/
|
public class JsonUtils {
|
|
/**
|
* @param filePath 文件路径
|
* @return JSON字符串内容
|
* @throws IOException 文件读取异常
|
*/
|
public static String readJsonFile(String filePath) throws IOException {
|
StringBuilder content = new StringBuilder();
|
try (BufferedReader reader = new BufferedReader(new FileReader(filePath))) {
|
String line;
|
while ((line = reader.readLine()) != null) {
|
content.append(line).append("\n");
|
}
|
}
|
return content.toString();
|
}
|
|
/**
|
* 解析路径映射JSON
|
*
|
* @param jsonContent JSON内容
|
* @return 路径映射Map，key为路径编号，value为坐标信息
|
*/
|
public static Map<String, Map<String, Integer>> parsePathMapping(String jsonContent) {
|
Map<String, Map<String, Integer>> pathMapping = new HashMap<>();
|
|
try {
|
String pathIdSection = extractJsonSection(jsonContent, "path_id_to_coordinates");
|
if (pathIdSection == null) {
|
System.err.println("未找到path_id_to_coordinates部分");
|
return pathMapping;
|
}
|
|
String[] lines = pathIdSection.split("\n");
|
String currentKey = null;
|
|
for (String line : lines) {
|
line = line.trim();
|
|
// 查找路径ID
|
if (line.startsWith("\"") && line.contains("\":[")) {
|
int endIndex = line.indexOf("\":[");
|
currentKey = line.substring(1, endIndex);
|
pathMapping.put(currentKey, new HashMap<>());
|
}
|
|
// 查找坐标信息
|
if (currentKey != null && line.contains("\"x\":")) {
|
String xStr = line.substring(line.indexOf("\"x\":") + 4);
|
xStr = xStr.substring(0, xStr.indexOf(",")).trim();
|
try {
|
int x = Integer.parseInt(xStr);
|
pathMapping.get(currentKey).put("x", x);
|
} catch (NumberFormatException e) {
|
// 忽略解析错误
|
}
|
}
|
|
if (currentKey != null && line.contains("\"y\":")) {
|
String yStr = line.substring(line.indexOf("\"y\":") + 4);
|
if (yStr.contains(",")) {
|
yStr = yStr.substring(0, yStr.indexOf(",")).trim();
|
} else if (yStr.contains("}")) {
|
yStr = yStr.substring(0, yStr.indexOf("}")).trim();
|
}
|
try {
|
int y = Integer.parseInt(yStr);
|
pathMapping.get(currentKey).put("y", y);
|
} catch (NumberFormatException e) {
|
// 忽略解析错误
|
}
|
}
|
}
|
|
System.out.println("成功解析路径映射，包含 " + pathMapping.size() + " 个路径点");
|
|
} catch (Exception e) {
|
System.err.println("解析路径映射JSON时发生错误: " + e.getMessage());
|
}
|
|
return pathMapping;
|
}
|
|
/**
|
* 路径映射
|
*
|
* @param filePath 文件地址
|
* @return 路径映射Map，key为路径编号，value为坐标信息
|
*/
|
public static Map<String, Map<String, Integer>> loadPathMapping(String filePath) {
|
// 转换为目标结构 Map<String, Map<String, Integer>>
|
Map<String, Map<String, Integer>> pathMapping = new HashMap<>();
|
ObjectMapper objectMapper = new ObjectMapper();
|
try {
|
Map<String, Object> topLevelMap = objectMapper.readValue(
|
new File(filePath),
|
Map.class
|
);
|
|
if (!topLevelMap.containsKey("path_id_to_coordinates")) {
|
System.err.println("未找到path_id_to_coordinates部分");
|
return pathMapping;
|
}
|
|
// 处理 path_id_to_coordinates
|
Map<String, Object> pathIdCoords = (Map<String, Object>) topLevelMap.get("path_id_to_coordinates");
|
for (Map.Entry<String, Object> entry : pathIdCoords.entrySet()) {
|
String pathId = entry.getKey();
|
Object coordsObj = entry.getValue();
|
if (coordsObj instanceof List) {
|
List<?> coordsList = (List<?>) coordsObj;
|
if (!coordsList.isEmpty()) {
|
Map<?, ?> coordMap = (Map<?, ?>) coordsList.get(0);
|
int x = ((Number) coordMap.get("x")).intValue();
|
int y = ((Number) coordMap.get("y")).intValue();
|
Map<String, Integer> pointMap = new HashMap<>();
|
pointMap.put("x", x);
|
pointMap.put("y", y);
|
pathMapping.put(pathId, pointMap);
|
}
|
}
|
}
|
|
System.out.println("成功解析路径映射，包含 " + pathMapping.size() + " 个路径点");
|
} catch (FileNotFoundException e) {
|
System.err.println("路径映射文件不存在: " + e.getMessage());
|
} catch (IOException e) {
|
System.err.println("路径映射文件读取错误: " + e.getMessage());
|
} catch (Exception e) {
|
System.err.println("加载路径映射文件失败: " + e.getMessage());
|
}
|
return pathMapping;
|
}
|
|
/**
|
* 环境配置JSON
|
*
|
* @param jsonContent JSON内容
|
* @return 环境配置Map
|
*/
|
public static Map<String, Object> parseEnvironmentConfig(String jsonContent) {
|
Map<String, Object> config = new HashMap<>();
|
|
try {
|
if (jsonContent.contains("\"width\":")) {
|
String widthStr = jsonContent.substring(jsonContent.indexOf("\"width\":") + 8);
|
widthStr = widthStr.substring(0, widthStr.indexOf(",")).trim();
|
try {
|
config.put("width", Integer.parseInt(widthStr));
|
} catch (NumberFormatException e) {
|
config.put("width", 78);
|
}
|
}
|
|
if (jsonContent.contains("\"height\":")) {
|
String heightStr = jsonContent.substring(jsonContent.indexOf("\"height\":") + 9);
|
heightStr = heightStr.substring(0, heightStr.indexOf(",")).trim();
|
try {
|
config.put("height", Integer.parseInt(heightStr));
|
} catch (NumberFormatException e) {
|
config.put("height", 50);
|
}
|
}
|
|
Map<String, Map<String, Object>> stations = parseStations(jsonContent);
|
config.put("stations", stations);
|
config.put("stationCount", stations.size());
|
|
System.out.println("成功解析环境配置，包含 " + stations.size() + " 个工作站");
|
|
} catch (Exception e) {
|
System.err.println("解析环境配置JSON时发生错误: " + e.getMessage());
|
}
|
|
return config;
|
}
|
|
/**
|
* 解析environment.json中的stations信息
|
*
|
* @param filePath 文件地址
|
* @return 环境配置Map
|
*/
|
public static Map<String, Object> loadEnvironment(String filePath) {
|
// 转换为目标结构 Map<String, Object>
|
Map<String, Object> environmentMap = new HashMap<>();
|
ObjectMapper objectMapper = new ObjectMapper();
|
try {
|
// 先解析为顶层Map<String, Object>
|
Map<String, Object> topLevelMap = objectMapper.readValue(
|
new File(filePath),
|
Map.class
|
);
|
|
if (topLevelMap.containsKey("width")) {
|
environmentMap.put("width", Integer.parseInt(topLevelMap.get("width").toString()));
|
} else {
|
environmentMap.put("width", 78);
|
}
|
|
if (topLevelMap.containsKey("width")) {
|
environmentMap.put("height", Integer.parseInt(topLevelMap.get("height").toString()));
|
} else {
|
environmentMap.put("height", 50);
|
}
|
|
if (topLevelMap.containsKey("stations")) {
|
Map<String, Map<String, Object>> stations = new HashMap<>();
|
Map<String, Object> stationMap = (Map<String, Object>) topLevelMap.get("stations");
|
for (Map.Entry<String, Object> stationEntry : stationMap.entrySet()) {
|
// 工作站ID
|
String pathId = stationEntry.getKey();
|
Map<String, Object> stationInfo = (Map<String, Object>) stationEntry.getValue();
|
Map<String, Object> map = new HashMap<>();
|
// 解析capacity
|
if (stationInfo.containsKey("capacity")) {
|
map.put("capacity", Integer.parseInt(stationInfo.get("capacity").toString()));
|
}
|
// 解析load_position和unload_position
|
if (stationInfo.containsKey("load_position")) {
|
List<Integer> loadPos = (List<Integer>) stationInfo.get("load_position");
|
map.put("load_position", loadPos);
|
}
|
if (stationInfo.containsKey("unload_position")) {
|
List<Integer> unloadPos = (List<Integer>) stationInfo.get("unload_position");
|
map.put("unload_position", unloadPos);
|
}
|
stations.put(pathId, map);
|
}
|
environmentMap.put("stations", stations);
|
environmentMap.put("stationCount", stations.size());
|
System.out.println("成功解析环境配置，包含 " + stations.size() + " 个工作站");
|
}
|
} catch (FileNotFoundException e) {
|
System.err.println("环境配置文件不存在: " + e.getMessage());
|
} catch (IOException e) {
|
System.err.println("环境配置文件读取错误: " + e.getMessage());
|
} catch (Exception e) {
|
System.err.println("加载环境配置文件失败: " + e.getMessage());
|
}
|
return environmentMap;
|
}
|
|
/**
|
* 解析stations信息
|
*
|
* @param jsonContent JSON内容
|
* @return stations Map
|
*/
|
private static Map<String, Map<String, Object>> parseStations(String jsonContent) {
|
Map<String, Map<String, Object>> stations = new HashMap<>();
|
|
try {
|
String stationsSection = extractJsonSection(jsonContent, "stations");
|
if (stationsSection == null) {
|
return stations;
|
}
|
|
String[] lines = stationsSection.split("\n");
|
String currentStation = null;
|
|
for (String line : lines) {
|
line = line.trim();
|
|
if (line.startsWith("\"") && line.contains("\":{")) {
|
int endIndex = line.indexOf("\":{");
|
currentStation = line.substring(1, endIndex);
|
stations.put(currentStation, new HashMap<>());
|
}
|
|
if (currentStation != null && line.contains("\"capacity\":")) {
|
String capacityStr = line.substring(line.indexOf("\"capacity\":") + 11);
|
capacityStr = capacityStr.substring(0, capacityStr.indexOf(",")).trim();
|
try {
|
int capacity = Integer.parseInt(capacityStr);
|
stations.get(currentStation).put("capacity", capacity);
|
} catch (NumberFormatException e) {
|
// 忽略解析错误
|
}
|
}
|
|
if (currentStation != null && line.contains("\"load_position\":")) {
|
List<Integer> loadPos = parsePosition(stationsSection, currentStation, "load_position");
|
if (loadPos != null) {
|
stations.get(currentStation).put("load_position", loadPos);
|
}
|
}
|
|
if (currentStation != null && line.contains("\"unload_position\":")) {
|
List<Integer> unloadPos = parsePosition(stationsSection, currentStation, "unload_position");
|
if (unloadPos != null) {
|
stations.get(currentStation).put("unload_position", unloadPos);
|
}
|
}
|
}
|
|
} catch (Exception e) {
|
System.err.println("解析stations信息时发生错误: " + e.getMessage());
|
}
|
|
return stations;
|
}
|
|
/**
|
* 解析位置信息
|
*
|
* @param content JSON内容
|
* @param stationId 工作站ID
|
* @param positionType 位置类型（load_position或unload_position）
|
* @return 位置坐标列表
|
*/
|
private static List<Integer> parsePosition(String content, String stationId, String positionType) {
|
try {
|
String stationSection = content.substring(content.indexOf("\"" + stationId + "\":{"));
|
String positionSection = stationSection.substring(stationSection.indexOf("\"" + positionType + "\":"));
|
|
String[] lines = positionSection.split("\n");
|
List<Integer> position = new ArrayList<>();
|
|
for (String line : lines) {
|
line = line.trim();
|
if (line.matches("\\d+,?")) {
|
String numStr = line.replaceAll("[,\\s]", "");
|
try {
|
position.add(Integer.parseInt(numStr));
|
} catch (NumberFormatException e) {
|
// 忽略解析错误
|
}
|
}
|
if (line.contains("]")) {
|
break;
|
}
|
}
|
|
return position.size() == 2 ? position : null;
|
} catch (Exception e) {
|
return null;
|
}
|
}
|
|
/**
|
* 提取JSON中特定部分
|
*
|
* @param jsonContent JSON内容
|
* @param sectionName 部分名称
|
* @return 提取的部分内容
|
*/
|
private static String extractJsonSection(String jsonContent, String sectionName) {
|
try {
|
String startPattern = "\"" + sectionName + "\": {";
|
int startIndex = jsonContent.indexOf(startPattern);
|
if (startIndex == -1) {
|
return null;
|
}
|
|
int braceCount = 0;
|
int currentIndex = startIndex + startPattern.length() - 1;
|
|
while (currentIndex < jsonContent.length()) {
|
char ch = jsonContent.charAt(currentIndex);
|
if (ch == '{') {
|
braceCount++;
|
} else if (ch == '}') {
|
braceCount--;
|
if (braceCount == 0) {
|
break;
|
}
|
}
|
currentIndex++;
|
}
|
|
return jsonContent.substring(startIndex + startPattern.length() - 1, currentIndex + 1);
|
} catch (Exception e) {
|
return null;
|
}
|
}
|
|
/**
|
* 获取路径点的坐标
|
*
|
* @param pathId 路径点ID
|
* @param pathMapping 路径映射
|
* @return 坐标数组 [x, y]，如果未找到返回null
|
*/
|
public static int[] getCoordinate(String pathId, Map<String, Map<String, Integer>> pathMapping) {
|
Map<String, Integer> coordMap = pathMapping.get(pathId);
|
if (coordMap != null && coordMap.containsKey("x") && coordMap.containsKey("y")) {
|
return new int[]{coordMap.get("x"), coordMap.get("y")};
|
}
|
return null;
|
}
|
|
/**
|
* 加载实际物理坐标映射
|
*
|
* @param filePath 文件路径
|
* @return 实际坐标映射 Map<pathId, double[]{x_mm, y_mm}>
|
*/
|
public static Map<String, double[]> loadRealCoordinateMapping(String filePath) {
|
Map<String, double[]> coordinateMapping = new HashMap<>();
|
ObjectMapper objectMapper = new ObjectMapper();
|
|
try {
|
Map<String, Object> topLevelMap = objectMapper.readValue(new File(filePath), Map.class);
|
|
if (!topLevelMap.containsKey("path_id_to_coordinates")) {
|
System.err.println("未找到path_id_to_coordinates部分");
|
return coordinateMapping;
|
}
|
|
Map<String, Object> pathIdCoords = (Map<String, Object>) topLevelMap.get("path_id_to_coordinates");
|
for (Map.Entry<String, Object> entry : pathIdCoords.entrySet()) {
|
String pathId = entry.getKey();
|
Object coordsObj = entry.getValue();
|
|
if (coordsObj instanceof List) {
|
List<?> coordsList = (List<?>) coordsObj;
|
if (!coordsList.isEmpty()) {
|
Map<?, ?> coordMap = (Map<?, ?>) coordsList.get(0);
|
double x = ((Number) coordMap.get("x")).doubleValue();
|
double y = ((Number) coordMap.get("y")).doubleValue();
|
coordinateMapping.put(pathId, new double[]{x, y});
|
}
|
}
|
}
|
|
System.out.println("成功加载实际坐标映射，包含 " + coordinateMapping.size() + " 个路径点");
|
|
} catch (FileNotFoundException e) {
|
System.err.println("实际坐标文件不存在: " + e.getMessage());
|
} catch (IOException e) {
|
System.err.println("实际坐标文件读取错误: " + e.getMessage());
|
} catch (Exception e) {
|
System.err.println("加载实际坐标文件失败: " + e.getMessage());
|
}
|
|
return coordinateMapping;
|
}
|
|
/**
|
* 获取路径点的实际坐标
|
*
|
* @param pathId 路径点ID
|
* @param realCoordinateMap 实际坐标映射
|
* @return 坐标数组 [x_mm, y_mm]，如果未找到返回null
|
*/
|
public static double[] getRealCoordinate(String pathId, Map<String, double[]> realCoordinateMap) {
|
return realCoordinateMap.get(pathId);
|
}
|
|
/**
|
* 获取工作站信息
|
*
|
* @param stationId 工作站ID
|
* @param environmentConfig 环境配置
|
* @return 工作站信息Map
|
*/
|
@SuppressWarnings("unchecked")
|
public static Map<String, Object> getStationInfo(String stationId, Map<String, Object> environmentConfig) {
|
Map<String, Map<String, Object>> stations =
|
(Map<String, Map<String, Object>>) environmentConfig.get("stations");
|
|
if (stations != null) {
|
return stations.get(stationId);
|
}
|
return null;
|
}
|
|
/**
|
* 判断位置是否为工作站
|
*
|
* @param position 位置字符串
|
* @param environmentConfig 环境配置
|
* @return 是否为工作站
|
*/
|
public static boolean isStation(String position, Map<String, Object> environmentConfig) {
|
return getStationInfo(position, environmentConfig) != null;
|
}
|
|
/**
|
* 计算两点之间的曼哈顿距离
|
*
|
* @param coord1 坐标1 [x, y]
|
* @param coord2 坐标2 [x, y]
|
* @return 曼哈顿距离
|
*/
|
public static double calculateManhattanDistance(int[] coord1, int[] coord2) {
|
if (coord1 == null || coord2 == null || coord1.length != 2 || coord2.length != 2) {
|
return Double.MAX_VALUE;
|
}
|
|
return Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
|
}
|
|
/**
|
* 计算两点之间的欧几里得距离
|
*
|
* @param coord1 坐标1 [x, y]
|
* @param coord2 坐标2 [x, y]
|
* @return 欧几里得距离
|
*/
|
public static double calculateEuclideanDistance(int[] coord1, int[] coord2) {
|
if (coord1 == null || coord2 == null || coord1.length != 2 || coord2.length != 2) {
|
return Double.MAX_VALUE;
|
}
|
|
int dx = coord1[0] - coord2[0];
|
int dy = coord1[1] - coord2[1];
|
return Math.sqrt(dx * dx + dy * dy);
|
}
|
|
/**
|
* 加载环境路径连通性数据（environment.json中的paths和obstacles）
|
*
|
* @param filePath 环境文件路径
|
* @return 环境连通性数据
|
*/
|
public static EnvironmentConnectivity loadEnvironmentConnectivity(String filePath) {
|
EnvironmentConnectivity connectivity = new EnvironmentConnectivity();
|
ObjectMapper objectMapper = new ObjectMapper();
|
|
try {
|
Map<String, Object> topLevelMap = objectMapper.readValue(new File(filePath), Map.class);
|
|
// 加载可通行路径
|
if (topLevelMap.containsKey("paths")) {
|
List<Map<String, Object>> pathsList = (List<Map<String, Object>>) topLevelMap.get("paths");
|
for (Map<String, Object> pathPoint : pathsList) {
|
Integer x = (Integer) pathPoint.get("x");
|
Integer y = (Integer) pathPoint.get("y");
|
if (x != null && y != null) {
|
connectivity.addValidPath(x, y);
|
}
|
}
|
}
|
|
// 加载障碍物
|
if (topLevelMap.containsKey("obstacles")) {
|
List<Map<String, Object>> obstaclesList = (List<Map<String, Object>>) topLevelMap.get("obstacles");
|
for (Map<String, Object> obstacle : obstaclesList) {
|
Integer x = (Integer) obstacle.get("x");
|
Integer y = (Integer) obstacle.get("y");
|
if (x != null && y != null) {
|
connectivity.addObstacle(x, y);
|
}
|
}
|
}
|
|
System.out.println("成功加载环境连通性数据：" +
|
connectivity.getValidPathsCount() + " 个可通行点，" +
|
connectivity.getObstaclesCount() + " 个障碍物点");
|
|
} catch (FileNotFoundException e) {
|
System.err.println("环境文件不存在: " + e.getMessage());
|
} catch (IOException e) {
|
System.err.println("环境文件读取错误: " + e.getMessage());
|
} catch (Exception e) {
|
System.err.println("加载环境连通性数据失败: " + e.getMessage());
|
}
|
|
return connectivity;
|
}
|
|
/**
|
* 环境连通性数据类
|
*/
|
public static class EnvironmentConnectivity {
|
private final Set<String> validPaths = new HashSet<>();
|
private final Set<String> obstacles = new HashSet<>();
|
|
public void addValidPath(int x, int y) {
|
validPaths.add(x + "," + y);
|
}
|
|
public void addObstacle(int x, int y) {
|
obstacles.add(x + "," + y);
|
}
|
|
public boolean isValidPath(int x, int y) {
|
return validPaths.contains(x + "," + y);
|
}
|
|
public boolean isObstacle(int x, int y) {
|
return obstacles.contains(x + "," + y);
|
}
|
|
public boolean isTraversable(int x, int y) {
|
return isValidPath(x, y) && !isObstacle(x, y);
|
}
|
|
public int getValidPathsCount() {
|
return validPaths.size();
|
}
|
|
public int getObstaclesCount() {
|
return obstacles.size();
|
}
|
|
/**
|
* 获取有效的相邻节点
|
*/
|
public List<int[]> getValidNeighbors(int x, int y) {
|
List<int[]> neighbors = new ArrayList<>();
|
int[][] directions = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}}; // 上下左右
|
|
for (int[] dir : directions) {
|
int newX = x + dir[0];
|
int newY = y + dir[1];
|
|
if (isTraversable(newX, newY)) {
|
neighbors.add(new int[]{newX, newY});
|
}
|
}
|
|
return neighbors;
|
}
|
}
|
}
|
