257 lines
9.5 KiB
Python
257 lines
9.5 KiB
Python
"""
|
||
Joystick Control Module
|
||
Python equivalent of the C++ Joystick functionality for ROS Joy messages
|
||
"""
|
||
import os
|
||
import time
|
||
import yaml
|
||
import threading
|
||
from dataclasses import dataclass
|
||
from typing import Optional
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from sensor_msgs.msg import Joy
|
||
import numpy as np
|
||
|
||
@dataclass
|
||
class ControlFlag:
|
||
"""手柄控制标志"""
|
||
fsm_state_command: str = "gotoZERO"
|
||
# 禁用、启用手柄控制标志
|
||
enable: bool = True
|
||
|
||
@dataclass
|
||
class YUNZHUOMap:
|
||
"""云卓T12手柄按键映射 (对应ROS Joy消息)"""
|
||
a: float = 0.0 # axes[8] #a,b,c,d手柄轴初始值为-1
|
||
b: float = 0.0 # axes[9]
|
||
c: float = 0.0 # axes[10]
|
||
d: float = 0.0 # axes[11]
|
||
e: float = 0.0 # axes[4] e,f,g,h手柄轴初始值为0.0
|
||
f: float = 0.0 # axes[7]
|
||
g: float = 0.0 # axes[5]
|
||
h: float = 0.0 # axes[6]
|
||
x1: float = 0.0 # axes[3]
|
||
x2: float = 0.0 # axes[0]
|
||
y1: float = 0.0 # axes[2]
|
||
y2: float = 0.0 # axes[1]
|
||
|
||
|
||
class YUNZHUOFlag(ControlFlag): # 继承ControlFlag
|
||
def __init__(self):
|
||
super().__init__() # 调用父类初始化
|
||
# walk command
|
||
self.x_speed_command: float = 0.0
|
||
self.y_speed_command: float = 0.0
|
||
self.yaw_speed_command: float = 0.0
|
||
self.walk_height_command: float = 0.0
|
||
# floating base command
|
||
self.waist_roll_command: float = 0.0
|
||
self.waist_pitch_command: float = 0.0
|
||
self.waist_yaw_command: float = 0.0
|
||
self.waist_height_command: float = 0.0
|
||
|
||
|
||
class JoystickHumanoid:
|
||
"""人形机器人手柄控制器 (ROS Joy版本)"""
|
||
|
||
def __init__(self):
|
||
print("Joystick Start")
|
||
|
||
# 初始化成员变量
|
||
self.joy_map = YUNZHUOMap()
|
||
self.joy_flag = YUNZHUOFlag()
|
||
self.data_mutex = threading.Lock()
|
||
|
||
# 配置参数
|
||
self.initial_height = 0.0
|
||
self.current_height = 0.0
|
||
self.max_height = 0.0
|
||
self.min_height = 0.0
|
||
self.x_command_offset = 0.0
|
||
self.y_command_offset = 0.0
|
||
self.yaw_command_offset = 0.0
|
||
self.max_x_plus_speed = 0.0
|
||
self.max_x_minus_speed = 0.0
|
||
self.max_y_speed = 0.0
|
||
self.max_yaw_speed = 0.0
|
||
# 高度平滑控制
|
||
self.target_height = 0.0
|
||
self.last_input_time = 0.0
|
||
self.last_fsm_command_time = 0.0
|
||
|
||
# 加载配置文件
|
||
self._load_config()
|
||
|
||
def _load_config(self):
|
||
"""加载YAML配置文件"""
|
||
config_path = os.path.join('.', "config", "dex_config.yaml")
|
||
|
||
with open(config_path, 'r') as file:
|
||
config = yaml.safe_load(file)
|
||
|
||
if not config:
|
||
print("[Joystick_humanoid] Failed to load config file")
|
||
return
|
||
|
||
joystick_cfg = config.get("joystick", {})
|
||
|
||
# 加载配置参数
|
||
self.initial_height = joystick_cfg.get("initial_height")
|
||
self.x_command_offset = joystick_cfg.get("x_command_offset")
|
||
self.y_command_offset = joystick_cfg.get("y_command_offset")
|
||
self.yaw_command_offset = joystick_cfg.get("yaw_command_offset")
|
||
self.max_x_plus_speed = joystick_cfg.get("max_x_plus_speed")
|
||
self.max_x_minus_speed = joystick_cfg.get("max_x_minus_speed")
|
||
self.max_y_speed = joystick_cfg.get("max_y_speed")
|
||
self.max_yaw_speed = joystick_cfg.get("max_yaw_speed")
|
||
self.max_height = joystick_cfg.get("max_height")
|
||
self.min_height = joystick_cfg.get("min_height")
|
||
|
||
print(f"Loaded initial_height: {self.initial_height}, "
|
||
f"x_command_offset: {self.x_command_offset}, "
|
||
f"y_command_offset: {self.y_command_offset}, "
|
||
f"yaw_command_offset: {self.yaw_command_offset},"
|
||
f"max_x_plus_speed: {self.max_x_plus_speed}, "
|
||
f"max_x_minus_speed: {self.max_x_minus_speed}, "
|
||
f"max_y_speed: {self.max_y_speed}, "
|
||
f"max_yaw_speed: {self.max_yaw_speed}, "
|
||
f"max_height: {self.max_height},"
|
||
f"min_height: {self.min_height}")
|
||
|
||
self.current_height = self.initial_height
|
||
self.target_height = self.initial_height
|
||
self.joy_flag.waist_height_command = self.current_height
|
||
self.joy_flag.walk_height_command = self.current_height
|
||
|
||
|
||
def joy_map_read(self, msg: Joy):
|
||
"""处理ROS Joy消息，更新手柄映射"""
|
||
with self.data_mutex:
|
||
if len(msg.axes) >= 12: # 确保有足够的轴数据
|
||
yunzhuo_map = YUNZHUOMap(
|
||
a=msg.axes[8] if len(msg.axes) > 8 else 0.0,
|
||
b=msg.axes[9] if len(msg.axes) > 9 else 0.0,
|
||
c=msg.axes[10] if len(msg.axes) > 10 else 0.0,
|
||
d=msg.axes[11] if len(msg.axes) > 11 else 0.0,
|
||
e=msg.axes[4] if len(msg.axes) > 4 else 0.0,
|
||
f=msg.axes[7] if len(msg.axes) > 7 else 0.0,
|
||
g=msg.axes[5] if len(msg.axes) > 5 else 0.0,
|
||
h=msg.axes[6] if len(msg.axes) > 6 else 0.0,
|
||
x1=msg.axes[3] if len(msg.axes) > 3 else 0.0,
|
||
x2=msg.axes[0] if len(msg.axes) > 1 else 0.0,
|
||
y1=msg.axes[2] if len(msg.axes) > 2 else 0.0,
|
||
y2=msg.axes[1] if len(msg.axes) > 0 else 0.0)
|
||
if yunzhuo_map != self.joy_map:
|
||
self.last_input_time = time.time()
|
||
self.joy_map = yunzhuo_map
|
||
|
||
def joy_flag_update(self):
|
||
"""根据手柄输入更新控制标志"""
|
||
with self.data_mutex:
|
||
fsm_command_updated = False
|
||
# 更新手柄启动标志
|
||
if self.joy_map.f == -1.0:
|
||
self.joy_flag.enable = False
|
||
else:
|
||
self.joy_flag.enable = True
|
||
# FSM状态切换命令
|
||
if self.joy_map.c == 1.0:
|
||
self.joy_flag.fsm_state_command = "gotoSTOP"
|
||
fsm_command_updated = True
|
||
else:
|
||
button_pressed_nums = self.check_button_pressed_nums(
|
||
self.joy_map)
|
||
if button_pressed_nums == 0:
|
||
if self.joy_map.d == 1.0:
|
||
self.joy_flag.fsm_state_command = "gotoZERO"
|
||
fsm_command_updated = True
|
||
elif self.joy_map.a == 1.0:
|
||
self.joy_flag.fsm_state_command = "gotoWALKAMP"
|
||
fsm_command_updated = True
|
||
# 获取walk速度命令
|
||
self.get_x_y_yaw_speed_command()
|
||
# 获取高度命令
|
||
self.get_walk_height_command()
|
||
if button_pressed_nums == 1:
|
||
if self.joy_map.e == -1.0:
|
||
#e上拨
|
||
if self.joy_map.a == 1.0:
|
||
self.joy_flag.fsm_state_command = "gotoBEYONDMIMIC"
|
||
fsm_command_updated = True
|
||
elif self.joy_map.d == 1.0:
|
||
self.joy_flag.fsm_state_command = "gotoBEYONDZERO"
|
||
fsm_command_updated = True
|
||
|
||
if fsm_command_updated:
|
||
self.last_fsm_command_time = time.time()
|
||
|
||
|
||
|
||
def get_joy_flag(self) -> ControlFlag:
|
||
"""获取当前手柄标志"""
|
||
with self.data_mutex:
|
||
return self.joy_flag
|
||
|
||
def get_last_input_time(self) -> float:
|
||
return self.last_input_time
|
||
|
||
def get_last_fsm_command_time(self) -> float:
|
||
return self.last_fsm_command_time
|
||
|
||
def init(self) -> int:
|
||
"""初始化手柄控制器"""
|
||
print("Joystick controller initialized")
|
||
return 0
|
||
|
||
def check_button_pressed_nums(self, joy_map: YUNZHUOMap) -> int:
|
||
"""检查按下的按钮数量"""
|
||
count = 0
|
||
if joy_map.e != 0.0:
|
||
count += 1
|
||
if joy_map.f != 0.0:
|
||
count += 1
|
||
if joy_map.g != 0.0:
|
||
count += 1
|
||
if joy_map.h != 0.0:
|
||
count += 1
|
||
return count
|
||
|
||
def get_x_y_yaw_speed_command(self):
|
||
"""获取当前速度命令"""
|
||
# 速度命令计算
|
||
self.joy_flag.y_speed_command = (self.joy_map.x1 * -self.max_y_speed +
|
||
self.y_command_offset)
|
||
|
||
# X速度 (前进/后退)
|
||
if self.joy_map.y1 >= 0:
|
||
self.joy_flag.x_speed_command = (
|
||
self.joy_map.y1 * self.max_x_plus_speed + self.x_command_offset
|
||
) # 前进快一点
|
||
else:
|
||
self.joy_flag.x_speed_command = self.joy_map.y1 * self.max_x_minus_speed # 后退慢一点
|
||
|
||
# 偏航速度
|
||
self.joy_flag.yaw_speed_command = (
|
||
self.joy_map.x2 * -self.max_yaw_speed + self.yaw_command_offset)
|
||
|
||
def get_walk_height_command(self):
|
||
"""获取当前高度命令"""
|
||
current_height_command = self.joy_flag.walk_height_command
|
||
deadzone_height = 0.5
|
||
# 高度命令计算
|
||
if self.joy_map.x2 >= deadzone_height:
|
||
# x2 下拨
|
||
self.joy_flag.walk_height_command += -self.joy_map.x2 * (
|
||
self.joy_flag.walk_height_command - self.min_height)
|
||
if self.joy_map.x2 <= -deadzone_height:
|
||
# x2 上拨
|
||
self.joy_flag.walk_height_command += -self.joy_map.x2 * (
|
||
self.max_height - self.joy_flag.walk_height_command)
|
||
|
||
# 1s中高度变化3cm, step= 0.03 / 100 hz = 0.0003
|
||
step = 0.03 / 100
|
||
self.joy_flag.walk_height_command = np.clip(
|
||
self.joy_flag.walk_height_command, current_height_command - step,
|
||
current_height_command + step)
|