1385 lines
77 KiB
Python
1385 lines
77 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
nav_sim2sim.py: 机器人高精度绝对惯导 2D 交互导航系统 & 实时遥测仪表盘。
|
||
|
||
特性：
|
||
- 动态 XML 地图解析引擎：自动读取并解析 scene_terrain.xml 中的所有 geom 障碍物，实现 100% 地图画面与物理环境精准对应。
|
||
- 极佳的可视化表现：支持旋转 box 顶点多边形解算、柱状 cylinder 绘制以及 hfield 区域半透明渲染，支持鼠标滚轮缩放与右键拖拽。
|
||
- 鼠标手绘轨迹巡航 (Path Drawing)：长按左键即可在地图上画出任意曲线，系统智能离散化为航点，控制机器人顺畅驶完轨迹。
|
||
- 纯坐标触发自适应限高爬行：不依赖高度传感器，依据全局 IMU 坐标范围自动下蹲过杆。
|
||
- 关键点记录与指令微调：支持 R 键打点、TXT 文件导出以及命令行（如 "go 3.5 -6.0"）直接发令控制。
|
||
- 全界面中文本地化。
|
||
"""
|
||
|
||
import os
|
||
import sys
|
||
import time
|
||
import math
|
||
import torch
|
||
import numpy as np
|
||
import pygame
|
||
import xml.etree.ElementTree as ET
|
||
from pathlib import Path
|
||
|
||
# 插入局部路径，确保模块导入顺畅
|
||
sys.path.append(str(Path(__file__).parent.absolute()))
|
||
|
||
from interface.mujoco_io import MuJoCoIO
|
||
from policy.policy_runner import PolicyRunner
|
||
import mujoco
|
||
import mujoco.viewer
|
||
|
||
# ==============================================================================
|
||
# HSL / RGB 科技感深色调配色系统
|
||
# ==============================================================================
|
||
COLOR_BG = (10, 15, 30) # Slate 950 深邃星空蓝
|
||
COLOR_GRID = (22, 29, 48) # 浅灰色网格线
|
||
COLOR_AXIS = (38, 50, 78) # 主轴线
|
||
COLOR_HUD_BG = (18, 24, 42) # 半透明磨砂控制面板背景
|
||
COLOR_HUD_BORDER = (38, 52, 84) # 边框线
|
||
COLOR_TEXT_LIGHT = (248, 250, 252) # 高对比度白色文字
|
||
COLOR_TEXT_MUTED = (148, 163, 184) # 灰色辅助说明文字
|
||
COLOR_EMERALD = (16, 185, 129) # 翡翠绿：自动模式 / 正常状态
|
||
COLOR_ROSE = (244, 63, 94) # 玫瑰红：手动模式 / 警告状态
|
||
COLOR_AMBER = (245, 158, 11) # 琥珀黄：高墙 / 爬坡提示
|
||
COLOR_GOLD = (234, 179, 8) # 亮金色：导航目标点
|
||
COLOR_CYAN = (6, 182, 212) # 霓虹青：机器人本体与行进轨迹
|
||
COLOR_PURPLE = (139, 92, 246) # 科技紫：关键点标记与 hfield 区域
|
||
COLOR_CONSOLE_BG = (7, 10, 19) # 控制台终端深黑色背景
|
||
|
||
# ==============================================================================
|
||
# 动态 XML 地图解析引擎
|
||
# ==============================================================================
|
||
def parse_xml_obstacles(xml_path):
|
||
"""
|
||
解析 scene_terrain.xml，动态提取所有的 geom 障碍物、平台、限高柱。
|
||
支持提取：pos, size, type, quat, rgba, name, 碰撞屏蔽(contype/conaffinity)。
|
||
"""
|
||
obstacles = []
|
||
hfields = {}
|
||
|
||
if not os.path.exists(xml_path):
|
||
print(f"[XML 解析器] 警告：未找到 XML 地图文件: {xml_path}")
|
||
return obstacles
|
||
|
||
try:
|
||
tree = ET.parse(xml_path)
|
||
root = tree.getroot()
|
||
|
||
# 1. 扫描并缓存所有 hfield 几何参数
|
||
for hf in root.iter('hfield'):
|
||
hf_name = hf.get('name')
|
||
hf_size_str = hf.get('size')
|
||
if hf_name and hf_size_str:
|
||
try:
|
||
hfields[hf_name] = [float(x) for x in hf_size_str.split()]
|
||
except ValueError:
|
||
continue
|
||
|
||
# 2. 解析所有的 geom 碰撞体
|
||
for geom in root.iter('geom'):
|
||
g_name = geom.get('name', 'geom')
|
||
# 过滤地面
|
||
if g_name == 'floor':
|
||
continue
|
||
|
||
g_type = geom.get('type')
|
||
pos_str = geom.get('pos')
|
||
size_str = geom.get('size')
|
||
quat_str = geom.get('quat', '1 0 0 0')
|
||
rgba_str = geom.get('rgba', '0.7 0.7 0.7 1')
|
||
contype = geom.get('contype', '1')
|
||
conaffinity = geom.get('conaffinity', '1')
|
||
hfield_ref = geom.get('hfield')
|
||
|
||
if not pos_str or not g_type:
|
||
continue
|
||
|
||
try:
|
||
pos = [float(x) for x in pos_str.split()]
|
||
quat = [float(x) for x in quat_str.split()]
|
||
rgba = [float(x) for x in rgba_str.split()]
|
||
|
||
# 如果是 hfield，则从缓存的 hfield 大小中提取尺寸
|
||
if g_type == 'hfield' and hfield_ref in hfields:
|
||
size = hfields[hfield_ref]
|
||
else:
|
||
size = [float(x) for x in size_str.split()] if size_str else []
|
||
except ValueError:
|
||
continue
|
||
|
||
# contype="0" 且 conaffinity="0" 表示仅渲染不发生碰撞的指示标
|
||
collidable = (contype != '0') and (conaffinity != '0')
|
||
|
||
obstacles.append({
|
||
'name': g_name,
|
||
'type': g_type,
|
||
'pos': pos,
|
||
'size': size,
|
||
'quat': quat,
|
||
'rgba': rgba,
|
||
'collidable': collidable
|
||
})
|
||
|
||
print(f"[XML 解析器] 成功加载地图：共解析出 {len(obstacles)} 个几何元素，已完成 100% 位置对应。")
|
||
except Exception as e:
|
||
print(f"[XML 解析器] 解析 XML 时发生错误: {e}")
|
||
|
||
return obstacles
|
||
|
||
|
||
# ==============================================================================
|
||
# 数学计算工具与姿态解算
|
||
# ==============================================================================
|
||
def quat_to_euler(quat_wxyz):
|
||
"""将 MuJoCo 的 wxyz 四元数转换为机身欧拉角 Roll, Pitch, Yaw (弧度)。"""
|
||
qw, qx, qy, qz = quat_wxyz
|
||
|
||
# 横滚角 Roll (x轴自转)
|
||
sinr_cosp = 2.0 * (qw * qx + qy * qz)
|
||
cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy)
|
||
roll = math.atan2(sinr_cosp, cosr_cosp)
|
||
|
||
# 俯仰角 Pitch (y轴摆动)
|
||
sinp = 2.0 * (qw * qy - qz * qx)
|
||
pitch = math.asin(np.clip(sinp, -1.0, 1.0))
|
||
|
||
# 偏航角 Yaw (z轴航向)
|
||
siny_cosp = 2.0 * (qw * qz + qx * qy)
|
||
cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
|
||
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||
|
||
return roll, pitch, yaw
|
||
|
||
|
||
# ==============================================================================
|
||
# 自动巡航控制器
|
||
# ==============================================================================
|
||
class NavController:
|
||
def __init__(self, io):
|
||
self.io = io
|
||
self.mode = "MANUAL" # "MANUAL" (手动键盘) 或 "AUTO" (自动导航)
|
||
|
||
# 导航目标点
|
||
self.target_x = None
|
||
self.target_y = None
|
||
self.target_z = 0.0
|
||
|
||
# 预设巡航任务状态
|
||
self.mission_name = "无"
|
||
self.waypoints = [] # list of (x, y, speed, policy)
|
||
self.waypoint_idx = 0
|
||
self.drawn_path = [] # 记录当前整条规划路径，用于 2D 高亮线段绘制
|
||
|
||
# 关键点记录表
|
||
self.recorded_keypoints = []
|
||
|
||
# APF 动态人工势场避障列表（将在主程序启动时根据 XML 解析结果动态填充）
|
||
self.obstacles = []
|
||
|
||
def start_mission(self, name, waypoints):
|
||
self.mode = "AUTO"
|
||
self.mission_name = name
|
||
self.waypoints = waypoints
|
||
self.waypoint_idx = 0
|
||
self.drawn_path = [(wp[0], wp[1]) for wp in waypoints]
|
||
if waypoints:
|
||
self.target_x, self.target_y = waypoints[0][0], waypoints[0][1]
|
||
print(f"[导航系统] 启动巡航任务：{name}。前往第 1 个航点: ({self.target_x:.2f}, {self.target_y:.2f})")
|
||
else:
|
||
self.target_x, self.target_y = None, None
|
||
|
||
def cancel_mission(self):
|
||
self.mode = "MANUAL"
|
||
self.mission_name = "无"
|
||
self.waypoints = []
|
||
self.waypoint_idx = 0
|
||
self.target_x, self.target_y = None, None
|
||
self.drawn_path = []
|
||
print("[导航系统] 自动巡航已取消，机器人切换回手动控制模式。")
|
||
|
||
def update_navigation(self, rx, ry, ryaw, runner, dt=0.02):
|
||
"""
|
||
核心控制计算逻辑：基于 PD 路径寻迹、自适应转向锁定与边界感知避障势场。
|
||
"""
|
||
if self.mode != "AUTO" or self.target_x is None or self.target_y is None:
|
||
return 0.0, 0.0, 0.0
|
||
|
||
# ----------------------------------------------------------------------
|
||
# 正常的 PD 路径跟随算法
|
||
# ----------------------------------------------------------------------
|
||
dx = self.target_x - rx
|
||
dy = self.target_y - ry
|
||
dist = math.sqrt(dx*dx + dy*dy)
|
||
|
||
# 判定抵达航点
|
||
if dist < 0.15:
|
||
if len(self.waypoints) > 0 and self.waypoint_idx < len(self.waypoints) - 1:
|
||
self.waypoint_idx += 1
|
||
next_wp = self.waypoints[self.waypoint_idx]
|
||
self.target_x, self.target_y = next_wp[0], next_wp[1]
|
||
print(f"[导航系统] 顺利抵达航点 WP {self.waypoint_idx}。下一个航点: ({self.target_x:.2f}, {self.target_y:.2f})")
|
||
return 0.0, 0.0, 0.0
|
||
else:
|
||
print("[导航系统] 自动巡航顺利走完！已平稳停留在终点。")
|
||
self.mode = "MANUAL"
|
||
self.mission_name = "无"
|
||
self.target_x, self.target_y = None, None
|
||
return 0.0, 0.0, 0.0
|
||
|
||
# 提取当前子航点的运行速度限制
|
||
speed_limit = 0.5
|
||
if self.waypoints and self.waypoint_idx < len(self.waypoints):
|
||
wp = self.waypoints[self.waypoint_idx]
|
||
if len(wp) >= 3:
|
||
speed_limit = wp[2]
|
||
|
||
# ----------------------------------------------------------------------
|
||
# 根据当前绝对地理坐标动态分配具体速度上限 (全局峰值限制为 1.2 m/s)
|
||
# ----------------------------------------------------------------------
|
||
dynamic_speed_limit = 0.6 # 默认平地过渡速度
|
||
|
||
# 1. 高越障墙冲越区 (拉满至峰值 1.2 m/s 以获取最大过墙动能)
|
||
if 1.0 <= rx <= 2.6 and -8.5 <= ry <= -5.0:
|
||
dynamic_speed_limit = 1.2
|
||
# 2. 限高低姿爬行区 (下蹲慢行，安全速度 0.3 m/s)
|
||
elif 4.8 <= rx <= 6.4 and -9.5 <= ry <= -8.5:
|
||
dynamic_speed_limit = 0.3
|
||
# 3. 蛇形绕障避险区 (精密控弯，平稳速度 0.4 m/s)
|
||
elif 0.5 <= rx <= 3.5 and -14.0 <= ry <= -9.5:
|
||
dynamic_speed_limit = 0.4
|
||
# 4. 台阶独木桥跨越区 (攀爬跨越，精细速度 0.4 m/s)
|
||
elif 1.0 <= rx <= 4.5 and -5.0 <= ry <= 0.0:
|
||
dynamic_speed_limit = 0.4
|
||
|
||
# 综合考虑航点限速、地理限速与 1.2 m/s 全局硬上限
|
||
speed_limit = min(speed_limit, dynamic_speed_limit, 1.2)
|
||
|
||
# ----------------------------------------------------------------------
|
||
# 直接路径寻迹 (已按需彻底移除人工势场避障功能)
|
||
# ----------------------------------------------------------------------
|
||
# 最终目标航向角 (直接朝着目标点行驶)
|
||
target_yaw = math.atan2(dy, dx)
|
||
|
||
# 航向偏差角度规范化至 [-pi, pi]
|
||
yaw_err = (target_yaw - ryaw + math.pi) % (2.0 * math.pi) - math.pi
|
||
|
||
# 自适应转向 PD 控制
|
||
Kp_yaw = 1.8
|
||
cmd_wz = Kp_yaw * yaw_err
|
||
cmd_wz = np.clip(cmd_wz, -1.0, 1.0)
|
||
|
||
# 转向优先自锁机制：当航向误差过大时（>45度），停止向前行进，全力原地自转对准
|
||
if abs(yaw_err) > 0.8:
|
||
cmd_vx = 0.0
|
||
else:
|
||
if "高墙" in self.mission_name or "charge" in self.mission_name.lower():
|
||
# 冲越复杂障碍物（如高越障墙）时，不随距离缩减速度，保持全速冲越！
|
||
cmd_vx = speed_limit
|
||
else:
|
||
Kp_dist = 0.8
|
||
cmd_vx = Kp_dist * dist * math.cos(yaw_err)
|
||
cmd_vx = np.clip(cmd_vx, -0.2, speed_limit)
|
||
|
||
return float(cmd_vx), 0.0, float(cmd_wz)
|
||
|
||
|
||
# ==============================================================================
|
||
# UI 组件：现代化圆角高光按钮
|
||
# ==============================================================================
|
||
class PygameButton:
|
||
def __init__(self, rect, text, bg_color, hover_color, text_color):
|
||
self.rect = pygame.Rect(rect)
|
||
self.text = text
|
||
self.bg_color = bg_color
|
||
self.hover_color = hover_color
|
||
self.text_color = text_color
|
||
|
||
def draw(self, surface, font):
|
||
mouse_pos = pygame.mouse.get_pos()
|
||
color = self.hover_color if self.rect.collidepoint(mouse_pos) else self.bg_color
|
||
pygame.draw.rect(surface, color, self.rect, border_radius=6)
|
||
pygame.draw.rect(surface, COLOR_HUD_BORDER, self.rect, width=1, border_radius=6)
|
||
|
||
text_surf = font.render(self.text, True, self.text_color)
|
||
text_rect = text_surf.get_rect(center=self.rect.center)
|
||
surface.blit(text_surf, text_rect)
|
||
|
||
def is_clicked(self, event):
|
||
if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
|
||
return self.rect.collidepoint(event.pos)
|
||
return False
|
||
|
||
|
||
# ==============================================================================
|
||
# 主循环与渲染主程序
|
||
# ==============================================================================
|
||
def main():
|
||
# 检测计算卡与渲染配置
|
||
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
|
||
print(f"[主程序] 正在使用计算设备: {device}")
|
||
|
||
# 路径解析
|
||
project_root = Path(__file__).parent.parent.absolute()
|
||
terrain_dir = Path(__file__).parent / "terrain"
|
||
terrain_xml = terrain_dir / "scene_terrain.xml"
|
||
robot_xml = project_root / "mjcf" / "wheelleg.xml"
|
||
rough_onnx = project_root / "model_6800.onnx"
|
||
crawl_onnx = project_root / "model_crawl.onnx"
|
||
policy_path = {
|
||
"rough": rough_onnx if rough_onnx.exists() else project_root / "model_rough.pt",
|
||
"crawl": crawl_onnx if crawl_onnx.exists() else project_root / "model_crawl.pt"
|
||
}
|
||
|
||
# 1. 解析 XML 地图障碍物，实现 100% 可视化精准对应
|
||
print("\n[主程序] 解析 scene_terrain.xml 实景物理元素...")
|
||
xml_obstacles = parse_xml_obstacles(terrain_xml)
|
||
|
||
# 2. 初始化 MuJoCo 物理接口与神经网络策略
|
||
print("\n[主程序] 正在编译 MuJoCo 物理环境模型...")
|
||
io = MuJoCoIO(terrain_xml, robot_xml, terrain_dir)
|
||
|
||
print("\n[主程序] 载入神经网络策略驱动模型...")
|
||
runner = PolicyRunner(policy_path, device)
|
||
|
||
# 初始化机器人姿态
|
||
io.reset_robot(runner.default_dof_pos)
|
||
runner.reset()
|
||
|
||
# 高频时序同步
|
||
control_dt = io.control_dt # 0.02s
|
||
sim_steps_per_control = int(round(control_dt / io.m.opt.timestep)) # 10
|
||
|
||
# 初始化自建巡航控制器
|
||
nav = NavController(io)
|
||
|
||
# 动态把从 XML 解析出来的碰撞体积 geom，自动注册装配到导航避障势场列表中（剔除翻越的高墙）
|
||
for obs in xml_obstacles:
|
||
# 1. 排除出发区
|
||
if obs['name'] == 'spawn_zone' or not obs['collidable']:
|
||
continue
|
||
# 2. 排除位于 Y = -7.0 处的 30cm 高越障墙，实现直接加速跨越冲锋
|
||
if abs(obs['pos'][1] - (-7.0)) < 0.1:
|
||
continue
|
||
|
||
# 根据几何形状计算其近似避障边界外切半径 o_rad
|
||
cx, cy = obs['pos'][0], obs['pos'][1]
|
||
if obs['type'] == 'cylinder':
|
||
o_rad = obs['size'][0]
|
||
elif obs['type'] == 'box':
|
||
o_rad = math.sqrt(obs['size'][0]**2 + obs['size'][1]**2)
|
||
else:
|
||
o_rad = 0.3 # 默认缺省值
|
||
|
||
nav.obstacles.append((cx, cy, o_rad, obs['name']))
|
||
|
||
print(f"[避障系统] 已成功动态注册 {len(nav.obstacles)} 个避障势场障碍体。高越障墙已做冲越通行处理。")
|
||
|
||
# ==========================================================================
|
||
# Pygame 图形化仪表盘窗口设置
|
||
# ==========================================================================
|
||
pygame.init()
|
||
pygame.font.init()
|
||
|
||
WINDOW_WIDTH, WINDOW_HEIGHT = 1024, 768
|
||
MAP_WIDTH = 680 # 稍微缩减地图面幅，给右侧 HUD 面板腾出充足排版空间
|
||
screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
|
||
pygame.display.set_caption("Robot High-Precision Absolute Navigation & Real-time Telemetry Dashboard")
|
||
|
||
# 中文兼容与高解析度字体加载系统，直接加载 Windows 系统字体文件，杜绝方框（Tofu 乱码）现象
|
||
def get_chinese_font(size, bold=False):
|
||
# 优先读取系统物理路径下的字体文件，彻底避开 pygame.font.SysFont Silently-Fail Bug
|
||
font_paths = [
|
||
"C:/Windows/Fonts/msyhbd.ttc" if bold else "C:/Windows/Fonts/msyh.ttc",
|
||
"C:/Windows/Fonts/msyh.ttc",
|
||
"C:/Windows/Fonts/simhei.ttf",
|
||
"C:/Windows/Fonts/simsun.ttc",
|
||
]
|
||
for path in font_paths:
|
||
if os.path.exists(path):
|
||
try:
|
||
return pygame.font.Font(path, size)
|
||
except Exception:
|
||
continue
|
||
|
||
# 备用 SysFont 尝试
|
||
for font_name in ["microsoftyahei", "微软雅黑", "simhei", "黑体", "simsun", "宋体"]:
|
||
try:
|
||
return pygame.font.SysFont(font_name, size, bold=bold)
|
||
except Exception:
|
||
continue
|
||
|
||
# 最终安全降级
|
||
return pygame.font.Font(None, size)
|
||
|
||
font_large = get_chinese_font(22, bold=True)
|
||
font_medium = get_chinese_font(16, bold=True)
|
||
font_small = get_chinese_font(13, bold=False)
|
||
font_mono = get_chinese_font(13, bold=False)
|
||
|
||
# 视口状态量
|
||
zoom = 30.0 # 物理单位到屏幕像素倍数 (像素/米)
|
||
pan_x = 0.0 # pan平移偏移量
|
||
pan_y = 0.0
|
||
is_dragging = False
|
||
drag_start_x = 0
|
||
drag_start_y = 0
|
||
auto_center = True
|
||
sim_speed_factor = 1.0 # 仿真物理流速倍率
|
||
|
||
# 轨迹面包屑历史缓存
|
||
robot_trail = []
|
||
max_trail_len = 250
|
||
|
||
# 手绘轨迹变量
|
||
draw_mode = False
|
||
is_drawing_path = False
|
||
drawn_points = []
|
||
|
||
# HUD 侧边交互按钮排版（垂直布局与位置微调，彻底消除重叠，完美对齐分界线）
|
||
# Column 1 X: 700, Column 2 X: 860. 宽度 140 像素
|
||
btn_slalom = PygameButton((700, 307, 140, 26), "S形绕杆", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_crawl = PygameButton((860, 307, 140, 26), "限高下蹲", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_gravel = PygameButton((700, 338, 140, 26), "砂砾碎石", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_wall = PygameButton((860, 338, 140, 26), "高墙越障", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_stairs = PygameButton((700, 369, 140, 26), "台阶攀爬", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_bridge = PygameButton((860, 369, 140, 26), "斜坡木桥", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
|
||
# 终极任务：大满贯 (科技紫圆角大按钮)
|
||
btn_grand = PygameButton((700, 400, 300, 26), "障碍赛大满贯 (总任务)", (139, 92, 246), (124, 58, 237), COLOR_TEXT_LIGHT)
|
||
|
||
# 系统与视图控制按钮
|
||
btn_clear_trg = PygameButton((700, 467, 140, 26), "清除与停止", (244, 63, 94, 100), (225, 29, 72), COLOR_TEXT_LIGHT)
|
||
btn_center_toggle = PygameButton((860, 467, 140, 26), "视角居中:开", (16, 185, 129), (5, 150, 105), COLOR_TEXT_LIGHT)
|
||
btn_draw_mode = PygameButton((700, 498, 140, 26), "手绘模式:关", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_speed_down = PygameButton((860, 498, 44, 26), "倍速-", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_speed_normal = PygameButton((908, 498, 44, 26), "标准", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_speed_up = PygameButton((956, 498, 44, 26), "倍速+", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
|
||
# 坐标直接微调控制发令键
|
||
btn_x_plus = PygameButton((700, 565, 65, 24), "X+", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_x_minus = PygameButton((775, 565, 65, 24), "X-", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_y_plus = PygameButton((860, 565, 65, 24), "Y+", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_y_minus = PygameButton((935, 565, 65, 24), "Y-", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
|
||
# 关键点记录按键
|
||
btn_rec_key = PygameButton((700, 628, 95, 24), "保存位置", (139, 92, 246), (124, 58, 237), COLOR_TEXT_LIGHT)
|
||
btn_exp_key = PygameButton((805, 628, 95, 24), "导出文件", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
btn_clear_key = PygameButton((910, 628, 90, 24), "清空位置", (30, 41, 59), (51, 65, 85), COLOR_TEXT_LIGHT)
|
||
|
||
# 动态指令命令行输入窗口
|
||
console_active = False
|
||
console_text = ""
|
||
console_rect = pygame.Rect(700, 708, 300, 22)
|
||
|
||
# 绝对坐标变换函数闭包
|
||
def world_to_screen(wx, wy):
|
||
sx = int(MAP_WIDTH / 2.0 + wx * zoom + pan_x)
|
||
sy = int(WINDOW_HEIGHT / 2.0 - wy * zoom + pan_y)
|
||
return sx, sy
|
||
|
||
def screen_to_world(sx, sy):
|
||
wx = (sx - MAP_WIDTH / 2.0 - pan_x) / zoom
|
||
wy = -(sy - WINDOW_HEIGHT / 2.0 - pan_y) / zoom
|
||
return wx, wy
|
||
|
||
# 锁频同步计数
|
||
next_exec_time = time.perf_counter()
|
||
clock = pygame.time.Clock()
|
||
viewer_counter = 0
|
||
|
||
print(f"\n[主程序] 高频控制与 2D 可视化仪表盘已启动 (采样步频: {control_dt:.3f}s)")
|
||
|
||
# 拉起 MuJoCo 被动渲染视口
|
||
with mujoco.viewer.launch_passive(io.m, io.d) as viewer:
|
||
viewer.cam.distance = 5.0
|
||
viewer.cam.elevation = -20.0
|
||
viewer.cam.azimuth = 45.0
|
||
|
||
running = True
|
||
while running and viewer.is_running():
|
||
step_start_time = time.perf_counter()
|
||
|
||
# ------------------------------------------------------------------
|
||
# 读取高精度绝对全局惯导系统 (IMU simulated readings)
|
||
# ------------------------------------------------------------------
|
||
rx = io.d.qpos[0]
|
||
ry = io.d.qpos[1]
|
||
rz = io.d.qpos[2]
|
||
|
||
# 读取四元数
|
||
quat = io.d.qpos[3:7].copy()
|
||
roll_rad, pitch_rad, yaw_rad = quat_to_euler(quat)
|
||
roll_deg = math.degrees(roll_rad)
|
||
pitch_deg = math.degrees(pitch_rad)
|
||
yaw_deg = math.degrees(yaw_rad)
|
||
|
||
# 读取线速度与角速度
|
||
vx_world = io.d.qvel[0]
|
||
vy_world = io.d.qvel[1]
|
||
linear_speed = math.sqrt(vx_world*vx_world + vy_world*vy_world)
|
||
wz_rate = io.d.qvel[5]
|
||
|
||
# 网格地图行迹更新
|
||
if not robot_trail or math.dist((rx, ry), robot_trail[-1]) > 0.05:
|
||
robot_trail.append((rx, ry))
|
||
if len(robot_trail) > max_trail_len:
|
||
robot_trail.pop(0)
|
||
|
||
# 居中锁定机身
|
||
if auto_center:
|
||
pan_x = -rx * zoom
|
||
pan_y = ry * zoom
|
||
|
||
# ------------------------------------------------------------------
|
||
# 统一自适应高度与神经网络控制策略切换决策系统
|
||
# ------------------------------------------------------------------
|
||
# 默认维持常规站立策略
|
||
desired_policy = "rough"
|
||
|
||
# 1. 自动导航模式下，且当前航点的期望策略为 crawl (触发限高下蹲)
|
||
if nav.mode == "AUTO" and nav.waypoints and nav.waypoint_idx < len(nav.waypoints):
|
||
wp = nav.waypoints[nav.waypoint_idx]
|
||
if len(wp) >= 4 and wp[3] == "crawl":
|
||
# 计算指向目标的方位角与偏角偏差，确保在对正后立即切换下蹲
|
||
dx = nav.target_x - rx
|
||
dy = nav.target_y - ry
|
||
target_yaw = math.atan2(dy, dx)
|
||
yaw_err = (target_yaw - yaw_rad + math.pi) % (2.0 * math.pi) - math.pi
|
||
|
||
# 只要车头对齐（偏角小于 10 度 / 0.18 弧度），立即在行进中提前下蹲准备过杆
|
||
if abs(yaw_err) < 0.18:
|
||
desired_policy = "crawl"
|
||
|
||
# 2. 绝对物理位置安全兜底：踏入限高栏区间 (Y in [-9.5, -8.5] 且 X in [4.8, 6.4]) 强制下蹲
|
||
if 4.8 <= rx <= 6.4 and -9.5 <= ry <= -8.5:
|
||
desired_policy = "crawl"
|
||
|
||
# 执行高度状态的敏捷切换
|
||
if runner.current_policy_name != desired_policy and not runner.transition_in_progress:
|
||
runner.trigger_transition(desired_policy)
|
||
|
||
# ------------------------------------------------------------------
|
||
# Pygame 键鼠事件与拖拽交互响应
|
||
# ------------------------------------------------------------------
|
||
for event in pygame.event.get():
|
||
if event.type == pygame.QUIT:
|
||
running = False
|
||
|
||
# 滚轮缩放事件
|
||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||
if event.button == 4: # 向上滚：放大
|
||
zoom = min(120.0, zoom * 1.1)
|
||
elif event.button == 5: # 向下滚：缩小
|
||
zoom = max(8.0, zoom / 1.1)
|
||
|
||
# 鼠标左键点击地图设置目标点
|
||
elif event.button == 1:
|
||
if event.pos[0] < MAP_WIDTH:
|
||
wx, wy = screen_to_world(event.pos[0], event.pos[1])
|
||
if draw_mode:
|
||
is_drawing_path = True
|
||
drawn_points = [(wx, wy)]
|
||
nav.cancel_mission() # 清除以往规划
|
||
else:
|
||
# 正常的单点巡航目标发令
|
||
nav.mode = "AUTO"
|
||
nav.target_x = wx
|
||
nav.target_y = wy
|
||
nav.target_z = rz
|
||
nav.drawn_path = [(wx, wy)] # 生成一段直线指示
|
||
print(f"[导航系统] 用户手动指派新目标坐标: ({wx:.2f}, {wy:.2f})")
|
||
console_active = False
|
||
elif console_rect.collidepoint(event.pos):
|
||
console_active = True
|
||
else:
|
||
console_active = False
|
||
|
||
# 鼠标右键拖拽地图平移
|
||
elif event.button == 3:
|
||
if event.pos[0] < MAP_WIDTH:
|
||
is_dragging = True
|
||
auto_center = False
|
||
drag_start_x, drag_start_y = event.pos
|
||
|
||
elif event.type == pygame.MOUSEBUTTONUP:
|
||
if event.button == 3:
|
||
is_dragging = False
|
||
elif event.button == 1:
|
||
if is_drawing_path:
|
||
is_drawing_path = False
|
||
# 用户拖拽完鼠标，将所有坐标降采样过滤转化为离散巡航航点
|
||
if len(drawn_points) > 1:
|
||
wps = []
|
||
for pt in drawn_points:
|
||
if not wps or math.dist(pt, (wps[-1][0], wps[-1][1])) >= 0.25:
|
||
wps.append((pt[0], pt[1], 0.5, "rough"))
|
||
if wps:
|
||
nav.start_mission("手绘自定义轨迹", wps)
|
||
print(f"[导航系统] 成功加载手绘轨迹，已装配成 {len(wps)} 个精密航点。")
|
||
else:
|
||
nav.cancel_mission()
|
||
|
||
elif event.type == pygame.MOUSEMOTION:
|
||
if is_dragging:
|
||
dx_pix = event.pos[0] - drag_start_x
|
||
dy_pix = event.pos[1] - drag_start_y
|
||
pan_x += dx_pix
|
||
pan_y += dy_pix
|
||
drag_start_x, drag_start_y = event.pos
|
||
elif is_drawing_path:
|
||
if event.pos[0] < MAP_WIDTH:
|
||
wx, wy = screen_to_world(event.pos[0], event.pos[1])
|
||
# 间距大于 0.15m 时记录画线点，保证线条采样平滑
|
||
if not drawn_points or math.dist((wx, wy), drawn_points[-1]) > 0.15:
|
||
drawn_points.append((wx, wy))
|
||
|
||
# 键盘输入事件
|
||
elif event.type == pygame.KEYDOWN:
|
||
if console_active:
|
||
if event.key == pygame.K_RETURN:
|
||
cmd = console_text.strip().lower()
|
||
print(f"[终端控制] 捕获命令行输入: {cmd}")
|
||
if cmd.startswith("go "):
|
||
try:
|
||
parts = cmd.split()
|
||
cx = float(parts[1])
|
||
cy = float(parts[2])
|
||
nav.mode = "AUTO"
|
||
nav.target_x = cx
|
||
nav.target_y = cy
|
||
print(f"[终端控制] 执行导航，目的地坐标: ({cx}, {cy})")
|
||
except:
|
||
print("[终端控制] 指令语法错误！例：go <X坐标> <Y坐标>")
|
||
elif cmd == "clear" or cmd == "c":
|
||
nav.cancel_mission()
|
||
elif cmd == "record" or cmd == "r":
|
||
key_id = len(nav.recorded_keypoints) + 1
|
||
nav.recorded_keypoints.append({
|
||
'id': key_id, 'x': rx, 'y': ry, 'z': rz, 'yaw': yaw_deg
|
||
})
|
||
print(f"[关键点] 已记录 WP{key_id}：({rx:.2f}, {ry:.2f})")
|
||
elif cmd == "crawl":
|
||
runner.trigger_transition("crawl")
|
||
elif cmd == "rough":
|
||
runner.trigger_transition("rough")
|
||
elif cmd == "slalom":
|
||
# 包含三处必经红色点并且留出宽裕避让余量的精准S形绕杆路径
|
||
slalom_wps = [
|
||
(2.9, -9.5, 0.4, "rough"), # 1. 安全过渡点，引导机器人向西南切入
|
||
(1.8, -9.95, 0.4, "rough"), # 2. 触发红点 1，与 Pole 1 保持 0.55m 超大安全间距
|
||
(0.9, -10.0, 0.4, "rough"), # 3. 绕过 Pole 1 左侧的保护航点，拉开间距
|
||
(0.8, -10.8, 0.4, "rough"), # 4. 左侧平滑过渡点，为对准 Pole 1-2 缝隙做准备
|
||
(0.9, -11.0, 0.4, "rough"), # 5. 缝隙入口前置对齐点
|
||
(2.7, -11.0, 0.4, "rough"), # 6. 横穿 Pole 1-2 缝隙，与上下两杆均保持 0.50m 绝对中心间距
|
||
(2.8, -11.8, 0.4, "rough"), # 7. 右侧平滑过渡点，为对准 Pole 2-3 缝隙做准备
|
||
(2.7, -12.0, 0.4, "rough"), # 8. 缝隙入口前置对齐点
|
||
(0.9, -12.0, 0.4, "rough"), # 9. 横穿 Pole 2-3 缝隙，与上下两杆均保持 0.50m 绝对中心间距
|
||
(0.9, -12.75, 0.4, "rough"), # 10. 左侧平滑过渡点，为侧向触发红点 3 做准备
|
||
(1.42, -12.82, 0.4, "rough"), # 11. 触发红点 3，与 Pole 3 保持 0.50m 超大安全间距
|
||
(2.3, -13.3, 0.4, "rough"), # 12. 向下切入 Pole 3-4 缝隙的正下方对齐点
|
||
(2.3, -11.5, 0.4, "rough"), # 13. 笔直向北穿过 Pole 3-4 缝隙，与两杆各留 0.50m 最大对称间距
|
||
(3.32, -12.44, 0.4, "rough"), # 14. 触发红点 2，与 Pole 4 保持 0.52m 超大安全间距
|
||
(3.7, -9.0, 0.6, "rough") # 15. 安全返回起点
|
||
]
|
||
nav.start_mission("S形绕行任务", slalom_wps)
|
||
elif cmd == "charge":
|
||
# 包含前置对位与直线冲刺的高墙全速越障路径
|
||
charge_wps = [
|
||
(3.3, -9.0, 0.5, "rough"), # 1. 移出起步区，向西对准 X = 3.3 轴线中途过渡
|
||
(1.8, -9.0, 0.5, "rough"), # 2. 轴线对齐入口，将 X 轴拉正到 1.8
|
||
(1.8, -8.0, 0.4, "rough"), # 3. 垂直逼近高墙第一步
|
||
(1.8, -7.5, 0.4, "rough"), # 4. 垂直逼近高墙第二步（距墙仅 0.5m，车身彻底正对）
|
||
(1.8, -5.8, 1.2, "rough"), # 5. 全速冲锋！越过高墙并在 Y = -5.8 处的安全平地上平稳落地（绝不碰台阶）
|
||
(3.3, -5.8, 0.5, "rough"), # 6. 安全东移折回，对齐到 X = 3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 7. 沿 X = 3.3 安全线直行南下，完全避开高墙
|
||
(3.7, -9.0, 0.6, "rough") # 8. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("高墙加速冲锋", charge_wps)
|
||
elif cmd == "bridge":
|
||
bridge_wps = [
|
||
(3.3, -9.0, 0.5, "rough"), # 1. 移出起步区，向西对准 X = 3.3 避开高墙通道
|
||
(3.3, -1.6, 0.5, "rough"), # 2. 沿 X = 3.3 笔直向北，完全避开所有高墙与台阶结构
|
||
(1.8, -1.6, 0.5, "rough"), # 3. 水平移入木桥 A 斜坡起点的平地
|
||
(1.8, -1.38, 0.4, "rough"), # 4. 对齐木桥 A 坡底起点
|
||
(1.8, 0.0, 0.4, "rough"), # 5. 稳定爬上木桥 A 斜坡登上平台
|
||
(3.2, 0.0, 0.4, "rough"), # 6. 沿独木桥 Y=0 轴线精细循迹
|
||
(4.85, 0.0, 0.4, "rough"), # 7. 沿独木桥 Y=0 轴线继续前进
|
||
(5.7, -0.5, 0.4, "rough"), # 8. 驶入东侧木桥转弯缓冲段
|
||
(5.7, -2.25, 0.4, "rough"), # 9. 驶上中间木桥平台
|
||
(5.7, -3.5, 0.4, "rough"), # 10. 到达木桥 B 斜坡起爬平整区
|
||
(5.7, -5.0, 0.4, "rough"), # 11. 稳健行进下斜坡 B
|
||
(5.7, -7.0, 0.4, "rough"), # 12. 平稳落地脱离木桥 B，在平地上放平车身
|
||
(3.3, -7.0, 0.5, "rough"), # 13. 横向西移，重新汇入 X=3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 14. 沿 X=3.3 安全走廊直行南下
|
||
(3.7, -9.0, 0.6, "rough") # 15. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("斜坡木桥任务", bridge_wps)
|
||
elif cmd == "gravel":
|
||
gravel_wps = [
|
||
(3.7, -12.5, 0.5, "rough"), # 1. 沿 X=3.7 轴线一路笔直向南，在 flat 地面上完全避开限高门柱
|
||
(4.84, -12.5, 0.4, "rough"), # 2. 横向东进，从西侧入口完美切入 1号砂砾平台 tip
|
||
(5.84, -12.0, 0.4, "rough"), # 3. 横跨转移至 2号砂砾平台
|
||
(5.84, -10.2, 0.4, "rough"), # 4. 笔直向北走下平台，平稳落于北侧平地 tip
|
||
(3.3, -10.2, 0.5, "rough"), # 5. 西向横移，重新汇入 X=3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 6. 沿着 X=3.3 直行北上
|
||
(3.7, -9.0, 0.6, "rough") # 7. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("砂砾碎石任务", gravel_wps)
|
||
elif cmd == "grand":
|
||
grand_wps = [
|
||
# 1. S形绕杆
|
||
(2.9, -9.5, 0.4, "rough"),
|
||
(1.8, -9.95, 0.4, "rough"),
|
||
(0.9, -10.0, 0.4, "rough"),
|
||
(0.8, -10.8, 0.4, "rough"),
|
||
(0.9, -11.0, 0.4, "rough"),
|
||
(2.7, -11.0, 0.4, "rough"),
|
||
(2.8, -11.8, 0.4, "rough"),
|
||
(2.7, -12.0, 0.4, "rough"),
|
||
(0.9, -12.0, 0.4, "rough"),
|
||
(0.9, -12.75, 0.4, "rough"),
|
||
(1.42, -12.82, 0.4, "rough"),
|
||
(2.3, -13.3, 0.4, "rough"),
|
||
(2.3, -11.5, 0.4, "rough"),
|
||
(3.32, -12.44, 0.4, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 2. 限高下蹲
|
||
(5.7, -10.2, 0.5, "rough"),
|
||
(5.7, -9.0, 0.3, "crawl"),
|
||
(5.7, -7.8, 0.3, "rough"),
|
||
(5.7, -7.0, 0.5, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 3. 砂砾碎石
|
||
(3.7, -12.5, 0.5, "rough"),
|
||
(4.84, -12.5, 0.4, "rough"),
|
||
(5.84, -12.0, 0.4, "rough"),
|
||
(5.84, -10.2, 0.4, "rough"),
|
||
(3.3, -10.2, 0.5, "rough"),
|
||
(3.3, -9.0, 0.5, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 4. 高墙越障
|
||
(3.3, -9.0, 0.5, "rough"),
|
||
(1.8, -9.0, 0.5, "rough"),
|
||
(1.8, -8.0, 0.4, "rough"),
|
||
(1.8, -7.5, 0.4, "rough"),
|
||
(1.8, -5.8, 1.2, "rough"),
|
||
# 5. 台阶攀爬
|
||
(1.8, -3.5, 0.4, "rough"),
|
||
(1.8, -1.6, 0.4, "rough"),
|
||
# 6. 斜坡木桥
|
||
(1.8, -1.38, 0.4, "rough"),
|
||
(1.8, 0.0, 0.4, "rough"),
|
||
(3.2, 0.0, 0.4, "rough"),
|
||
(4.85, 0.0, 0.4, "rough"),
|
||
(5.7, -0.5, 0.4, "rough"),
|
||
(5.7, -2.25, 0.4, "rough"),
|
||
(5.7, -3.5, 0.4, "rough"),
|
||
(5.7, -5.0, 0.4, "rough"),
|
||
(5.7, -7.0, 0.4, "rough"),
|
||
(3.3, -7.0, 0.5, "rough"), # 横向西移，重新汇入 X=3.3 安全通道
|
||
(3.3, -9.0, 0.5, "rough"), # 沿 X=3.3 通道南下返回
|
||
(3.7, -9.0, 0.6, "rough") # 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("障碍赛大满贯", grand_wps)
|
||
console_text = ""
|
||
elif event.key == pygame.K_BACKSPACE:
|
||
console_text = console_text[:-1]
|
||
else:
|
||
console_text += event.unicode
|
||
else:
|
||
# 正常打点快捷键
|
||
if event.key == pygame.K_r:
|
||
key_id = len(nav.recorded_keypoints) + 1
|
||
nav.recorded_keypoints.append({
|
||
'id': key_id, 'x': rx, 'y': ry, 'z': rz, 'yaw': yaw_deg
|
||
})
|
||
print(f"[关键点] 快捷键记录 WP{key_id}：({rx:.2f}, {ry:.2f})")
|
||
elif event.key == pygame.K_SPACE:
|
||
# 紧急刹车 / 停止
|
||
nav.cancel_mission()
|
||
|
||
# 动态交互按钮响应 (防止与终端输入冲突)
|
||
if not console_active:
|
||
if btn_slalom.is_clicked(event):
|
||
# 包含三处必经红色点并且留出宽裕避让余量的精准S形绕杆路径
|
||
slalom_wps = [
|
||
(2.9, -9.5, 0.4, "rough"), # 1. 安全过渡点，引导机器人向西南切入
|
||
(1.8, -9.95, 0.4, "rough"), # 2. 触发红点 1，与 Pole 1 保持 0.55m 超大安全间距
|
||
(0.9, -10.0, 0.4, "rough"), # 3. 绕过 Pole 1 左侧的保护航点，拉开间距
|
||
(0.8, -10.8, 0.4, "rough"), # 4. 左侧平滑过渡点，为对准 Pole 1-2 缝隙做准备
|
||
(0.9, -11.0, 0.4, "rough"), # 5. 缝隙入口前置对齐点
|
||
(2.7, -11.0, 0.4, "rough"), # 6. 横穿 Pole 1-2 缝隙，与上下两杆均保持 0.50m 绝对中心间距
|
||
(2.8, -11.8, 0.4, "rough"), # 7. 右侧平滑过渡点，为对准 Pole 2-3 缝隙做准备
|
||
(2.7, -12.0, 0.4, "rough"), # 8. 缝隙入口前置对齐点
|
||
(0.9, -12.0, 0.4, "rough"), # 9. 横穿 Pole 2-3 缝隙，与上下两杆均保持 0.50m 绝对中心间距
|
||
(0.9, -12.75, 0.4, "rough"), # 10. 左侧平滑过渡点，为侧向触发红点 3 做准备
|
||
(1.42, -12.82, 0.4, "rough"), # 11. 触发红点 3，与 Pole 3 保持 0.50m 超大安全间距
|
||
(2.3, -13.3, 0.4, "rough"), # 12. 向下切入 Pole 3-4 缝隙的正下方对齐点
|
||
(2.3, -11.5, 0.4, "rough"), # 13. 笔直向北穿过 Pole 3-4 缝隙，与两杆各留 0.50m 最大对称间距
|
||
(3.32, -12.44, 0.4, "rough"), # 14. 触发红点 2，与 Pole 4 保持 0.52m 超大安全间距
|
||
(3.7, -9.0, 0.6, "rough") # 15. 安全返回起点
|
||
]
|
||
nav.start_mission("S形绕行演练", slalom_wps)
|
||
elif btn_wall.is_clicked(event):
|
||
# 包含前置对位与直线冲刺的高墙全速越障路径
|
||
charge_wps = [
|
||
(3.3, -9.0, 0.5, "rough"), # 1. 移出起步区，向西对准 X = 3.3 轴线中途过渡
|
||
(1.8, -9.0, 0.5, "rough"), # 2. 轴线对齐入口，将 X 轴拉正到 1.8
|
||
(1.8, -8.0, 0.4, "rough"), # 3. 垂直逼近高墙第一步
|
||
(1.8, -7.5, 0.4, "rough"), # 4. 垂直逼近高墙第二步（距墙仅 0.5m，车身彻底正对）
|
||
(1.8, -5.8, 1.2, "rough"), # 5. 全速冲锋！越过高墙并在 Y = -5.8 处的安全平地上平稳落地（绝不碰台阶）
|
||
(3.3, -5.8, 0.5, "rough"), # 6. 安全东移折回，对齐到 X = 3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 7. 沿 X = 3.3 安全线直行南下，完全避开高墙
|
||
(3.7, -9.0, 0.6, "rough") # 8. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("高墙加速越障", charge_wps)
|
||
elif btn_crawl.is_clicked(event):
|
||
# 正确的 Y 轴方向对准跨越限高门
|
||
crawl_wps = [
|
||
(5.7, -10.2, 0.5, "rough"), # 首先对准限高门中心轴线前方
|
||
(5.7, -9.0, 0.3, "crawl"), # 弯腰通过限高门中心
|
||
(5.7, -7.8, 0.3, "rough"), # 只要过了门槛，直接恢复为常规高度
|
||
(3.7, -9.0, 0.5, "rough") # 顺畅返回起点
|
||
]
|
||
nav.start_mission("限高爬行任务", crawl_wps)
|
||
elif btn_stairs.is_clicked(event):
|
||
# 阶梯攀爬：利用 X=3.3 通道绕过高墙边缘，水平对齐轴线登顶，并完整从另一侧下台阶穿过
|
||
stairs_wps = [
|
||
(3.3, -9.0, 0.5, "rough"), # 1. 移出起步区，向西对准 X = 3.3 避开高墙通道
|
||
(3.3, -5.8, 0.5, "rough"), # 2. 沿 X = 3.3 笔直向北，完全绕开高墙右侧边缘（留出超大空间）
|
||
(1.8, -5.8, 0.5, "rough"), # 3. 水平移入台阶前方的平地/空地中心，完美对齐轴线
|
||
(1.8, -3.5, 0.4, "rough"), # 4. 直线攀爬登上顶部平台
|
||
(1.8, -1.6, 0.4, "rough"), # 5. 直线走下另一侧台阶，到 Y=-1.6m，后腿完全过台阶，前腿也不上桥，在平地平稳落地
|
||
(3.3, -1.6, 0.5, "rough"), # 6. 横向东移，重新对齐到 X = 3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 7. 沿 X = 3.3 安全线直行南下，完全避开高墙与台阶
|
||
(3.7, -9.0, 0.6, "rough") # 8. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("阶梯攀爬登顶", stairs_wps)
|
||
elif btn_bridge.is_clicked(event):
|
||
bridge_wps = [
|
||
(3.3, -9.0, 0.5, "rough"), # 1. 移出起步区，向西对准 X = 3.3 避开高墙通道
|
||
(3.3, -1.6, 0.5, "rough"), # 2. 沿 X = 3.3 笔直向北，完全绕开所有高墙与台阶结构
|
||
(1.8, -1.6, 0.5, "rough"), # 3. 水平移入木桥 A 斜坡起点的平地
|
||
(1.8, -1.38, 0.4, "rough"), # 4. 对齐木桥 A 坡底起点
|
||
(1.8, 0.0, 0.4, "rough"), # 5. 稳定爬上木桥 A 斜坡登上平台
|
||
(3.2, 0.0, 0.4, "rough"), # 6. 沿独木桥 Y=0 轴线精细循迹
|
||
(4.85, 0.0, 0.4, "rough"), # 7. 沿独木桥 Y=0 轴线继续前进
|
||
(5.7, -0.5, 0.4, "rough"), # 8. 驶入东侧木桥转弯缓冲段
|
||
(5.7, -2.25, 0.4, "rough"), # 9. 驶上中间木桥平台
|
||
(5.7, -3.5, 0.4, "rough"), # 10. 到达木桥 B 斜坡起爬平整区
|
||
(5.7, -5.0, 0.4, "rough"), # 11. 稳健行进下斜坡 B
|
||
(5.7, -7.0, 0.4, "rough"), # 12. 平稳落地脱离木桥 B，在平地上放平车身
|
||
(3.3, -7.0, 0.5, "rough"), # 13. 横向西移，重新汇入 X=3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 14. 沿 X=3.3 安全走廊直行南下
|
||
(3.7, -9.0, 0.6, "rough") # 15. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("斜坡木桥任务", bridge_wps)
|
||
elif btn_gravel.is_clicked(event):
|
||
gravel_wps = [
|
||
(3.7, -12.5, 0.5, "rough"), # 1. 沿 X=3.7 轴线一路笔直向南，在 flat 地面上完全避开限高门柱
|
||
(4.84, -12.5, 0.4, "rough"), # 2. 横向东进，从西侧入口完美切入 1号砂砾平台 tip
|
||
(5.84, -12.0, 0.4, "rough"), # 3. 横跨转移至 2号砂砾平台
|
||
(5.84, -10.2, 0.4, "rough"), # 4. 笔直向北走下平台，平稳落于北侧平地 tip
|
||
(3.3, -10.2, 0.5, "rough"), # 5. 西向横移，重新汇入 X=3.3 安全返航通道
|
||
(3.3, -9.0, 0.5, "rough"), # 6. 沿着 X=3.3 直行北上
|
||
(3.7, -9.0, 0.6, "rough") # 7. 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("砂砾碎石任务", gravel_wps)
|
||
elif btn_grand.is_clicked(event):
|
||
grand_wps = [
|
||
# 1. S形绕杆
|
||
(2.9, -9.5, 0.4, "rough"),
|
||
(1.8, -9.95, 0.4, "rough"),
|
||
(0.9, -10.0, 0.4, "rough"),
|
||
(0.8, -10.8, 0.4, "rough"),
|
||
(0.9, -11.0, 0.4, "rough"),
|
||
(2.7, -11.0, 0.4, "rough"),
|
||
(2.8, -11.8, 0.4, "rough"),
|
||
(2.7, -12.0, 0.4, "rough"),
|
||
(0.9, -12.0, 0.4, "rough"),
|
||
(0.9, -12.75, 0.4, "rough"),
|
||
(1.42, -12.82, 0.4, "rough"),
|
||
(2.3, -13.3, 0.4, "rough"),
|
||
(2.3, -11.5, 0.4, "rough"),
|
||
(3.32, -12.44, 0.4, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 2. 限高下蹲
|
||
(5.7, -10.2, 0.5, "rough"),
|
||
(5.7, -9.0, 0.3, "crawl"),
|
||
(5.7, -7.8, 0.3, "rough"),
|
||
(5.7, -7.0, 0.5, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 3. 砂砾碎石
|
||
(3.7, -12.5, 0.5, "rough"),
|
||
(4.84, -12.5, 0.4, "rough"),
|
||
(5.84, -12.0, 0.4, "rough"),
|
||
(5.84, -10.2, 0.4, "rough"),
|
||
(3.3, -10.2, 0.5, "rough"),
|
||
(3.3, -9.0, 0.5, "rough"),
|
||
(3.7, -9.0, 0.5, "rough"),
|
||
# 4. 高墙越障
|
||
(3.3, -9.0, 0.5, "rough"),
|
||
(1.8, -9.0, 0.5, "rough"),
|
||
(1.8, -8.0, 0.4, "rough"),
|
||
(1.8, -7.5, 0.4, "rough"),
|
||
(1.8, -5.8, 1.2, "rough"),
|
||
# 5. 台阶攀爬
|
||
(1.8, -3.5, 0.4, "rough"),
|
||
(1.8, -1.6, 0.4, "rough"),
|
||
# 6. 斜坡木桥
|
||
(1.8, -1.38, 0.4, "rough"),
|
||
(1.8, 0.0, 0.4, "rough"),
|
||
(3.2, 0.0, 0.4, "rough"),
|
||
(4.85, 0.0, 0.4, "rough"),
|
||
(5.7, -0.5, 0.4, "rough"),
|
||
(5.7, -2.25, 0.4, "rough"),
|
||
(5.7, -3.5, 0.4, "rough"),
|
||
(5.7, -5.0, 0.4, "rough"),
|
||
(5.7, -7.0, 0.4, "rough"),
|
||
(3.3, -7.0, 0.5, "rough"), # 横向西移，重新汇入 X=3.3 安全通道
|
||
(3.3, -9.0, 0.5, "rough"), # 沿 X=3.3 通道南下返回
|
||
(3.7, -9.0, 0.6, "rough") # 顺畅平准移入起点
|
||
]
|
||
nav.start_mission("障碍赛大满贯", grand_wps)
|
||
elif btn_clear_trg.is_clicked(event):
|
||
nav.cancel_mission()
|
||
elif btn_center_toggle.is_clicked(event):
|
||
auto_center = not auto_center
|
||
btn_center_toggle.text = "视角居中:开" if auto_center else "视角居中:关"
|
||
btn_center_toggle.bg_color = (16, 185, 129) if auto_center else (244, 63, 94)
|
||
elif btn_draw_mode.is_clicked(event):
|
||
draw_mode = not draw_mode
|
||
btn_draw_mode.text = "手绘模式:开" if draw_mode else "手绘模式:关"
|
||
btn_draw_mode.bg_color = (16, 185, 129) if draw_mode else (30, 41, 59)
|
||
elif btn_speed_down.is_clicked(event):
|
||
# 物理减速
|
||
if sim_speed_factor > 1.0:
|
||
if sim_speed_factor >= 5.0: sim_speed_factor = 3.0
|
||
elif sim_speed_factor >= 3.0: sim_speed_factor = 2.0
|
||
elif sim_speed_factor >= 2.0: sim_speed_factor = 1.5
|
||
else: sim_speed_factor = 1.0
|
||
else:
|
||
if sim_speed_factor >= 1.0: sim_speed_factor = 0.8
|
||
elif sim_speed_factor >= 0.8: sim_speed_factor = 0.5
|
||
elif sim_speed_factor >= 0.5: sim_speed_factor = 0.2
|
||
print(f"[仿真控制] 物理减速。当前加速倍率: {sim_speed_factor:.1f}x")
|
||
elif btn_speed_normal.is_clicked(event):
|
||
sim_speed_factor = 1.0
|
||
print(f"[仿真控制] 恢复标准时间流速 (1x)")
|
||
elif btn_speed_up.is_clicked(event):
|
||
# 物理加速
|
||
if sim_speed_factor < 1.0:
|
||
if sim_speed_factor <= 0.2: sim_speed_factor = 0.5
|
||
elif sim_speed_factor <= 0.5: sim_speed_factor = 0.8
|
||
else: sim_speed_factor = 1.0
|
||
else:
|
||
if sim_speed_factor <= 1.0: sim_speed_factor = 1.5
|
||
elif sim_speed_factor <= 1.5: sim_speed_factor = 2.0
|
||
elif sim_speed_factor <= 2.0: sim_speed_factor = 3.0
|
||
elif sim_speed_factor <= 3.0: sim_speed_factor = 5.0
|
||
print(f"[仿真控制] 物理加速。当前加速倍率: {sim_speed_factor:.1f}x")
|
||
elif btn_x_plus.is_clicked(event):
|
||
nav.mode = "AUTO"
|
||
nav.target_x = (nav.target_x if nav.target_x is not None else rx) + 0.5
|
||
nav.target_y = nav.target_y if nav.target_y is not None else ry
|
||
elif btn_x_minus.is_clicked(event):
|
||
nav.mode = "AUTO"
|
||
nav.target_x = (nav.target_x if nav.target_x is not None else rx) - 0.5
|
||
nav.target_y = nav.target_y if nav.target_y is not None else ry
|
||
elif btn_y_plus.is_clicked(event):
|
||
nav.mode = "AUTO"
|
||
nav.target_x = nav.target_x if nav.target_x is not None else rx
|
||
nav.target_y = (nav.target_y if nav.target_y is not None else ry) + 0.5
|
||
elif btn_y_minus.is_clicked(event):
|
||
nav.mode = "AUTO"
|
||
nav.target_x = nav.target_x if nav.target_x is not None else rx
|
||
nav.target_y = (nav.target_y if nav.target_y is not None else ry) - 0.5
|
||
elif btn_rec_key.is_clicked(event):
|
||
key_id = len(nav.recorded_keypoints) + 1
|
||
nav.recorded_keypoints.append({
|
||
'id': key_id, 'x': rx, 'y': ry, 'z': rz, 'yaw': yaw_deg
|
||
})
|
||
print(f"[关键点] 已记录 WP{key_id}：({rx:.2f}, {ry:.2f})")
|
||
elif btn_exp_key.is_clicked(event):
|
||
try:
|
||
exp_path = Path(__file__).parent / "recorded_keypoints.txt"
|
||
with open(exp_path, "w", encoding="utf-8") as f:
|
||
f.write("序号, X坐标(米), Y坐标(米), Z高度(米), 机身航向角(度)\n")
|
||
for kp in nav.recorded_keypoints:
|
||
f.write(f"WP{kp['id']}, {kp['x']:.4f}, {kp['y']:.4f}, {kp['z']:.4f}, {kp['yaw']:.2f}\n")
|
||
print(f"[关键点] 数据文件已成功导出至: {exp_path}")
|
||
except Exception as ex:
|
||
print(f"[关键点] 导出出错: {ex}")
|
||
elif btn_clear_key.is_clicked(event):
|
||
nav.recorded_keypoints = []
|
||
print("[关键点] 所有已记录的关键点已清空。")
|
||
|
||
# ------------------------------------------------------------------
|
||
# 自动路径寻寻航与物理指令步进 (50Hz 控制频率)
|
||
# ------------------------------------------------------------------
|
||
if nav.mode == "AUTO":
|
||
cmd_vx, cmd_vy, cmd_wz = nav.update_navigation(rx, ry, yaw_rad, runner, dt=control_dt)
|
||
command_input = np.array([cmd_vx, cmd_vy, cmd_wz], dtype=np.float32)
|
||
else:
|
||
# 读取键盘输入控制：方向键控制 x (前后) 和 yaw (旋转)，A/D 键控制 y (左右横移)
|
||
keys = pygame.key.get_pressed()
|
||
cmd_vx, cmd_vy, cmd_wz = 0.0, 0.0, 0.0
|
||
|
||
# 方向键控制 x (前后，峰值限制为 1.2m/s)
|
||
if keys[pygame.K_UP]: cmd_vx = 1.2
|
||
elif keys[pygame.K_DOWN]: cmd_vx = -1.2
|
||
|
||
# A/D 键控制 y (左右横移)
|
||
if keys[pygame.K_a]: cmd_vy = 0.8
|
||
elif keys[pygame.K_d]: cmd_vy = -0.8
|
||
|
||
# 方向键控制 yaw (旋转，峰值限制为 1.2r/s)
|
||
if keys[pygame.K_LEFT]: cmd_wz = 1.2
|
||
elif keys[pygame.K_RIGHT]: cmd_wz = -1.2
|
||
|
||
command_input = np.array([cmd_vx, cmd_vy, cmd_wz], dtype=np.float32)
|
||
|
||
# 融合观测数据
|
||
obs = io.get_obs_53d(command_input, runner.default_dof_pos, runner.last_actions)
|
||
|
||
# 策略网络推理
|
||
scaled_actions, raw_actions = runner.step(obs)
|
||
|
||
# 向仿真器下发关节控制指令
|
||
io.send_actions(scaled_actions, runner.default_dof_pos)
|
||
|
||
# 推进物理步进
|
||
for _ in range(sim_steps_per_control):
|
||
mujoco.mj_step(io.m, io.d)
|
||
|
||
# 控制视口镜头跟随
|
||
viewer_counter += 1
|
||
if viewer_counter >= 2:
|
||
base_id = mujoco.mj_name2id(io.m, mujoco.mjtObj.mjOBJ_BODY, "base_link")
|
||
if base_id != -1:
|
||
viewer.cam.lookat[:] = io.d.xpos[base_id]
|
||
viewer.sync()
|
||
viewer_counter = 0
|
||
|
||
# ------------------------------------------------------------------
|
||
# 画面绘制：左侧网格物理元素实景图 (100% 实景对应解析绘制)
|
||
# ------------------------------------------------------------------
|
||
pygame.draw.rect(screen, COLOR_BG, (0, 0, MAP_WIDTH, WINDOW_HEIGHT))
|
||
|
||
# 计算当前网格绘制边界
|
||
grid_min_w, grid_min_h = screen_to_world(0, WINDOW_HEIGHT)
|
||
grid_max_w, grid_max_h = screen_to_world(MAP_WIDTH, 0)
|
||
|
||
start_grid_x = int(math.floor(grid_min_w))
|
||
end_grid_x = int(math.ceil(grid_max_w))
|
||
start_grid_y = int(math.floor(grid_min_h))
|
||
end_grid_y = int(math.ceil(grid_max_h))
|
||
|
||
# 绘制纵轴网格线
|
||
for gx in range(start_grid_x, end_grid_x + 1):
|
||
sx, _ = world_to_screen(gx, 0)
|
||
color = COLOR_AXIS if gx == 0 else COLOR_GRID
|
||
width = 2 if gx == 0 else 1
|
||
pygame.draw.line(screen, color, (sx, 0), (sx, WINDOW_HEIGHT), width)
|
||
if gx % 2 == 0 and 0 < sx < MAP_WIDTH - 20:
|
||
lbl = font_mono.render(f"{gx}m", True, COLOR_TEXT_MUTED)
|
||
screen.blit(lbl, (sx + 3, WINDOW_HEIGHT - 20))
|
||
|
||
# 绘制横轴网格线
|
||
for gy in range(start_grid_y, end_grid_y + 1):
|
||
_, sy = world_to_screen(0, gy)
|
||
color = COLOR_AXIS if gy == 0 else COLOR_GRID
|
||
width = 2 if gy == 0 else 1
|
||
pygame.draw.line(screen, color, (0, sy), (MAP_WIDTH, sy), width)
|
||
if gy % 2 == 0 and 0 < sy < WINDOW_HEIGHT - 20:
|
||
lbl = font_mono.render(f"{gy}m", True, COLOR_TEXT_MUTED)
|
||
screen.blit(lbl, (5, sy + 3))
|
||
|
||
# ------------------------------------------------------------------
|
||
# 🌟 动态地图绘制引擎 —— 解析并精确渲染 scene_terrain.xml 所有实景元素
|
||
# ------------------------------------------------------------------
|
||
for geom in xml_obstacles:
|
||
g_type = geom['type']
|
||
pos = geom['pos']
|
||
size = geom['size']
|
||
quat = geom['quat']
|
||
rgba = geom['rgba']
|
||
name = geom['name']
|
||
|
||
# 转换色彩至 0-255 并增加发光度
|
||
color = (int(rgba[0]*255), int(rgba[1]*255), int(rgba[2]*255))
|
||
cx, cy = pos[0], pos[1]
|
||
|
||
# 1. 矩形盒子渲染（支持任意偏航角 Z轴自转 顶点多边形解算）
|
||
if g_type == 'box':
|
||
qw, qx, qy, qz = quat
|
||
siny_cosp = 2.0 * (qw * qz + qx * qy)
|
||
cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
|
||
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||
|
||
sx, sy = size[0], size[1]
|
||
local_corners = [
|
||
(-sx, -sy), (sx, -sy), (sx, sy), (-sx, sy)
|
||
]
|
||
screen_corners = []
|
||
for lx, ly in local_corners:
|
||
# 空间旋转
|
||
wx = cx + lx * math.cos(yaw) - ly * math.sin(yaw)
|
||
wy = cy + lx * math.sin(yaw) + ly * math.cos(yaw)
|
||
screen_corners.append(world_to_screen(wx, wy))
|
||
|
||
# 填充半透明色，勾画发光边界
|
||
pygame.draw.polygon(screen, color, screen_corners)
|
||
border_color = (min(color[0]+40, 255), min(color[1]+40, 255), min(color[2]+40, 255))
|
||
pygame.draw.polygon(screen, border_color, screen_corners, width=1)
|
||
|
||
# 如果是高越障墙，渲染额外标识
|
||
if abs(cy - (-7.0)) < 0.1:
|
||
wall_lbl = font_small.render("Obstacle Wall", True, COLOR_TEXT_LIGHT)
|
||
screen.blit(wall_lbl, (screen_corners[0][0] + 5, screen_corners[0][1] - 18))
|
||
|
||
# 2. 圆柱体渲染
|
||
elif g_type == 'cylinder':
|
||
sx, sy = world_to_screen(cx, cy)
|
||
rad = max(int(size[0] * zoom), 3)
|
||
pygame.draw.circle(screen, color, (sx, sy), rad)
|
||
border_color = (min(color[0]+40, 255), min(color[1]+40, 255), min(color[2]+40, 255))
|
||
pygame.draw.circle(screen, border_color, (sx, sy), rad, width=1)
|
||
|
||
# 如果是绕杆立柱，加注底图安全边界环
|
||
if "Slalom" in name:
|
||
pygame.draw.circle(screen, (6, 182, 212, 30), (sx, sy), int(0.1 * zoom))
|
||
pygame.draw.circle(screen, COLOR_CYAN, (sx, sy), int(0.1 * zoom), width=1)
|
||
|
||
# 3. 半透明 hfield 高原区域渲染
|
||
elif g_type == 'hfield':
|
||
sx = size[0] if len(size) > 0 else 1.0
|
||
sy = size[1] if len(size) > 1 else 1.0
|
||
x1, y1 = world_to_screen(cx - sx, cy + sy)
|
||
x2, y2 = world_to_screen(cx + sx, cy - sy)
|
||
w = x2 - x1
|
||
h = y2 - y1
|
||
if w > 0 and h > 0:
|
||
surf = pygame.Surface((w, h), pygame.SRCALPHA)
|
||
surf.fill((139, 92, 246, 35)) # 紫色高原提示
|
||
pygame.draw.rect(surf, COLOR_PURPLE, (0, 0, w, h), width=1)
|
||
screen.blit(surf, (x1, y1))
|
||
hf_lbl = font_mono.render("HField Platform", True, COLOR_PURPLE)
|
||
screen.blit(hf_lbl, (x1 + 6, y1 + 4))
|
||
|
||
# ------------------------------------------------------------------
|
||
# 规划与运动行迹线绘制
|
||
# ------------------------------------------------------------------
|
||
# 绘制机器人面包屑行迹线
|
||
if len(robot_trail) > 1:
|
||
trail_points = [world_to_screen(pt[0], pt[1]) for pt in robot_trail]
|
||
pygame.draw.lines(screen, COLOR_CYAN, False, trail_points, 2)
|
||
|
||
# 绘制正在实时手划的绿色轨迹
|
||
if is_drawing_path and len(drawn_points) > 1:
|
||
draw_pts = [world_to_screen(pt[0], pt[1]) for pt in drawn_points]
|
||
pygame.draw.lines(screen, COLOR_EMERALD, False, draw_pts, 3)
|
||
|
||
# 绘制已加载的自动路径寻迹线
|
||
if len(nav.drawn_path) > 1:
|
||
path_pts = [world_to_screen(pt[0], pt[1]) for pt in nav.drawn_path]
|
||
pygame.draw.lines(screen, COLOR_EMERALD, False, path_pts, 2)
|
||
# 渲染每一个航点节点小球
|
||
for pt in nav.drawn_path:
|
||
sx, sy = world_to_screen(pt[0], pt[1])
|
||
pygame.draw.circle(screen, COLOR_CYAN, (sx, sy), 4)
|
||
|
||
# 绘制关键点数据打点
|
||
for kp in nav.recorded_keypoints:
|
||
kx, ky = world_to_screen(kp['x'], kp['y'])
|
||
pygame.draw.circle(screen, COLOR_PURPLE, (kx, ky), 7)
|
||
pygame.draw.circle(screen, COLOR_TEXT_LIGHT, (kx, ky), 7, width=1)
|
||
kp_lbl = font_mono.render(f"WP{kp['id']}", True, COLOR_TEXT_LIGHT)
|
||
screen.blit(kp_lbl, (kx - 8, ky - 18))
|
||
|
||
# 绘制当前正在行进指向的目标点
|
||
if nav.target_x is not None and nav.target_y is not None:
|
||
tx, ty = world_to_screen(nav.target_x, nav.target_y)
|
||
pulse_rad = int(8 + 4 * math.sin(time.time() * 8.0))
|
||
pygame.draw.circle(screen, COLOR_GOLD, (tx, ty), pulse_rad, width=2)
|
||
pygame.draw.circle(screen, COLOR_GOLD, (tx, ty), 3)
|
||
pygame.draw.line(screen, COLOR_GOLD, (tx - pulse_rad - 4, ty), (tx + pulse_rad + 4, ty), 1)
|
||
pygame.draw.line(screen, COLOR_GOLD, (tx, ty - pulse_rad - 4), (tx, ty + pulse_rad + 4), 1)
|
||
trg_lbl = font_mono.render(f"Target: ({nav.target_x:.2f}, {nav.target_y:.2f})", True, COLOR_GOLD)
|
||
screen.blit(trg_lbl, (tx + 12, ty - 6))
|
||
|
||
# ------------------------------------------------------------------
|
||
# 渲染机器人本体图标 (多边形轮骨，指示航向)
|
||
# ------------------------------------------------------------------
|
||
rx_pix, ry_pix = world_to_screen(rx, ry)
|
||
robot_rad = max(int(0.25 * zoom), 6)
|
||
|
||
# 画机身盘
|
||
pygame.draw.circle(screen, COLOR_CYAN, (rx_pix, ry_pix), robot_rad)
|
||
pygame.draw.circle(screen, COLOR_TEXT_LIGHT, (rx_pix, ry_pix), robot_rad, width=2)
|
||
|
||
# 航向箭头鼻线
|
||
nose_wx = rx + 0.35 * math.cos(yaw_rad)
|
||
nose_wy = ry + 0.35 * math.sin(yaw_rad)
|
||
nose_x_pix, nose_y_pix = world_to_screen(nose_wx, nose_wy)
|
||
pygame.draw.line(screen, COLOR_EMERALD, (rx_pix, ry_pix), (nose_x_pix, nose_y_pix), 3)
|
||
pygame.draw.circle(screen, COLOR_EMERALD, (nose_x_pix, nose_y_pix), 3)
|
||
|
||
# 四轮足点映射
|
||
wheel_offset_w = 0.2
|
||
for angle_offset in [-0.7, 0.7, -2.4, 2.4]:
|
||
w_angle = yaw_rad + angle_offset
|
||
w_wx = rx + wheel_offset_w * math.cos(w_angle)
|
||
w_wy = ry + wheel_offset_w * math.sin(w_angle)
|
||
w_x_pix, w_y_pix = world_to_screen(w_wx, w_wy)
|
||
pygame.draw.rect(screen, (0, 0, 0), (w_x_pix - 4, w_y_pix - 4, 8, 8), border_radius=4)
|
||
|
||
# ------------------------------------------------------------------
|
||
# 渲染右侧 HUD 磨砂玻璃科技控制面板
|
||
# ------------------------------------------------------------------
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH, 0), (MAP_WIDTH, WINDOW_HEIGHT), 2)
|
||
pygame.draw.rect(screen, COLOR_HUD_BG, (MAP_WIDTH, 0, WINDOW_WIDTH - MAP_WIDTH, WINDOW_HEIGHT))
|
||
|
||
# 面板标题
|
||
screen.blit(font_large.render("数据与控制面板", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, 15))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, 42), (WINDOW_WIDTH - 20, 42), 1)
|
||
|
||
# 自动模式状态指示
|
||
y_offset = 55
|
||
screen.blit(font_medium.render("当前状态：", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, y_offset))
|
||
if nav.mode == "AUTO":
|
||
pygame.draw.rect(screen, (16, 185, 129, 40), (MAP_WIDTH + 110, y_offset - 2, 85, 22), border_radius=4)
|
||
screen.blit(font_medium.render("自动巡航", True, COLOR_EMERALD), (MAP_WIDTH + 120, y_offset))
|
||
else:
|
||
pygame.draw.rect(screen, (244, 63, 94, 40), (MAP_WIDTH + 110, y_offset - 2, 85, 22), border_radius=4)
|
||
screen.blit(font_medium.render("手动接管", True, COLOR_ROSE), (MAP_WIDTH + 120, y_offset))
|
||
|
||
# 当前驱动神经网络控制策略
|
||
y_offset = 85
|
||
screen.blit(font_medium.render("当前高度：", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, y_offset))
|
||
p_name = runner.current_policy_name.upper()
|
||
p_color = COLOR_CYAN if p_name == "CRAWL" else COLOR_AMBER
|
||
cn_pname = "低姿爬行" if p_name == "CRAWL" else "常规站立"
|
||
screen.blit(font_medium.render(cn_pname, True, p_color), (MAP_WIDTH + 110, y_offset))
|
||
|
||
# (已移除自动脱困及卡死报警提醒)
|
||
|
||
# 当前仿真时间倍速
|
||
y_offset = 115
|
||
screen.blit(font_medium.render("仿真倍速：", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, y_offset))
|
||
speed_display_str = f"{sim_speed_factor:.1f}x"
|
||
screen.blit(font_medium.render(speed_display_str, True, COLOR_EMERALD), (MAP_WIDTH + 110, y_offset))
|
||
|
||
# 全局惯导 telemetry 数据读取
|
||
y_offset = 145
|
||
screen.blit(font_small.render("实时全局位置(m)：", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, y_offset))
|
||
y_offset += 18
|
||
pygame.draw.rect(screen, COLOR_CONSOLE_BG, (MAP_WIDTH + 20, y_offset, 285, 45), border_radius=4)
|
||
pos_text = f"X: {rx:6.3f} m Y: {ry:6.3f} m Z: {rz:5.3f} m"
|
||
screen.blit(font_mono.render(pos_text, True, COLOR_EMERALD), (MAP_WIDTH + 30, y_offset + 5))
|
||
speed_text = f"速度: {linear_speed:4.2f} m/s 转向: {wz_rate:5.2f} r/s"
|
||
screen.blit(font_mono.render(speed_text, True, COLOR_EMERALD), (MAP_WIDTH + 30, y_offset + 22))
|
||
|
||
# Euler 机身姿态
|
||
y_offset = 218
|
||
screen.blit(font_small.render("机身姿态角(deg)：", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, y_offset))
|
||
y_offset += 18
|
||
pygame.draw.rect(screen, COLOR_CONSOLE_BG, (MAP_WIDTH + 20, y_offset, 285, 30), border_radius=4)
|
||
euler_text = f"横滚: {roll_deg:5.1f}° 俯仰: {pitch_deg:5.1f}° 航向: {yaw_deg:5.1f}°"
|
||
screen.blit(font_medium.render(euler_text, True, COLOR_EMERALD), (MAP_WIDTH + 25, y_offset + 6))
|
||
|
||
# PRESET routines title
|
||
y_offset = 280
|
||
screen.blit(font_large.render("预设任务列表", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, y_offset))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, y_offset + 25), (WINDOW_WIDTH - 20, y_offset + 25), 1)
|
||
|
||
# 绘制所有功能按钮
|
||
btn_slalom.draw(screen, font_small)
|
||
btn_crawl.draw(screen, font_small)
|
||
btn_gravel.draw(screen, font_small)
|
||
btn_wall.draw(screen, font_small)
|
||
btn_stairs.draw(screen, font_small)
|
||
btn_bridge.draw(screen, font_small)
|
||
btn_grand.draw(screen, font_small)
|
||
|
||
# 系统与视图控制
|
||
y_offset = 440
|
||
screen.blit(font_large.render("系统与视图控制", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, y_offset))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, y_offset + 22), (WINDOW_WIDTH - 20, y_offset + 22), 1)
|
||
|
||
btn_clear_trg.draw(screen, font_small)
|
||
btn_center_toggle.draw(screen, font_small)
|
||
btn_draw_mode.draw(screen, font_small)
|
||
btn_speed_down.draw(screen, font_small)
|
||
btn_speed_normal.draw(screen, font_small)
|
||
btn_speed_up.draw(screen, font_small)
|
||
|
||
# 发令命令
|
||
y_offset = 542
|
||
screen.blit(font_medium.render("目标微调", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, y_offset))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, y_offset + 18), (WINDOW_WIDTH - 20, y_offset + 18), 1)
|
||
|
||
btn_x_plus.draw(screen, font_small)
|
||
btn_x_minus.draw(screen, font_small)
|
||
btn_y_plus.draw(screen, font_small)
|
||
btn_y_minus.draw(screen, font_small)
|
||
|
||
# 打点打点打点
|
||
y_offset = 605
|
||
screen.blit(font_medium.render("关键点记录 (R键)", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, y_offset))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, y_offset + 18), (WINDOW_WIDTH - 20, y_offset + 18), 1)
|
||
|
||
# 渲染最新记录位置
|
||
if nav.recorded_keypoints:
|
||
kp = nav.recorded_keypoints[-1]
|
||
kp_str = f"最新记录: WP{kp['id']}: X:{kp['x']:5.2f} Y:{kp['y']:5.2f} YAW:{kp['yaw']:4.0f}°"
|
||
screen.blit(font_mono.render(kp_str, True, COLOR_PURPLE), (MAP_WIDTH + 20, 661))
|
||
else:
|
||
screen.blit(font_small.render("最新记录: 暂无记录位置 (按R键保存)", True, COLOR_TEXT_MUTED), (MAP_WIDTH + 20, 661))
|
||
|
||
btn_rec_key.draw(screen, font_small)
|
||
btn_exp_key.draw(screen, font_small)
|
||
btn_clear_key.draw(screen, font_small)
|
||
|
||
# 命令行
|
||
y_offset = 685
|
||
screen.blit(font_medium.render("命令行输入", True, COLOR_TEXT_LIGHT), (MAP_WIDTH + 20, y_offset))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (MAP_WIDTH + 20, y_offset + 18), (WINDOW_WIDTH - 20, y_offset + 18), 1)
|
||
|
||
# 指令框
|
||
con_color = COLOR_EMERALD if console_active else COLOR_HUD_BORDER
|
||
pygame.draw.rect(screen, COLOR_CONSOLE_BG, console_rect, border_radius=4)
|
||
pygame.draw.rect(screen, con_color, console_rect, width=1, border_radius=4)
|
||
|
||
cursor = "_" if (int(time.time() * 2.0) % 2 == 0 and console_active) else ""
|
||
con_str = f" > {console_text}{cursor}"
|
||
screen.blit(font_mono.render(con_str, True, COLOR_EMERALD), (console_rect.x + 5, console_rect.y + 4))
|
||
|
||
# 地图横条
|
||
pygame.draw.rect(screen, COLOR_CONSOLE_BG, (0, WINDOW_HEIGHT - 25, MAP_WIDTH, 25))
|
||
pygame.draw.line(screen, COLOR_HUD_BORDER, (0, WINDOW_HEIGHT - 25), (MAP_WIDTH, WINDOW_HEIGHT - 25), 1)
|
||
status_str = f"Target: X: {nav.target_x if nav.target_x is not None else 0.0:.2f} m, Y: {nav.target_y if nav.target_y is not None else 0.0:.2f} m | Mission: {nav.mission_name}"
|
||
screen.blit(font_mono.render(status_str, True, COLOR_TEXT_MUTED), (10, WINDOW_HEIGHT - 20))
|
||
|
||
pygame.display.flip()
|
||
|
||
# 50Hz 物理锁帧同步计算
|
||
dt_real = control_dt / sim_speed_factor
|
||
next_exec_time += dt_real
|
||
now = time.perf_counter()
|
||
sleep_time = next_exec_time - now
|
||
if sleep_time > 0:
|
||
time.sleep(sleep_time)
|
||
elif sleep_time < -dt_real:
|
||
next_exec_time = now
|
||
|
||
clock.tick(int(50 * sim_speed_factor))
|
||
|
||
print("\n[主程序] 退出网格主循环。正在关闭物理引擎环境...")
|
||
pygame.quit()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|