451 lines
15 KiB
Python
451 lines
15 KiB
Python
"""
|
||
RL Control Plugin (Python Version)
|
||
Main ROS2 node for humanoid robot RL control system
|
||
"""
|
||
import math
|
||
import os
|
||
import queue
|
||
import threading
|
||
import time
|
||
import sys
|
||
|
||
import rclpy
|
||
import yaml
|
||
from rclpy.node import Node
|
||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||
# ROS messages
|
||
from sensor_msgs.msg import Joy
|
||
|
||
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||
|
||
from common.joystick import JoystickHumanoid, ControlFlag
|
||
from common.xbox_control import XBOXController
|
||
# Local imports
|
||
from common.robot_data import RobotData
|
||
from FSM.robot_fsm import get_robot_fsm
|
||
from FSM.fsm_base import FSMStateName
|
||
from common.robot_interface import get_robot_interface
|
||
from common.stdin_keyboard_control import KeyboardController
|
||
from udp_loopback.udp_fsm_controller import UDPFSMController
|
||
import functools
|
||
|
||
def timing_decorator(func):
|
||
"""
|
||
装饰器：记录函数执行时间
|
||
"""
|
||
@functools.wraps(func)
|
||
def wrapper(*args, **kwargs):
|
||
start_time = time.perf_counter()
|
||
result = func(*args, **kwargs)
|
||
end_time = time.perf_counter()
|
||
execution_time = end_time - start_time
|
||
print(f"[TIMING] {func.__name__} executed in {execution_time:.6f} seconds")
|
||
return result
|
||
return wrapper
|
||
|
||
|
||
class XMIGCSControlNode(Node):
|
||
"""xMIGCS控制节点Python版本"""
|
||
|
||
VALID_CONTROL_TOOLS = ("joystick", "xbox", "keyboard", "udp_loopback")
|
||
|
||
def __init__(self, debug=False):
|
||
super().__init__('xmigcs_control_node')
|
||
|
||
# 配置和参数
|
||
self.debug = debug
|
||
self.whole_joint_num = 35
|
||
self.pi = math.pi
|
||
self.rpm2rps = math.pi / 30.0
|
||
|
||
self.config = {}
|
||
|
||
# 加载配置
|
||
self._load_config()
|
||
|
||
# 初始化数据结构
|
||
self._init_data_structures()
|
||
|
||
# 机器人接口
|
||
self.robot_interface = get_robot_interface(self.robot_data,
|
||
self.config_file)
|
||
self.robot_interface.init(self) # 传入node实例
|
||
|
||
# 机器人FSM
|
||
self.robot_fsm = get_robot_fsm(
|
||
self.robot_data,
|
||
self.config,
|
||
)
|
||
|
||
# 初始化ROS接口
|
||
self._init_ros_interfaces()
|
||
|
||
# 初始化控制系统
|
||
self._init_control_system()
|
||
|
||
# 启动控制线程
|
||
self._start_control_thread()
|
||
|
||
def _load_config(self):
|
||
"""加载配置文件"""
|
||
# 获取包路径
|
||
self.config_file = os.path.join('.', 'config', 'dex_config.yaml')
|
||
|
||
with open(self.config_file, 'r') as f:
|
||
self.config = yaml.safe_load(f)
|
||
|
||
print(self.config)
|
||
|
||
# 获取控制器类型
|
||
raw_control_tool = self.config.get('control_tool', 'keyboard')
|
||
self.control_tools = self._normalize_control_tools(raw_control_tool)
|
||
# 提取关键配置参数
|
||
self.motor_num = self.config.get('motor_num')
|
||
self.dt = self.config.get('dt')
|
||
self.sim = self.config.get('sim')
|
||
|
||
# 检查当前用户名，如果是ubuntu则抛出异常
|
||
import getpass
|
||
user_name = getpass.getuser().lower()
|
||
if self.sim and user_name == 'ubuntu':
|
||
raise RuntimeError("On ubuntu user, sim must be set to false")
|
||
|
||
def _normalize_control_tools(self, raw_control_tool):
|
||
"""Normalize control_tool config to an ordered tool list."""
|
||
if isinstance(raw_control_tool, str):
|
||
normalized = raw_control_tool.replace("+", ",")
|
||
requested_tools = [
|
||
item.strip() for item in normalized.split(",") if item.strip()
|
||
]
|
||
elif isinstance(raw_control_tool, list):
|
||
requested_tools = [str(item).strip() for item in raw_control_tool if str(item).strip()]
|
||
else:
|
||
raise ValueError("control_tool must be a string or list")
|
||
|
||
if not requested_tools:
|
||
requested_tools = ["keyboard"]
|
||
|
||
deduped_tools = []
|
||
for tool_name in requested_tools:
|
||
if tool_name not in self.VALID_CONTROL_TOOLS:
|
||
raise ValueError(
|
||
f"Unsupported control tool '{tool_name}'. "
|
||
f"Expected one of {self.VALID_CONTROL_TOOLS}"
|
||
)
|
||
if tool_name not in deduped_tools:
|
||
deduped_tools.append(tool_name)
|
||
|
||
print(f"[control] active tools: {deduped_tools}")
|
||
return deduped_tools
|
||
|
||
def _init_data_structures(self):
|
||
"""初始化数据结构"""
|
||
# 机器人数据
|
||
self.robot_data = RobotData(self.motor_num, self.whole_joint_num)
|
||
self.robot_data.config_file_ = getattr(self, 'config_file', '')
|
||
|
||
# joysticks 消息队列
|
||
self.queue_joy_cmd = queue.Queue(maxsize=1)
|
||
self.queue_xbox_cmd = queue.Queue(maxsize=1)
|
||
self.control_flag = ControlFlag()
|
||
self.last_applied_fsm_command_time = 0.0
|
||
|
||
def _init_ros_interfaces(self):
|
||
"""初始化ROS接口（仅非电机相关）"""
|
||
qos_profile = QoSProfile(reliability=ReliabilityPolicy.RELIABLE,
|
||
history=HistoryPolicy.KEEP_LAST,
|
||
depth=5)
|
||
|
||
# 订阅者（非电机相关）
|
||
if "joystick" in self.control_tools:
|
||
self.sub_joy_cmd = self.create_subscription(
|
||
Joy, '/sbus_data', self._joy_callback, qos_profile)
|
||
if "xbox" in self.control_tools:
|
||
self.sub_xbox_cmd = self.create_subscription(
|
||
Joy, '/xbox_data', self._xbox_callback, qos_profile)
|
||
|
||
def _init_control_system(self):
|
||
"""初始化控制系统"""
|
||
|
||
# 手柄控制器
|
||
if "joystick" in self.control_tools:
|
||
self.joystick_humanoid = JoystickHumanoid()
|
||
self.joystick_humanoid.init()
|
||
|
||
# 键盘控制器
|
||
if "keyboard" in self.control_tools:
|
||
self.keyboard_controller = KeyboardController()
|
||
self.keyboard_controller.init()
|
||
# 如果使用键盘控制，启动键盘监听
|
||
self.keyboard_controller.start()
|
||
|
||
# UDP控制器
|
||
if "udp_loopback" in self.control_tools:
|
||
self.udp_fsm_controller = UDPFSMController()
|
||
self.udp_fsm_controller.init()
|
||
self.udp_fsm_controller.start()
|
||
|
||
# Xbox控制器
|
||
if "xbox" in self.control_tools:
|
||
self.xbox_controller = XBOXController()
|
||
self.xbox_controller.init()
|
||
|
||
# 控制标志
|
||
self.control_running = False
|
||
self.control_thread = None
|
||
|
||
def _start_control_thread(self):
|
||
"""启动控制线程"""
|
||
self.control_running = True
|
||
self.control_thread = threading.Thread(target=self._rl_control_loop,
|
||
daemon=True)
|
||
self.control_thread.start()
|
||
self.get_logger().info("Control thread started")
|
||
|
||
def _rl_control_loop(self):
|
||
"""主控制循环"""
|
||
self.get_logger().info("RL control loop starting...")
|
||
|
||
# 初始化时间戳
|
||
time_passed = 0.0
|
||
|
||
# 处理手柄数据
|
||
self._process_controller_data()
|
||
# 更新机器人数据
|
||
self._update_robot_data(self.control_flag, time_passed)
|
||
|
||
while self.control_running and rclpy.ok():
|
||
|
||
# 运行FSM
|
||
loop_start = time.perf_counter()
|
||
self.robot_fsm.run_fsm(self.robot_data.control_flag)
|
||
|
||
# 发布控制命令
|
||
self.robot_interface.update_param(current_state=self.robot_fsm.get_current_state())
|
||
self._send_control_commands(self.robot_data.control_flag)
|
||
|
||
# 更新时间戳
|
||
time_passed += self.dt
|
||
|
||
# 处理手柄数据
|
||
self._process_controller_data()
|
||
|
||
# 更新机器人数据
|
||
self._update_robot_data(self.control_flag, time_passed)
|
||
|
||
# 控制频率
|
||
self._precise_sleep_until(loop_start + self.dt)
|
||
# print(
|
||
# f"current control freq: {1/(time.perf_counter() - loop_start):.2f} Hz"
|
||
# )
|
||
|
||
|
||
self.get_logger().info("RL control loop ended")
|
||
|
||
def _precise_sleep_until(self, target_time):
|
||
"""精确睡眠到目标时间"""
|
||
current_time = time.perf_counter()
|
||
sleep_time = target_time - current_time
|
||
|
||
if sleep_time <= 0:
|
||
return # 已经超时，立即返回
|
||
|
||
# 分级睡眠策略
|
||
if sleep_time > 0.003: # 3ms以上使用混合睡眠
|
||
# 先睡眠大部分时间
|
||
time.sleep(sleep_time * 0.9)
|
||
# 剩余时间忙等待
|
||
while time.perf_counter() < target_time:
|
||
pass
|
||
else: # 3ms以内纯忙等待
|
||
while time.perf_counter() < target_time:
|
||
pass
|
||
|
||
# def _wait_for_start_signal(self):
|
||
# """等待启动信号"""
|
||
# start_file = "/tmp/rl_start_signal"
|
||
# self.get_logger().info("Waiting for start signal...")
|
||
# self.get_logger().info("Run: touch /tmp/rl_start_signal")
|
||
|
||
# # 删除可能存在的旧文件
|
||
# if os.path.exists(start_file):
|
||
# os.remove(start_file)
|
||
|
||
# # 等待启动文件出现
|
||
# while not os.path.exists(start_file) and rclpy.ok():
|
||
# time.sleep(0.5)
|
||
|
||
# self.get_logger().info("Start signal received, beginning RL control!")
|
||
|
||
# @timing_decorator
|
||
def _process_controller_data(self):
|
||
source_flags = []
|
||
|
||
# 处理控制器输入
|
||
if "joystick" in self.control_tools:
|
||
# 处理手柄输入
|
||
while not self.queue_joy_cmd.empty():
|
||
try:
|
||
msg = self.queue_joy_cmd.get_nowait()
|
||
self.joystick_humanoid.joy_map_read(msg)
|
||
self.joystick_humanoid.joy_flag_update()
|
||
break
|
||
except queue.Empty:
|
||
break
|
||
source_flags.append((
|
||
"joystick",
|
||
self.joystick_humanoid.get_joy_flag(),
|
||
self.joystick_humanoid.get_last_input_time(),
|
||
self.joystick_humanoid.get_last_fsm_command_time(),
|
||
))
|
||
|
||
if "xbox" in self.control_tools:
|
||
while not self.queue_xbox_cmd.empty():
|
||
try:
|
||
msg = self.queue_xbox_cmd.get_nowait()
|
||
self.xbox_controller.xbox_map_read(msg)
|
||
self.xbox_controller.xbox_flag_update()
|
||
break
|
||
except queue.Empty:
|
||
break
|
||
source_flags.append((
|
||
"xbox",
|
||
self.xbox_controller.get_xbox_flag(),
|
||
self.xbox_controller.get_last_input_time(),
|
||
self.xbox_controller.get_last_fsm_command_time(),
|
||
))
|
||
|
||
if "keyboard" in self.control_tools:
|
||
self.keyboard_controller.update_flag()
|
||
source_flags.append((
|
||
"keyboard",
|
||
self.keyboard_controller.get_keyboard_flag(),
|
||
self.keyboard_controller.get_last_input_time(),
|
||
self.keyboard_controller.get_last_fsm_command_time(),
|
||
))
|
||
|
||
if "udp_loopback" in self.control_tools:
|
||
self.udp_fsm_controller.update_flag()
|
||
source_flags.append((
|
||
"udp_loopback",
|
||
self.udp_fsm_controller.get_udp_flag(),
|
||
self.udp_fsm_controller.get_last_input_time(),
|
||
self.udp_fsm_controller.get_last_fsm_command_time(),
|
||
))
|
||
|
||
source_name, active_flag = self._select_control_flag(source_flags)
|
||
flag = self._copy_control_flag(active_flag)
|
||
flag.fsm_state_command = self._select_fsm_command(source_flags)
|
||
print('*' * 30 + f"current flag source={source_name}: {flag}" + '*' * 30)
|
||
self.control_flag = flag
|
||
|
||
def _select_control_flag(self, source_flags):
|
||
"""Select the most recently active controller."""
|
||
if not source_flags:
|
||
print("[ERROR] No control tool specified")
|
||
return "none", ControlFlag()
|
||
|
||
selected_source = source_flags[0]
|
||
for candidate in source_flags[1:]:
|
||
if candidate[2] > selected_source[2]:
|
||
selected_source = candidate
|
||
|
||
return selected_source[0], selected_source[1]
|
||
|
||
def _copy_control_flag(self, flag: ControlFlag) -> ControlFlag:
|
||
flag_copy = type(flag)()
|
||
flag_copy.__dict__.update(flag.__dict__)
|
||
return flag_copy
|
||
|
||
def _select_fsm_command(self, source_flags) -> str:
|
||
latest_source = None
|
||
for candidate in source_flags:
|
||
if latest_source is None or candidate[3] > latest_source[3]:
|
||
latest_source = candidate
|
||
|
||
if latest_source is not None and latest_source[3] > self.last_applied_fsm_command_time:
|
||
self.last_applied_fsm_command_time = latest_source[3]
|
||
return latest_source[1].fsm_state_command
|
||
|
||
return self._get_hold_fsm_command()
|
||
|
||
def _get_hold_fsm_command(self) -> str:
|
||
state_to_command = {
|
||
FSMStateName.STOP: "gotoSTOP",
|
||
FSMStateName.ZERO: "gotoZERO",
|
||
FSMStateName.WALKAMP: "gotoWALKAMP",
|
||
FSMStateName.MYPOLICY: "gotoMYPOLICY",
|
||
FSMStateName.XSIMRUN: "gotoXSIMRUN",
|
||
}
|
||
current_state = self.robot_fsm.get_current_state()
|
||
return state_to_command.get(current_state, self.control_flag.fsm_state_command)
|
||
|
||
# @timing_decorator
|
||
def _update_robot_data(self, flag: ControlFlag, time_passed: float):
|
||
"""更新机器人数据"""
|
||
self.robot_interface.update_robot_data(flag, time_passed)
|
||
|
||
# @timing_decorator
|
||
def _send_control_commands(self, flag: ControlFlag):
|
||
"""发布控制命令"""
|
||
# 通过robot_interface发布控制命令
|
||
self.robot_interface.send_motor_commands(flag)
|
||
|
||
def _joy_callback(self, msg):
|
||
"""云卓手柄输入回调"""
|
||
try:
|
||
self.queue_joy_cmd.put_nowait(msg)
|
||
except queue.Full:
|
||
try:
|
||
self.queue_joy_cmd.get_nowait() # 移除旧数据
|
||
self.queue_joy_cmd.put_nowait(msg) # 加入新数据
|
||
except:
|
||
pass # 如果仍然无法加入，忽略
|
||
|
||
def _xbox_callback(self, msg):
|
||
"""xbox手柄输入回调"""
|
||
try:
|
||
self.queue_xbox_cmd.put_nowait(msg)
|
||
except queue.Full:
|
||
try:
|
||
self.queue_xbox_cmd.get_nowait() # 移除旧数据
|
||
self.queue_xbox_cmd.put_nowait(msg) # 加入新数据
|
||
except:
|
||
pass # 如果仍然无法加入，忽略
|
||
|
||
def destroy_node(self):
|
||
"""节点销毁"""
|
||
self.control_running = False
|
||
# # 先停止键盘控制器（重要！）
|
||
if hasattr(self,
|
||
'keyboard_controller') and "keyboard" in self.control_tools:
|
||
self.keyboard_controller.stop()
|
||
if hasattr(self,
|
||
'udp_fsm_controller') and "udp_loopback" in self.control_tools:
|
||
self.udp_fsm_controller.stop()
|
||
|
||
if self.control_thread and self.control_thread.is_alive():
|
||
self.control_thread.join(timeout=1.0)
|
||
|
||
super().destroy_node()
|
||
|
||
|
||
def main(args=None):
|
||
"""主函数"""
|
||
rclpy.init(args=args)
|
||
node = None
|
||
try:
|
||
node = XMIGCSControlNode(debug=False)
|
||
rclpy.spin(node)
|
||
except KeyboardInterrupt:
|
||
pass
|
||
finally:
|
||
if 'node' in locals() and node is not None:
|
||
node.destroy_node()
|
||
rclpy.shutdown()
|
||
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main()
|