836 lines
32 KiB
Python
836 lines
32 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
仙工SMAP到华睿地图包转换器
|
||
输入仙工SMAP文件和Scene文件，输出华睿地图包和站点映射表
|
||
"""
|
||
|
||
import json
|
||
import os
|
||
import sys
|
||
import argparse
|
||
import zipfile
|
||
import csv
|
||
from datetime import datetime
|
||
from PIL import Image
|
||
import numpy as np
|
||
|
||
class SmapToHuaruiConverter:
|
||
def __init__(self):
|
||
self.smap_data = None
|
||
self.scene_data = None
|
||
self.map_name = ""
|
||
self.map_width = 0
|
||
self.map_height = 0
|
||
self.x_attr_min = 0
|
||
self.y_attr_min = 0
|
||
self.pgm_data = None
|
||
self.huarui_stations = []
|
||
self.station_mapping = []
|
||
self.csv_mapping = {} # 从Scene文件提取的站点映射
|
||
self.points_mapping = [] # Scene文件中的点位映射
|
||
|
||
def load_scene_file(self, scene_path):
|
||
"""加载Scene文件"""
|
||
try:
|
||
with open(scene_path, 'r', encoding='utf-8') as f:
|
||
self.scene_data = json.load(f)
|
||
print(f"Scene文件加载成功: {scene_path}")
|
||
return True
|
||
except FileNotFoundError:
|
||
print(f"文件不存在: {scene_path}")
|
||
return False
|
||
except json.JSONDecodeError as e:
|
||
print(f"Scene文件格式错误: {e}")
|
||
return False
|
||
except Exception as e:
|
||
print(f"加载Scene文件失败: {e}")
|
||
return False
|
||
|
||
def extract_points_mapping(self):
|
||
"""提取points字段的映射关系"""
|
||
if not self.scene_data or 'points' not in self.scene_data:
|
||
print("Scene数据中未找到points字段")
|
||
return False
|
||
|
||
points = self.scene_data['points']
|
||
if not points:
|
||
print("points字段为空")
|
||
return False
|
||
|
||
print(f"开始提取站点映射关系，共{len(points)}个站点")
|
||
|
||
self.points_mapping = []
|
||
|
||
for i, point in enumerate(points, 1):
|
||
mapping = {
|
||
'sequence': i,
|
||
'name': point.get('name', ''),
|
||
'id': point.get('id', '')
|
||
}
|
||
|
||
self.points_mapping.append(mapping)
|
||
# 同时填充csv_mapping用于后续处理
|
||
if mapping['name']:
|
||
self.csv_mapping[mapping['name']] = mapping['id']
|
||
|
||
print(f"站点映射提取完成，共{len(self.points_mapping)}个站点")
|
||
return True
|
||
|
||
def generate_csv_report(self, output_path):
|
||
"""生成CSV格式的映射表"""
|
||
if not self.points_mapping:
|
||
print("没有映射数据可导出")
|
||
return False
|
||
|
||
try:
|
||
with open(output_path, 'w', newline='', encoding='utf-8-sig') as csvfile:
|
||
fieldnames = ['sequence', 'name', 'id']
|
||
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
|
||
|
||
# 写入表头
|
||
writer.writeheader()
|
||
|
||
# 写入数据
|
||
for mapping in self.points_mapping:
|
||
writer.writerow(mapping)
|
||
|
||
print(f"CSV映射表生成完成: {output_path}")
|
||
return True
|
||
except Exception as e:
|
||
print(f"生成CSV文件失败: {e}")
|
||
return False
|
||
|
||
def load_smap_file(self, smap_path):
|
||
"""加载SMAP文件"""
|
||
try:
|
||
with open(smap_path, 'r', encoding='utf-8') as f:
|
||
self.smap_data = json.load(f)
|
||
print(f"SMAP文件加载成功: {smap_path}")
|
||
return True
|
||
except FileNotFoundError:
|
||
print(f"文件不存在: {smap_path}")
|
||
return False
|
||
except json.JSONDecodeError as e:
|
||
print(f"SMAP文件格式错误: {e}")
|
||
return False
|
||
except Exception as e:
|
||
print(f"加载SMAP文件失败: {e}")
|
||
return False
|
||
|
||
def set_map_parameters(self, map_name, map_width, map_height, x_attr_min, y_attr_min):
|
||
"""设置地图参数"""
|
||
self.map_name = map_name
|
||
self.map_width = map_width
|
||
self.map_height = map_height
|
||
self.x_attr_min = x_attr_min
|
||
self.y_attr_min = y_attr_min
|
||
print(f"地图参数设置完成: {map_name} ({map_width}x{map_height})")
|
||
|
||
def generate_pgm_from_pointcloud(self):
|
||
"""从点云数据生成PGM地图，参照map_generator.html的方法"""
|
||
if not self.smap_data or 'normalPosList' not in self.smap_data:
|
||
print("SMAP数据中未找到normalPosList")
|
||
return False
|
||
|
||
normal_pos_list = self.smap_data['normalPosList']
|
||
if not normal_pos_list:
|
||
print("normalPosList为空")
|
||
return False
|
||
|
||
print(f"开始生成PGM地图，点云数量: {len(normal_pos_list)}")
|
||
|
||
# 过滤和验证点云数据
|
||
valid_points = []
|
||
invalid_count = 0
|
||
|
||
for i, point in enumerate(normal_pos_list):
|
||
# 检查点是否有必要的坐标字段
|
||
if isinstance(point, dict) and 'x' in point and 'y' in point:
|
||
# 检查坐标是否为有效数字
|
||
try:
|
||
x = float(point['x'])
|
||
y = float(point['y'])
|
||
valid_points.append({'x': x, 'y': y})
|
||
except (ValueError, TypeError):
|
||
invalid_count += 1
|
||
if invalid_count <= 5: # 只显示前5个错误
|
||
print(f"点 {i} 坐标无效: {point}")
|
||
else:
|
||
invalid_count += 1
|
||
if invalid_count <= 5: # 只显示前5个错误
|
||
print(f"点 {i} 格式无效: {point}")
|
||
|
||
if invalid_count > 0:
|
||
print(f"发现 {invalid_count} 个无效点，已过滤掉")
|
||
|
||
if not valid_points:
|
||
print("没有有效的点云数据")
|
||
return False
|
||
|
||
print(f"有效点云数量: {len(valid_points)}")
|
||
|
||
# 计算点云范围
|
||
xs = [p['x'] for p in valid_points]
|
||
ys = [p['y'] for p in valid_points]
|
||
min_x, max_x = min(xs), max(xs)
|
||
min_y, max_y = min(ys), max(ys)
|
||
range_x = max_x - min_x
|
||
range_y = max_y - min_y
|
||
|
||
print(f"点云范围: X[{min_x:.2f}, {max_x:.2f}], Y[{min_y:.2f}, {max_y:.2f}]")
|
||
|
||
# 设置分辨率和图像尺寸（参照map_generator.html的方法）
|
||
resolution = 0.02 # 2cm/pixel
|
||
width = max(512, int(np.ceil(range_x / resolution)))
|
||
height = max(512, int(np.ceil(range_y / resolution)))
|
||
|
||
print(f"PGM尺寸: {width}x{height}, 分辨率: {resolution}m/pixel")
|
||
|
||
# 创建图像数据，初始化为255（自由空间）
|
||
image_data = np.full(width * height, 255, dtype=np.uint8)
|
||
|
||
# 将点云数据映射到图像（参照map_generator.html的逻辑）
|
||
for point in valid_points:
|
||
pixel_x = int(np.floor((point['x'] - min_x) / resolution))
|
||
pixel_y = int(np.floor((point['y'] - min_y) / resolution))
|
||
|
||
if 0 <= pixel_x < width and 0 <= pixel_y < height:
|
||
# Y轴翻转
|
||
index = (height - 1 - pixel_y) * width + pixel_x
|
||
image_data[index] = 255 # 自由空间
|
||
|
||
# 在周围添加一些障碍物用于测试（参照map_generator.html）
|
||
radius = 2 # 模拟障碍物半径
|
||
for dx in range(-radius, radius + 1):
|
||
for dy in range(-radius, radius + 1):
|
||
nx = pixel_x + dx
|
||
ny = pixel_y + dy
|
||
if 0 <= nx < width and 0 <= ny < height:
|
||
n_index = (height - 1 - ny) * width + nx
|
||
# 只在边界处添加障碍物
|
||
if dx == -radius or dx == radius or dy == -radius or dy == radius:
|
||
if np.random.random() < 0.1: # 10%的概率添加障碍物
|
||
image_data[n_index] = 0 # 障碍物
|
||
|
||
# 保存PGM数据
|
||
self.pgm_data = {
|
||
'width': width,
|
||
'height': height,
|
||
'data': image_data,
|
||
'resolution': resolution,
|
||
'origin_x': min_x,
|
||
'origin_y': min_y
|
||
}
|
||
|
||
# 统计像素值分布
|
||
unique, counts = np.unique(image_data, return_counts=True)
|
||
pixel_stats = dict(zip(unique, counts))
|
||
stats_str = ', '.join([f'{"自由空间" if k == 255 else "障碍物" if k == 0 else str(k)}:{v}' for k, v in pixel_stats.items()])
|
||
print(f"像素统计: {stats_str}")
|
||
|
||
print("PGM地图生成完成")
|
||
return True
|
||
|
||
def generate_huarui_stations(self):
|
||
"""生成华睿站点数据"""
|
||
if not self.smap_data or 'advancedPointList' not in self.smap_data:
|
||
print("SMAP数据中未找到advancedPointList")
|
||
return False
|
||
|
||
advanced_point_list = self.smap_data['advancedPointList']
|
||
if not advanced_point_list:
|
||
print("advancedPointList为空")
|
||
return False
|
||
|
||
if not self.csv_mapping:
|
||
print("CSV映射数据为空，请先加载CSV映射文件")
|
||
return False
|
||
|
||
print(f"开始生成华睿站点，仙工站点数量: {len(advanced_point_list)}")
|
||
|
||
# 类型映射
|
||
type_mapping = {
|
||
'ActionPoint': {'type': 1, 'prefix': '01', 'name': '动作站点'},
|
||
'LocationMark': {'type': 0, 'prefix': '02', 'name': '路径站点'},
|
||
'ParkPoint': {'type': 7, 'prefix': '03', 'name': '停靠站点'},
|
||
'ChargePoint': {'type': 6, 'prefix': '04', 'name': '充电站点'}
|
||
}
|
||
|
||
self.huarui_stations = []
|
||
self.station_mapping = []
|
||
|
||
for smap_station in advanced_point_list:
|
||
station_info = type_mapping.get(smap_station['className'], {'type': 0, 'prefix': '02', 'name': '普通站点'})
|
||
|
||
# 使用CSV映射中的ID，如果找不到则跳过该站点
|
||
smap_name = smap_station['instanceName']
|
||
if smap_name not in self.csv_mapping:
|
||
print(f"警告: 站点 {smap_name} 未在CSV映射文件中找到，跳过")
|
||
continue
|
||
|
||
station_id = self.csv_mapping[smap_name]
|
||
|
||
# 坐标转换（米转毫米）
|
||
x = round(smap_station['pos']['x'] * 1000)
|
||
y = round(smap_station['pos']['y'] * 1000)
|
||
|
||
# 创建华睿站点，使用CSV中的ID作为content和name
|
||
huarui_station = {
|
||
"content": station_id,
|
||
"coordinate": {"x": x, "y": y},
|
||
"name": station_id,
|
||
"type": station_info['type'],
|
||
"extraTypes": [],
|
||
"isTurn": 0,
|
||
"shelfModel": 1,
|
||
"evadeNode": 1,
|
||
"shelfIsTurn": 1,
|
||
"isHandoverArea": 0,
|
||
"isVirtualPoint": False,
|
||
"isTurnRotatableRange": [0, 360000],
|
||
"QRCodeAngle": 0,
|
||
"isNavigationMarkPoint": 0,
|
||
"contentEdges": [],
|
||
"isTextureInitPoint": True,
|
||
"navigationMarkPoint": -1,
|
||
"navTypeV3": ["QRCode", "Texture", "Laser", "Mileage"],
|
||
"isIgnorePayload": False,
|
||
"isIgnoreAGV": False,
|
||
"machineAngle": 0,
|
||
"slopeDistance": [1000, 1000, 1000, 1000],
|
||
"actionObstacleType": [],
|
||
"obstacleType": [1, 1, 1, 1, 2, 8] if station_info['type'] == 1 else [0, 0, 0, 0, 2, 8],
|
||
"slope": [0, 0, 0, 0],
|
||
"shelfModelAndAngles": [{
|
||
"model": 2 if station_info['type'] == 1 else 1,
|
||
"angle": [999000],
|
||
"forkToothRadio": "front",
|
||
"forkToothFrontToStackDistance": 0,
|
||
"forkToothBackToStackDistance": 999
|
||
}],
|
||
"movPrecExpand": True,
|
||
"areaType": 1,
|
||
"driftSet": {"x": 0, "y": 0},
|
||
"identifyType": [0],
|
||
"pickupRelease": 0,
|
||
"extraRotExpansion": True,
|
||
"palletAlignment": "center",
|
||
"palletAlignmentType": 0,
|
||
"custom": "",
|
||
"navSignType": "QRCode",
|
||
"highlightDetection": False,
|
||
"AvoidObstaclesDeceleration": [1000, 1000, 1000, 1000],
|
||
"AvoidObstaclesEmergencyStop": [50, 50, 50, 50],
|
||
"isNavSignPoint": False,
|
||
"shelfMode": 2 if station_info['type'] == 1 else 1,
|
||
"id": station_id,
|
||
"code": "1" if station_info['type'] == 1 else "0",
|
||
"desp": 0,
|
||
"shelfAngle": [999000],
|
||
"SafetyDistance": [[3000, 1000], [3000, 1000], [3000, 1000], [3000, 1000]],
|
||
"SafetyShape": [["Rectage"], ["Rectage"], ["Rectage"], ["Rectage"]],
|
||
"barrierSwitch": 1,
|
||
"edges": []
|
||
}
|
||
|
||
self.huarui_stations.append(huarui_station)
|
||
|
||
# 创建映射关系
|
||
mapping = {
|
||
'smap_name': smap_station['instanceName'],
|
||
'smap_type': smap_station['className'],
|
||
'smap_type_desc': station_info['name'],
|
||
'huarui_id': station_id,
|
||
'huarui_type': station_info['type'],
|
||
'coordinate': {'x': x, 'y': y}
|
||
}
|
||
self.station_mapping.append(mapping)
|
||
|
||
# 生成路径连接
|
||
self._generate_path_connections()
|
||
|
||
print(f"华睿站点生成完成，共{len(self.huarui_stations)}个站点")
|
||
return True
|
||
|
||
def _generate_path_connections(self):
|
||
"""生成站点之间的路径连接"""
|
||
if not self.smap_data or 'advancedCurveList' not in self.smap_data:
|
||
return
|
||
|
||
# 创建instanceName到stationId的映射
|
||
instance_to_station = {}
|
||
for i, mapping in enumerate(self.station_mapping):
|
||
instance_to_station[mapping['smap_name']] = mapping['huarui_id']
|
||
|
||
# 获取所有DegenerateBezier类型的路径
|
||
paths = [curve for curve in self.smap_data['advancedCurveList']
|
||
if curve.get('className') == 'DegenerateBezier']
|
||
|
||
print(f"处理路径连接，共{len(paths)}条路径")
|
||
|
||
# 为每个站点生成contentEdges
|
||
for station in self.huarui_stations:
|
||
station['contentEdges'] = []
|
||
|
||
for path in paths:
|
||
if not path.get('startPos') or not path.get('endPos'):
|
||
continue
|
||
|
||
start_name = path['startPos'].get('instanceName')
|
||
end_name = path['endPos'].get('instanceName')
|
||
|
||
if not start_name or not end_name:
|
||
continue
|
||
|
||
start_station_id = instance_to_station.get(start_name)
|
||
end_station_id = instance_to_station.get(end_name)
|
||
|
||
# 如果当前站点是路径的起点，添加到终点的连接
|
||
if start_station_id == station['name'] and end_station_id:
|
||
target_station = next((s for s in self.huarui_stations if s['name'] == end_station_id), None)
|
||
if target_station:
|
||
edge = {
|
||
"destination": target_station['content'],
|
||
"leftWidth": -1,
|
||
"rightWidth": -1,
|
||
"startExpandDistance": -1,
|
||
"endExpandDistance": -1,
|
||
"needFollow": 1,
|
||
"weight": 1,
|
||
"avoidScene": 1,
|
||
"aspect": "3",
|
||
"turningMode": 0,
|
||
"freePlan": 0,
|
||
"edgeTurnAble": 1,
|
||
"speed": 2000,
|
||
"tangentAngle": 999000,
|
||
"outTangentAngle": 999000,
|
||
"navMode": 0,
|
||
"trackId": 0,
|
||
"bypass": False,
|
||
"pointAccuracy": -1,
|
||
"angleAccuracy": -1000,
|
||
"orientObject": "",
|
||
"deviceTravelDirection": "unlimited",
|
||
"retrograde": False,
|
||
"ignoreSensor": [],
|
||
"shelfIsPass": 1,
|
||
"trafficAMR": [],
|
||
"trafficType": 0,
|
||
"trafficLoad": [],
|
||
"extraLockExpansion": True,
|
||
"headstock": "withLine",
|
||
"unloadReachMode": 0,
|
||
"loadReachMode": 0,
|
||
"useSingleLaserLocate": False,
|
||
"custom": "",
|
||
"loadPass": 0
|
||
}
|
||
|
||
# 添加DegenerateBezier的controlPoints支持
|
||
if path.get('controlPos1') and path.get('controlPos2'):
|
||
# 转换控制点坐标（米转毫米）
|
||
control_points = [
|
||
[
|
||
round(path['controlPos1']['x'] * 1000),
|
||
round(path['controlPos1']['y'] * 1000)
|
||
],
|
||
[
|
||
round(path['controlPos2']['x'] * 1000),
|
||
round(path['controlPos2']['y'] * 1000)
|
||
]
|
||
]
|
||
edge["controlPoints"] = control_points
|
||
|
||
station['contentEdges'].append(edge)
|
||
|
||
# 如果当前站点是路径的终点，添加到起点的连接（双向路径）
|
||
if end_station_id == station['name'] and start_station_id:
|
||
target_station = next((s for s in self.huarui_stations if s['name'] == start_station_id), None)
|
||
if target_station:
|
||
edge = {
|
||
"destination": target_station['content'],
|
||
"leftWidth": -1,
|
||
"rightWidth": -1,
|
||
"startExpandDistance": -1,
|
||
"endExpandDistance": -1,
|
||
"needFollow": 1,
|
||
"weight": 1,
|
||
"avoidScene": 1,
|
||
"aspect": "3",
|
||
"turningMode": 0,
|
||
"freePlan": 0,
|
||
"edgeTurnAble": 1,
|
||
"speed": 2000,
|
||
"tangentAngle": 999000,
|
||
"outTangentAngle": 999000,
|
||
"navMode": 0,
|
||
"trackId": 0,
|
||
"bypass": False,
|
||
"pointAccuracy": -1,
|
||
"angleAccuracy": -1000,
|
||
"orientObject": "",
|
||
"deviceTravelDirection": "unlimited",
|
||
"retrograde": False,
|
||
"ignoreSensor": [],
|
||
"shelfIsPass": 1,
|
||
"trafficAMR": [],
|
||
"trafficType": 0,
|
||
"trafficLoad": [],
|
||
"extraLockExpansion": True,
|
||
"headstock": "withLine",
|
||
"unloadReachMode": 0,
|
||
"loadReachMode": 0,
|
||
"useSingleLaserLocate": False,
|
||
"custom": "",
|
||
"loadPass": 0
|
||
}
|
||
|
||
# 添加DegenerateBezier的controlPoints支持（反向路径）
|
||
if path.get('controlPos1') and path.get('controlPos2'):
|
||
# 反向路径的控制点顺序相反
|
||
control_points = [
|
||
[
|
||
round(path['controlPos2']['x'] * 1000),
|
||
round(path['controlPos2']['y'] * 1000)
|
||
],
|
||
[
|
||
round(path['controlPos1']['x'] * 1000),
|
||
round(path['controlPos1']['y'] * 1000)
|
||
]
|
||
]
|
||
edge["controlPoints"] = control_points
|
||
|
||
station['contentEdges'].append(edge)
|
||
|
||
# 去重contentEdges
|
||
unique_edges = []
|
||
seen_destinations = set()
|
||
for edge in station['contentEdges']:
|
||
if edge['destination'] not in seen_destinations:
|
||
seen_destinations.add(edge['destination'])
|
||
unique_edges.append(edge)
|
||
station['contentEdges'] = unique_edges
|
||
|
||
# 生成edges字段
|
||
station['edges'] = []
|
||
for edge in station['contentEdges']:
|
||
station['edges'].append({
|
||
"destination": edge['destination'],
|
||
"weight": edge['weight']
|
||
})
|
||
|
||
def generate_topo_json(self):
|
||
"""生成topo.json文件"""
|
||
topo = {
|
||
"map": {
|
||
"name": self.map_name,
|
||
"type": "topo",
|
||
"width": self.map_width,
|
||
"height": self.map_height,
|
||
"xAttrMin": self.x_attr_min,
|
||
"yAttrMin": self.y_attr_min,
|
||
"initSpeed": 2,
|
||
"obstacleType": [0, 0, 0, 0, 7, 8],
|
||
"actionObstacleType": [],
|
||
"confidence": [],
|
||
"yawThreshold": [],
|
||
"forbiddenArea": [],
|
||
"operableArea": [],
|
||
"freePlan": 0,
|
||
"turningMode": -1,
|
||
"margin": 0,
|
||
"rcsImageName": "",
|
||
"relocationImageName": "",
|
||
"shelfModel": 0,
|
||
"version": "3.0",
|
||
"navType": "qrcode",
|
||
"devicePartition": None,
|
||
"saveDistance": 1000,
|
||
"brakeDistance": 1000,
|
||
"initSimple": "Rectage",
|
||
"timeStamp": int(datetime.now().timestamp() * 1000),
|
||
"times": 34,
|
||
"icsImageName": "",
|
||
"widthFor-1": 800,
|
||
"expandFor-1": 100,
|
||
"mp_widthFor-1": 800,
|
||
"mp_expandFor-1": 0,
|
||
"mutiple_pallet_generate": True
|
||
},
|
||
"nodes": self.huarui_stations,
|
||
"attrArea": [],
|
||
"relatingAreas": [],
|
||
"virtualPoint": [],
|
||
"guidelineConfigs": [
|
||
{
|
||
"lineId": 1,
|
||
"sensor": "camera",
|
||
"direction": 0,
|
||
"width": 200,
|
||
"line": [1000, 0],
|
||
"offset": 150
|
||
}
|
||
]
|
||
}
|
||
return json.dumps(topo, indent=2, ensure_ascii=False)
|
||
|
||
def generate_yaml_config(self):
|
||
"""生成YAML配置文件"""
|
||
if not self.pgm_data:
|
||
return ""
|
||
|
||
yaml_content = f"""image: {self.map_name}.pgm
|
||
resolution: {self.pgm_data['resolution']:.6f}
|
||
origin: [{self.pgm_data['origin_x']:.6f}, {self.pgm_data['origin_y']:.6f}, 0.000000]
|
||
negate: 0
|
||
occupied_thresh: 0.65
|
||
free_thresh: 0.196
|
||
width: {self.pgm_data['width']}
|
||
height: {self.pgm_data['height']}
|
||
intensity_threshold: 1000.000000
|
||
intensities:
|
||
"""
|
||
return yaml_content
|
||
|
||
def generate_pgm_file(self):
|
||
"""生成PGM文件内容"""
|
||
if not self.pgm_data:
|
||
return None
|
||
|
||
header = f"P5\n# Generated by SMAP to Huarui Converter\n{self.pgm_data['width']} {self.pgm_data['height']}\n255\n"
|
||
header_bytes = header.encode('ascii')
|
||
|
||
# 确保数据格式正确
|
||
image_array = np.array(self.pgm_data['data'], dtype=np.uint8)
|
||
|
||
pgm_content = header_bytes + image_array.tobytes()
|
||
return pgm_content
|
||
|
||
def generate_background_jpg(self):
|
||
"""生成background.jpg文件，参照map_generator.html的转换逻辑"""
|
||
if not self.pgm_data:
|
||
return None
|
||
|
||
# 获取PGM数据
|
||
width = self.pgm_data['width']
|
||
height = self.pgm_data['height']
|
||
pgm_array = np.array(self.pgm_data['data'], dtype=np.uint8)
|
||
|
||
# 创建RGB图像数据
|
||
rgb_data = np.zeros((height, width, 3), dtype=np.uint8)
|
||
|
||
# 按照map_generator.html的逻辑转换：
|
||
# 255 (自由空间) -> 白色 (255, 255, 255)
|
||
# 0 (障碍物) -> 黑色 (0, 0, 0)
|
||
for i in range(len(pgm_array)):
|
||
y = i // width
|
||
x = i % width
|
||
value = pgm_array[i]
|
||
|
||
if value == 255:
|
||
# 自由空间 -> 白色
|
||
rgb_data[y, x] = [255, 255, 255]
|
||
else:
|
||
# 障碍物 -> 黑色
|
||
rgb_data[y, x] = [0, 0, 0]
|
||
|
||
# 创建PIL图像
|
||
image = Image.fromarray(rgb_data, mode='RGB')
|
||
|
||
return image
|
||
|
||
def create_map_package(self, output_dir):
|
||
"""创建华睿地图包"""
|
||
if not os.path.exists(output_dir):
|
||
os.makedirs(output_dir)
|
||
|
||
print(f"开始创建华睿地图包到目录: {output_dir}")
|
||
|
||
# 创建ZIP文件
|
||
zip_path = os.path.join(output_dir, f"{self.map_name}.zip")
|
||
|
||
with zipfile.ZipFile(zip_path, 'w', zipfile.ZIP_DEFLATED) as zipf:
|
||
# 1. topo.json
|
||
topo_content = self.generate_topo_json()
|
||
zipf.writestr("topo.json", topo_content)
|
||
print("topo.json")
|
||
|
||
# 2. PGM文件
|
||
pgm_content = self.generate_pgm_file()
|
||
if pgm_content:
|
||
zipf.writestr(f"{self.map_name}.pgm", pgm_content)
|
||
print(f"{self.map_name}.pgm")
|
||
|
||
# 3. YAML配置文件
|
||
yaml_content = self.generate_yaml_config()
|
||
if yaml_content:
|
||
zipf.writestr(f"{self.map_name}.yaml", yaml_content)
|
||
print(f"{self.map_name}.yaml")
|
||
|
||
# 4. background.jpg
|
||
bg_image = self.generate_background_jpg()
|
||
if bg_image:
|
||
import io
|
||
img_buffer = io.BytesIO()
|
||
bg_image.save(img_buffer, format='JPEG', quality=95)
|
||
zipf.writestr("background.jpg", img_buffer.getvalue())
|
||
print("background.jpg")
|
||
|
||
# 5. dbVersion.txt
|
||
db_version = datetime.now().strftime('%Y%m%d%H%M')
|
||
zipf.writestr("dbVersion.txt", db_version)
|
||
print("dbVersion.txt")
|
||
|
||
print(f"华睿地图包创建完成: {zip_path}")
|
||
return zip_path
|
||
|
||
def convert(self, scene_path, smap_path, map_width, map_height, x_attr_min, y_attr_min, output_dir="output"):
|
||
"""执行完整的转换流程"""
|
||
print("开始SMAP到华睿地图包转换")
|
||
print("=" * 60)
|
||
|
||
# 1. 加载Scene文件
|
||
if not self.load_scene_file(scene_path):
|
||
return False
|
||
|
||
# 2. 提取Scene文件中的点位映射
|
||
if not self.extract_points_mapping():
|
||
return False
|
||
|
||
# 3. 加载SMAP文件
|
||
if not self.load_smap_file(smap_path):
|
||
return False
|
||
|
||
# 4. 使用smap文件名作为地图名称
|
||
smap_filename = os.path.splitext(os.path.basename(smap_path))[0]
|
||
self.set_map_parameters(smap_filename, map_width, map_height, x_attr_min, y_attr_min)
|
||
|
||
# 5. 生成PGM地图
|
||
if not self.generate_pgm_from_pointcloud():
|
||
return False
|
||
|
||
# 6. 生成华睿站点
|
||
if not self.generate_huarui_stations():
|
||
return False
|
||
|
||
# 7. 生成CSV映射表
|
||
if not os.path.exists(output_dir):
|
||
os.makedirs(output_dir)
|
||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||
csv_path = os.path.join(output_dir, f"{smap_filename}_PointsMapping_{timestamp}.csv")
|
||
self.generate_csv_report(csv_path)
|
||
|
||
# 8. 创建华睿地图包
|
||
map_package_path = self.create_map_package(output_dir)
|
||
|
||
print("=" * 60)
|
||
print("转换完成！")
|
||
print(f"华睿地图包: {map_package_path}")
|
||
print(f"站点映射表: {csv_path}")
|
||
|
||
return True
|
||
|
||
def main():
|
||
parser = argparse.ArgumentParser(description='仙工SMAP到华睿地图包转换器（集成Scene点位映射提取）')
|
||
parser.add_argument('scene_path', help='Scene文件路径')
|
||
parser.add_argument('smap_path', help='仙工SMAP文件路径')
|
||
parser.add_argument('map_width', type=int, help='地图宽度（mm）')
|
||
parser.add_argument('map_height', type=int, help='地图高度（mm）')
|
||
parser.add_argument('x_attr_min', type=int, help='X最小值（mm）')
|
||
parser.add_argument('y_attr_min', type=int, help='Y最小值（mm）')
|
||
|
||
args = parser.parse_args()
|
||
|
||
# 检查Scene文件是否存在
|
||
if not os.path.exists(args.scene_path):
|
||
print(f"Scene文件不存在: {args.scene_path}")
|
||
sys.exit(1)
|
||
|
||
# 检查SMAP文件是否存在
|
||
if not os.path.exists(args.smap_path):
|
||
print(f"SMAP文件不存在: {args.smap_path}")
|
||
sys.exit(1)
|
||
|
||
# 创建转换器并执行转换
|
||
converter = SmapToHuaruiConverter()
|
||
success = converter.convert(
|
||
args.scene_path,
|
||
args.smap_path,
|
||
args.map_width,
|
||
args.map_height,
|
||
args.x_attr_min,
|
||
args.y_attr_min,
|
||
args.output
|
||
)
|
||
|
||
if success:
|
||
print("\n✅ 转换成功完成！")
|
||
sys.exit(0)
|
||
else:
|
||
print("\n❌ 转换失败！")
|
||
sys.exit(1)
|
||
|
||
def convert_smap_to_iray(scene_path, smap_path, map_width, map_height, x_attr_min, y_attr_min, output_dir="iray_output"):
|
||
"""
|
||
API调用接口：将SMAP和Scene文件转换为华睿（IRAY）地图包
|
||
|
||
Args:
|
||
scene_path: Scene文件路径
|
||
smap_path: SMAP文件路径
|
||
map_width: 地图宽度（mm）
|
||
map_height: 地图高度（mm）
|
||
x_attr_min: X最小值（mm）
|
||
y_attr_min: Y最小值（mm）
|
||
output_dir: 输出目录，默认为 "iray_output"
|
||
|
||
Returns:
|
||
dict: 包含转换结果的字典
|
||
"""
|
||
try:
|
||
# 创建转换器实例
|
||
converter = SmapToHuaruiConverter()
|
||
|
||
# 执行转换
|
||
success = converter.convert(
|
||
scene_path,
|
||
smap_path,
|
||
map_width,
|
||
map_height,
|
||
x_attr_min,
|
||
y_attr_min,
|
||
output_dir
|
||
)
|
||
|
||
if success:
|
||
# 获取生成的文件信息
|
||
smap_filename = os.path.splitext(os.path.basename(smap_path))[0]
|
||
map_package_path = os.path.join(output_dir, f"{smap_filename}.zip")
|
||
|
||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||
csv_path = os.path.join(output_dir, f"{smap_filename}_PointsMapping_{timestamp}.csv")
|
||
|
||
return {
|
||
"success": True,
|
||
"message": "转换成功完成",
|
||
"map_package_path": map_package_path,
|
||
"csv_mapping_path": csv_path,
|
||
"station_count": len(converter.huarui_stations) if converter.huarui_stations else 0
|
||
}
|
||
else:
|
||
return {
|
||
"success": False,
|
||
"message": "转换失败",
|
||
"error": "转换过程中出现错误"
|
||
}
|
||
|
||
except Exception as e:
|
||
return {
|
||
"success": False,
|
||
"message": "转换失败",
|
||
"error": str(e)
|
||
}
|
||
|
||
if __name__ == "__main__":
|
||
main() |