第一版本
这个提交包含在:
386
main.py
普通文件
386
main.py
普通文件
@ -0,0 +1,386 @@
|
|||||||
|
import pygame
|
||||||
|
import sys
|
||||||
|
import websockets
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Dict, Any, List, Tuple
|
||||||
|
from enum import Enum
|
||||||
|
|
||||||
|
class Joint(Enum):
|
||||||
|
GROUND = "Ground"
|
||||||
|
JOINT0 = "Joint0"
|
||||||
|
JOINT2 = "Joint2"
|
||||||
|
JOINT3 = "Joint3"
|
||||||
|
TURNER = "Turner"
|
||||||
|
CLAW = "Claw"
|
||||||
|
|
||||||
|
class ControlKey(Enum):
|
||||||
|
MOVE_UP = pygame.K_UP
|
||||||
|
MOVE_DOWN = pygame.K_DOWN
|
||||||
|
MOVE_LEFT = pygame.K_LEFT
|
||||||
|
MOVE_RIGHT = pygame.K_RIGHT
|
||||||
|
JOINT2_UP = pygame.K_w
|
||||||
|
JOINT2_DOWN = pygame.K_s
|
||||||
|
JOINT3_UP = pygame.K_e
|
||||||
|
JOINT3_DOWN = pygame.K_d
|
||||||
|
CLAW_OPEN = pygame.K_SPACE
|
||||||
|
QUIT = pygame.K_ESCAPE
|
||||||
|
RESET_ALL = pygame.K_r
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class JointState:
|
||||||
|
id: str
|
||||||
|
value: int
|
||||||
|
min_val: int = 0
|
||||||
|
max_val: int = 180
|
||||||
|
step: int = 5 # 每帧的步进值,实现平滑移动
|
||||||
|
|
||||||
|
class RobotStatus:
|
||||||
|
def __init__(self):
|
||||||
|
self.joints = {
|
||||||
|
Joint.GROUND: JointState(id=Joint.GROUND.value, value=90),
|
||||||
|
Joint.JOINT0: JointState(id=Joint.JOINT0.value, value=90),
|
||||||
|
Joint.JOINT2: JointState(id=Joint.JOINT2.value, value=90),
|
||||||
|
Joint.JOINT3: JointState(id=Joint.JOINT3.value, value=90),
|
||||||
|
Joint.TURNER: JointState(id=Joint.TURNER.value, value=90),
|
||||||
|
Joint.CLAW: JointState(id=Joint.CLAW.value, value=90, min_val=0, max_val=120)
|
||||||
|
}
|
||||||
|
self.active_controls = {
|
||||||
|
Joint.JOINT0: 0, # -1: 减少, 0: 无操作, 1: 增加
|
||||||
|
Joint.JOINT2: 0,
|
||||||
|
Joint.JOINT3: 0,
|
||||||
|
Joint.TURNER: 0
|
||||||
|
}
|
||||||
|
|
||||||
|
def get_joint(self, joint: Joint) -> JointState:
|
||||||
|
return self.joints[joint]
|
||||||
|
|
||||||
|
def update_joint(self, joint: Joint, value: int):
|
||||||
|
joint_state = self.joints[joint]
|
||||||
|
joint_state.value = max(joint_state.min_val, min(joint_state.max_val, value))
|
||||||
|
|
||||||
|
def set_control_direction(self, joint: Joint, direction: int):
|
||||||
|
"""设置控制方向 (-1, 0, 1)"""
|
||||||
|
self.active_controls[joint] = direction
|
||||||
|
|
||||||
|
def update_continuous(self):
|
||||||
|
"""根据当前激活的控制更新关节状态"""
|
||||||
|
status_changed = False
|
||||||
|
|
||||||
|
for joint, direction in self.active_controls.items():
|
||||||
|
if direction != 0:
|
||||||
|
joint_state = self.joints[joint]
|
||||||
|
new_value = joint_state.value + direction * joint_state.step
|
||||||
|
new_value = max(joint_state.min_val, min(joint_state.max_val, new_value))
|
||||||
|
|
||||||
|
if new_value != joint_state.value:
|
||||||
|
joint_state.value = new_value
|
||||||
|
status_changed = True
|
||||||
|
|
||||||
|
return status_changed
|
||||||
|
|
||||||
|
def toggle_claw(self):
|
||||||
|
claw = self.joints[Joint.CLAW]
|
||||||
|
self.update_joint(Joint.CLAW, 0 if claw.value > 45 else 90)
|
||||||
|
return True
|
||||||
|
|
||||||
|
def reset_all(self):
|
||||||
|
for joint_state in self.joints.values():
|
||||||
|
joint_state.value = 90 if joint_state.max_val >= 90 else 0
|
||||||
|
return True
|
||||||
|
|
||||||
|
def get_updates(self) -> List[Dict[str, Any]]:
|
||||||
|
"""获取所有需要发送的更新数据"""
|
||||||
|
return [{"id": joint_state.id, "val": joint_state.value}
|
||||||
|
for joint_state in self.joints.values()]
|
||||||
|
|
||||||
|
class WebSocketClient:
|
||||||
|
def __init__(self, url: str):
|
||||||
|
self.url = url
|
||||||
|
self.connection = None
|
||||||
|
|
||||||
|
async def connect(self):
|
||||||
|
"""建立WebSocket连接"""
|
||||||
|
try:
|
||||||
|
self.connection = await websockets.connect(self.url)
|
||||||
|
print(f"已连接到 {self.url}")
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"无法连接到WebSocket服务器: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
async def send_updates(self, updates: List[Dict[str, Any]]) -> bool:
|
||||||
|
"""异步发送更新数据到服务器"""
|
||||||
|
if not self.connection:
|
||||||
|
print("未建立WebSocket连接")
|
||||||
|
return False
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 发送所有更新
|
||||||
|
for update in updates:
|
||||||
|
await self.connection.send(json.dumps(update))
|
||||||
|
await asyncio.sleep(0.001) # 小延迟避免发送过快
|
||||||
|
|
||||||
|
return True
|
||||||
|
except websockets.exceptions.ConnectionClosed:
|
||||||
|
print("WebSocket连接已关闭")
|
||||||
|
return False
|
||||||
|
except Exception as e:
|
||||||
|
print(f"发送数据时出错: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
async def close(self):
|
||||||
|
"""关闭WebSocket连接"""
|
||||||
|
if self.connection:
|
||||||
|
await self.connection.close()
|
||||||
|
|
||||||
|
class RobotController:
|
||||||
|
def __init__(self, websocket_client: WebSocketClient):
|
||||||
|
self.websocket_client = websocket_client
|
||||||
|
self.status = RobotStatus()
|
||||||
|
self.status_changed = False
|
||||||
|
self.screen:pygame.Surface
|
||||||
|
self.clock = pygame.time.Clock()
|
||||||
|
self.font:pygame.font.Font
|
||||||
|
self.last_update_time = 0
|
||||||
|
self.update_interval = 0.05 # 每秒最多更新20次
|
||||||
|
self.last_mouse_pos = None
|
||||||
|
self.mouse_sensitivity = 0.5 # 鼠标移动灵敏度
|
||||||
|
|
||||||
|
async def initialize(self):
|
||||||
|
"""初始化控制器"""
|
||||||
|
pygame.init()
|
||||||
|
self.screen = pygame.display.set_mode((1280, 960))
|
||||||
|
pygame.display.set_caption("设备控制端 - 连续控制模式")
|
||||||
|
self.font = pygame.font.SysFont("SimHei", 36)
|
||||||
|
pygame.mouse.set_visible(True)
|
||||||
|
|
||||||
|
# 连接WebSocket服务器
|
||||||
|
return await self.websocket_client.connect()
|
||||||
|
|
||||||
|
def handle_events(self):
|
||||||
|
"""处理Pygame事件"""
|
||||||
|
for event in pygame.event.get():
|
||||||
|
if event.type == pygame.QUIT:
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif event.type == pygame.KEYDOWN:
|
||||||
|
if not self.handle_keydown(event):
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif event.type == pygame.KEYUP:
|
||||||
|
self.handle_keyup(event)
|
||||||
|
|
||||||
|
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||||||
|
if event.button == 1: # 左键
|
||||||
|
self.status.update_joint(Joint.CLAW, 0)
|
||||||
|
self.status_changed = True
|
||||||
|
elif event.button == 3: # 右键
|
||||||
|
self.status.update_joint(Joint.CLAW, 120)
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
elif event.type == pygame.MOUSEBUTTONUP:
|
||||||
|
if event.button in [1, 3]: # 左右键释放
|
||||||
|
self.status.update_joint(Joint.CLAW, 120)
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
# 处理鼠标移动
|
||||||
|
self.handle_mouse_movement()
|
||||||
|
|
||||||
|
return True
|
||||||
|
|
||||||
|
def handle_keydown(self, event) -> bool:
|
||||||
|
"""处理键盘按下事件"""
|
||||||
|
if event.key == ControlKey.QUIT.value:
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif event.key == ControlKey.RESET_ALL.value:
|
||||||
|
self.status.reset_all()
|
||||||
|
self.status_changed = True
|
||||||
|
return True
|
||||||
|
|
||||||
|
# 关节连续控制
|
||||||
|
elif event.key == ControlKey.MOVE_UP.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT0, 1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.MOVE_DOWN.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT0, -1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.JOINT2_UP.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT2, 1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.JOINT2_DOWN.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT2, -1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.JOINT3_UP.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT3, 1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.JOINT3_DOWN.value:
|
||||||
|
self.status.set_control_direction(Joint.JOINT3, -1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.MOVE_LEFT.value:
|
||||||
|
self.status.set_control_direction(Joint.TURNER, -1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.MOVE_RIGHT.value:
|
||||||
|
self.status.set_control_direction(Joint.TURNER, 1)
|
||||||
|
|
||||||
|
elif event.key == ControlKey.CLAW_OPEN.value:
|
||||||
|
self.status.toggle_claw()
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
return True
|
||||||
|
|
||||||
|
def handle_keyup(self, event):
|
||||||
|
"""处理键盘释放事件"""
|
||||||
|
if event.key in [ControlKey.MOVE_UP.value, ControlKey.MOVE_DOWN.value]:
|
||||||
|
self.status.set_control_direction(Joint.JOINT0, 0)
|
||||||
|
|
||||||
|
elif event.key in [ControlKey.JOINT2_UP.value, ControlKey.JOINT2_DOWN.value]:
|
||||||
|
self.status.set_control_direction(Joint.JOINT2, 0)
|
||||||
|
|
||||||
|
elif event.key in [ControlKey.JOINT3_UP.value, ControlKey.JOINT3_DOWN.value]:
|
||||||
|
self.status.set_control_direction(Joint.JOINT3, 0)
|
||||||
|
|
||||||
|
elif event.key in [ControlKey.MOVE_LEFT.value, ControlKey.MOVE_RIGHT.value]:
|
||||||
|
self.status.set_control_direction(Joint.TURNER, 0)
|
||||||
|
|
||||||
|
def handle_mouse_movement(self):
|
||||||
|
"""处理鼠标移动控制"""
|
||||||
|
current_mouse_pos = pygame.mouse.get_pos()
|
||||||
|
|
||||||
|
if self.last_mouse_pos is not None:
|
||||||
|
dx = current_mouse_pos[0] - self.last_mouse_pos[0]
|
||||||
|
dy = current_mouse_pos[1] - self.last_mouse_pos[1]
|
||||||
|
|
||||||
|
# 左右移动控制Turner
|
||||||
|
if abs(dx) > 2: # 死区过滤
|
||||||
|
turner_change = int(dx * self.mouse_sensitivity)
|
||||||
|
current_turner = self.status.get_joint(Joint.TURNER).value
|
||||||
|
new_turner = max(0, min(180, current_turner + turner_change))
|
||||||
|
if new_turner != current_turner:
|
||||||
|
self.status.update_joint(Joint.TURNER, new_turner)
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
# 前后移动控制Joint3
|
||||||
|
if abs(dy) > 2: # 死区过滤
|
||||||
|
joint3_change = int(-dy * self.mouse_sensitivity) # 反向,因为鼠标向下移动是正方向
|
||||||
|
current_joint3 = self.status.get_joint(Joint.JOINT3).value
|
||||||
|
new_joint3 = max(0, min(180, current_joint3 + joint3_change))
|
||||||
|
if new_joint3 != current_joint3:
|
||||||
|
self.status.update_joint(Joint.JOINT3, new_joint3)
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
self.last_mouse_pos = current_mouse_pos
|
||||||
|
|
||||||
|
async def update(self, current_time: float):
|
||||||
|
"""更新控制器状态"""
|
||||||
|
# 更新连续控制
|
||||||
|
continuous_changed = self.status.update_continuous()
|
||||||
|
if continuous_changed:
|
||||||
|
self.status_changed = True
|
||||||
|
|
||||||
|
if self.status_changed and current_time - self.last_update_time >= self.update_interval:
|
||||||
|
updates = self.status.get_updates()
|
||||||
|
success = await self.websocket_client.send_updates(updates)
|
||||||
|
|
||||||
|
if not success:
|
||||||
|
# 尝试重新连接
|
||||||
|
if await self.websocket_client.connect():
|
||||||
|
await self.websocket_client.send_updates(updates)
|
||||||
|
|
||||||
|
self.status_changed = False
|
||||||
|
self.last_update_time = current_time
|
||||||
|
|
||||||
|
def render(self):
|
||||||
|
"""渲染界面"""
|
||||||
|
self.screen.fill((240, 240, 240))
|
||||||
|
|
||||||
|
# 显示标题
|
||||||
|
title_font = pygame.font.SysFont(None, 48)
|
||||||
|
title = title_font.render("机器人连续控制器", True, (0, 0, 139))
|
||||||
|
self.screen.blit(title, (50, 20))
|
||||||
|
|
||||||
|
# 显示状态信息
|
||||||
|
y_offset = 80
|
||||||
|
for joint, joint_state in self.status.joints.items():
|
||||||
|
# 关节名称和值
|
||||||
|
text = self.font.render(f"{joint.value}: {joint_state.value}°", True, (0, 0, 0))
|
||||||
|
self.screen.blit(text, (50, y_offset))
|
||||||
|
|
||||||
|
# 进度条
|
||||||
|
bar_width = 200
|
||||||
|
bar_height = 20
|
||||||
|
fill_width = int((joint_state.value - joint_state.min_val) /
|
||||||
|
(joint_state.max_val - joint_state.min_val) * bar_width)
|
||||||
|
|
||||||
|
pygame.draw.rect(self.screen, (200, 200, 200),
|
||||||
|
(250, y_offset, bar_width, bar_height))
|
||||||
|
pygame.draw.rect(self.screen, (0, 128, 0),
|
||||||
|
(250, y_offset, fill_width, bar_height))
|
||||||
|
|
||||||
|
y_offset += 40
|
||||||
|
|
||||||
|
# 显示控制说明
|
||||||
|
controls = [
|
||||||
|
"控制说明:",
|
||||||
|
"↑/↓: 控制Joint0角度 (按住连续控制)",
|
||||||
|
"←/→: 控制Turner角度 (按住连续控制)",
|
||||||
|
"W/S: 控制Joint2角度 (按住连续控制)",
|
||||||
|
"E/D: 控制Joint3角度 (按住连续控制)",
|
||||||
|
"鼠标左右移动: 控制Turner",
|
||||||
|
"鼠标前后移动: 控制Joint3",
|
||||||
|
"左键点击: 抓取,右键点击: 释放",
|
||||||
|
"空格键: 切换爪子开合",
|
||||||
|
"R键: 重置所有关节",
|
||||||
|
"ESC键: 退出程序"
|
||||||
|
]
|
||||||
|
|
||||||
|
y_offset += 40
|
||||||
|
for i, text in enumerate(controls):
|
||||||
|
color = (50, 50, 150) if i == 0 else (80, 80, 80)
|
||||||
|
rendered = self.font.render(text, True, color)
|
||||||
|
self.screen.blit(rendered, (50, y_offset + i * 35))
|
||||||
|
|
||||||
|
# 显示连接状态
|
||||||
|
status_text = "连接状态: 已连接" if self.websocket_client.connection else "连接状态: 未连接"
|
||||||
|
status_color = (0, 128, 0) if self.websocket_client.connection else (255, 0, 0)
|
||||||
|
status_rendered = self.font.render(status_text, True, status_color)
|
||||||
|
self.screen.blit(status_rendered, (50, 900))
|
||||||
|
|
||||||
|
# 显示鼠标控制提示
|
||||||
|
mouse_text = "鼠标控制中..." if self.last_mouse_pos and pygame.mouse.get_focused() else "移动鼠标控制Turner和Joint3"
|
||||||
|
mouse_rendered = self.font.render(mouse_text, True, (100, 100, 100))
|
||||||
|
self.screen.blit(mouse_rendered, (50, 930))
|
||||||
|
|
||||||
|
pygame.display.flip()
|
||||||
|
|
||||||
|
async def run(self):
|
||||||
|
"""运行主循环"""
|
||||||
|
running = True
|
||||||
|
|
||||||
|
while running:
|
||||||
|
current_time = pygame.time.get_ticks() / 1000.0 # 转换为秒
|
||||||
|
|
||||||
|
running = self.handle_events()
|
||||||
|
await self.update(current_time)
|
||||||
|
self.render()
|
||||||
|
self.clock.tick(60) # 限制帧率
|
||||||
|
|
||||||
|
await self.websocket_client.close()
|
||||||
|
pygame.quit()
|
||||||
|
sys.exit()
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
# 创建WebSocket客户端
|
||||||
|
websocket_client = WebSocketClient("ws://jamaccao.cn:8989/ws")
|
||||||
|
|
||||||
|
# 创建机器人控制器
|
||||||
|
controller = RobotController(websocket_client)
|
||||||
|
|
||||||
|
# 初始化并运行
|
||||||
|
if await controller.initialize():
|
||||||
|
await controller.run()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
asyncio.run(main())
|
||||||
3
requirements.txt
普通文件
3
requirements.txt
普通文件
@ -0,0 +1,3 @@
|
|||||||
|
websockets
|
||||||
|
pygame
|
||||||
|
asyncio
|
||||||
在新工单中引用
屏蔽一个用户