meiqi/tienkung-szu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

xbox_udp链路

Browse Source
This commit is contained in:
meiqi
2026-03-27 21:57:52 +08:00
parent c45245038f
commit 4788c0885a
10 changed files with 746 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@@ -0,0 +1,3 @@
/TienKung_URDF
/TienKung-Lab
/xGMR

38
Deploy_Tienkung/common/xbox_control.py
View File

@@ -69,7 +69,6 @@ class XBOXController:
# smoothing
self.height_step = 0.05
self._load_config()
# default button map indices (can be overridden in config)
self.button_map = {
'a': 0, 'b': 1, 'x': 2, 'y': 3,
@@ -84,6 +83,8 @@ class XBOXController:
'dpad_h': 6, 'dpad_v': 7
}
self._load_config()
def _load_config(self):
try:
config_path = os.path.join('.', 'config', 'dex_config.yaml')
@@ -91,22 +92,24 @@ class XBOXController:
cfg = yaml.safe_load(f) or {}
xbox_cfg = cfg.get('xbox', {})
# override button_map if provided
# bm = xbox_cfg.get('button_map')
# if isinstance(bm, dict):
# for k, v in bm.items():
# try:
# self.button_map[k] = int(v)
# except Exception:
# pass
bm = xbox_cfg.get('button_map')
if isinstance(bm, dict):
for k, v in bm.items():
if k in self.button_map:
try:
self.button_map[k] = int(v)
except Exception:
pass
# # override axis_map if provided
# am = xbox_cfg.get('axis_map')
# if isinstance(am, dict):
# for k, v in am.items():
# try:
# self.axis_map[k] = int(v)
# except Exception:
# pass
# override axis_map if provided
am = xbox_cfg.get('axis_map')
if isinstance(am, dict):
for k, v in am.items():
if k in self.axis_map:
try:
self.axis_map[k] = int(v)
except Exception:
pass
self.initial_height = xbox_cfg.get('initial_height', 0.89)
self.forward_command_offset = xbox_cfg.get('forward_command_offset', 0.0)
@@ -155,6 +158,9 @@ class XBOXController:
# c -> gotoSTOP
if self.map.y == 1:
self.flag.fsm_state_command = 'gotoSTOP'
# a -> gotoWALKAMP
elif self.map.a == 1:
self.flag.fsm_state_command = 'gotoWALKAMP'
# h -> gotoDH (Left trigger + A)
# v -> gotoBEYONDMIMIC (Left trigger + home)
elif self.map.l_trigger < -0.5 and self.map.home == 1:

2
Deploy_Tienkung/policy/stop/fsm_stop.py
View File

@@ -80,6 +80,8 @@ class FSMStateStop(FSMState):
return FSMStateName.STOP
elif flag.fsm_state_command == "gotoZERO":
return FSMStateName.ZERO
elif flag.fsm_state_command == "gotoWALKAMP":
return FSMStateName.WALKAMP
elif flag.fsm_state_command == "gotoMYPOLICY":
return FSMStateName.MYPOLICY
elif flag.fsm_state_command == "gotoXSIMRUN":

82
Deploy_Tienkung/policy/xsim_run/config/xsim_run.yaml Normal file
View File

@@ -0,0 +1,82 @@
model_path: "../mypolicy/model/policy.onnx"
motor_num: 29
actions_size: 23
dt: 0.01
warm_start_time: 0.0
command_clip: 1.0
sim:
mujoco_timestep: 0.005
joint_names: [
hip_pitch_l_joint, hip_pitch_r_joint, waist_yaw_joint,
hip_roll_l_joint, hip_roll_r_joint, waist_roll_joint,
hip_yaw_l_joint, hip_yaw_r_joint, waist_pitch_joint,
knee_pitch_l_joint, knee_pitch_r_joint,
shoulder_pitch_l_joint, shoulder_pitch_r_joint,
ankle_pitch_l_joint, ankle_pitch_r_joint,
shoulder_roll_l_joint, shoulder_roll_r_joint,
ankle_roll_l_joint, ankle_roll_r_joint,
shoulder_yaw_l_joint, shoulder_yaw_r_joint,
elbow_pitch_l_joint, elbow_pitch_r_joint
]
control:
action_scale: 0.25
decimation: 2
gait:
gait_air_ratio_l: 0.6
gait_air_ratio_r: 0.6
gait_phase_offset_l: 0.6
gait_phase_offset_r: 0.1
gait_cycle: 0.64
normalization:
clip_scales:
clip_observations: 100.0
clip_actions: 100.0
size:
num_hist: 10
observations_size: 84
gains:
kp: [
300.0, 300.0, 400.0,
300.0, 300.0, 400.0,
150.0, 150.0, 400.0,
350.0, 350.0,
150.0, 150.0,
30.0, 30.0,
50.0, 50.0,
16.8, 16.8,
50.0, 50.0,
150.0, 150.0
]
kd: [
10.0, 10.0, 5.0,
10.0, 10.0, 10.0,
5.0, 5.0, 10.0,
10.0, 10.0,
7.5, 7.5,
2.5, 2.5,
2.5, 2.5,
1.4, 1.4,
2.5, 2.5,
5.0, 5.0
]
init_state:
default_joint_angles: [
0.0, 0.0, 0.0,
-0.5, -0.5, 0.0,
0.0, 0.0, 0.0,
1.0, 1.0,
0.0, 0.0,
-0.5, -0.5,
0.2, -0.2,
0.0, 0.0,
0.0, 0.0,
-0.3, -0.3
]

303
Deploy_Tienkung/policy/xsim_run/fsm_xsim_run.py Normal file
View File

@@ -0,0 +1,303 @@
"""
FSM state implementation for an xSIM MuJoCo run policy that follows the
TienKung-Lab sim2sim observation/action flow more closely.
"""
import os
import numpy as np
import onnxruntime as ort
import yaml
from scipy.spatial.transform import Rotation
from FSM.fsm_base import FSMState, FSMStateName
from common.joystick import ControlFlag
from common.robot_data import RobotData
class FSMStateXSIMRUN(FSMState):
"""Direct-position run policy for xSIM MuJoCo."""
def __init__(self, robot_data: RobotData):
super().__init__(robot_data)
self.current_state_name = FSMStateName.XSIMRUN
self.log_prefix = "FSMStateXSIMRUN"
current_dir = os.path.dirname(os.path.abspath(__file__))
config_path = os.path.join(current_dir, "config", "xsim_run.yaml")
with open(config_path, "r", encoding="utf-8") as f:
policy_config = yaml.safe_load(f)
self.action_num_ = int(policy_config["actions_size"])
self.motor_num_ = int(policy_config["motor_num"])
self.dt_ = float(policy_config["dt"])
self.command_clip_ = float(policy_config.get("command_clip", 1.0))
size_config = policy_config.get("size", {})
self.num_hist_ = int(size_config["num_hist"])
self.obs_size_ = int(size_config["observations_size"])
control_config = policy_config.get("control", {})
self.action_scale_ = float(control_config["action_scale"])
self.decimation_ = int(control_config["decimation"])
self.warm_start_time_ = float(
control_config.get(
"warm_start_time",
policy_config.get("warm_start_time", 0.0),
)
)
sim_config = policy_config.get("sim", {})
self.mujoco_timestep_ = float(sim_config.get("mujoco_timestep", 0.005))
self.policy_period_ = self.dt_ * self.decimation_
gait_config = policy_config.get("gait", {})
self.gait_cycle_ = float(gait_config["gait_cycle"])
self.phase_ratio_ = np.array(
[
gait_config["gait_air_ratio_l"],
gait_config["gait_air_ratio_r"],
],
dtype=np.float32,
)
self.phase_offset_ = np.array(
[
gait_config["gait_phase_offset_l"],
gait_config["gait_phase_offset_r"],
],
dtype=np.float32,
)
norm_config = policy_config.get("normalization", {})
clip_config = norm_config.get("clip_scales", {})
self.clip_obs_ = float(clip_config.get("clip_observations", 100.0))
self.clip_act_ = float(clip_config.get("clip_actions", 100.0))
self.default_angles_lab_ = np.array(
policy_config["init_state"]["default_joint_angles"],
dtype=np.float32,
)
self.stiffness_lab_ = np.array(policy_config["gains"]["kp"], dtype=np.float32)
self.damping_lab_ = np.array(policy_config["gains"]["kd"], dtype=np.float32)
model_rel_path = policy_config["model_path"]
self.model_path_ = os.path.normpath(os.path.join(current_dir, model_rel_path))
self._init_onnx_session()
# sim2sim.py uses policy output in Isaac order and then maps to MuJoCo order.
self.mujoco_to_policy_idx_ = np.array(
[0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 15, 19, 4, 10, 16, 20, 5, 11, 17, 21, 18, 22],
dtype=int,
)
self.policy_to_mujoco_idx_ = np.array(
[0, 3, 6, 9, 13, 17, 1, 4, 7, 10, 14, 18, 2, 5, 8, 11, 15, 19, 21, 12, 16, 20, 22],
dtype=int,
)
# RobotData stores 29 joints in leg -> waist -> arm order.
self.mujoco_control_indices_ = np.array(
[
0, 1, 2, 3, 4, 5,
6, 7, 8, 9, 10, 11,
12, 13, 14,
15, 16, 17, 18,
22, 23, 24, 25,
],
dtype=int,
)
self.default_angles_mujoco23_ = self.default_angles_lab_[self.policy_to_mujoco_idx_]
self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
self.obs_history_ = np.zeros_like(self.observations_)
self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
self.current_gait_ = np.zeros(6, dtype=np.float32)
self.hold_pose_29_ = np.zeros(self.motor_num_, dtype=np.float32)
self._warm_start_pose_29_ = np.zeros(self.motor_num_, dtype=np.float32)
self._first_obs = True
self._policy_step_counter = 0
self.waiting_for_motion_ = True
self.motion_threshold_ = 1e-3
if self.warm_start_time_ > 0 and self.policy_period_ > 0:
self._warm_start_steps = max(1, int(self.warm_start_time_ / self.policy_period_))
else:
self._warm_start_steps = 0
self._warmup_inference_counter = 0
self.kp_29_ = np.zeros(self.motor_num_, dtype=np.float32)
self.kd_29_ = np.zeros(self.motor_num_, dtype=np.float32)
for lab_idx, mj_idx in enumerate(self.mujoco_control_indices_[self.policy_to_mujoco_idx_]):
self.kp_29_[mj_idx] = self.stiffness_lab_[lab_idx]
self.kd_29_[mj_idx] = self.damping_lab_[lab_idx]
def _init_onnx_session(self) -> None:
options = ort.SessionOptions()
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
options.intra_op_num_threads = 1
options.inter_op_num_threads = 1
self.ort_session_ = ort.InferenceSession(
self.model_path_,
options,
providers=["CPUExecutionProvider"],
)
print(f"[{self.log_prefix}] ONNX model loaded: {self.model_path_}")
def on_enter(self):
self.observations_.fill(0.0)
self.obs_history_.fill(0.0)
self.actions_.fill(0.0)
self.last_actions_.fill(0.0)
self.current_gait_.fill(0.0)
self._first_obs = True
self._policy_step_counter = 0
self._warmup_inference_counter = 0
self.waiting_for_motion_ = True
current_q = self.robot_data_.get_joint_pos().copy()
self.hold_pose_29_ = current_q
self._warm_start_pose_29_ = current_q
base = self.robot_data_.q_d_.shape[0] - self.motor_num_
self.robot_data_.q_d_[base:base + self.motor_num_] = current_q
self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
print(f"[{self.log_prefix}] enter")
def run(self, flag: ControlFlag):
walk_cmd = np.clip(
np.array(self.robot_data_.get_walk_cmd(), dtype=np.float32),
-self.command_clip_,
self.command_clip_,
)
base = self.robot_data_.q_d_.shape[0] - self.motor_num_
if self.waiting_for_motion_:
if np.max(np.abs(walk_cmd)) <= self.motion_threshold_:
self.robot_data_.q_d_[base:base + self.motor_num_] = self.hold_pose_29_
self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
return
self.waiting_for_motion_ = False
self._warm_start_pose_29_ = self.robot_data_.get_joint_pos().copy()
print(f"[{self.log_prefix}] motion command detected: {walk_cmd}")
if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
self.current_gait_ = self._compute_gait_features()
self.compute_observation(walk_cmd)
self.compute_actions()
target_mujoco23 = (
self.actions_[self.policy_to_mujoco_idx_] * self.action_scale_
+ self.default_angles_mujoco23_
)
target_q_29 = self.hold_pose_29_.copy()
target_q_29[self.mujoco_control_indices_] = target_mujoco23
commanded_q_29 = target_q_29
if self._warm_start_steps > 0 and self._warmup_inference_counter < self._warm_start_steps:
self._warmup_inference_counter += 1
blend = self._warmup_inference_counter / float(self._warm_start_steps)
commanded_q_29 = (1.0 - blend) * self._warm_start_pose_29_ + blend * target_q_29
self.robot_data_.q_d_[base:base + self.motor_num_] = commanded_q_29
self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
self.last_actions_[:] = self.actions_
def _compute_gait_features(self) -> np.ndarray:
t = self._policy_step_counter * self.policy_period_ / self.gait_cycle_
gait_phase = (t + self.phase_offset_) % 1.0
self._policy_step_counter += 1
return np.concatenate(
[
np.sin(2.0 * np.pi * gait_phase),
np.cos(2.0 * np.pi * gait_phase),
self.phase_ratio_,
],
axis=0,
).astype(np.float32)
def compute_observation(self, walk_cmd: np.ndarray):
if np.linalg.norm(self.robot_data_.imu_quat_) > 0.0:
q_wxyz = self.robot_data_.imu_quat_.astype(np.float32)
q_xyzw = np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]], dtype=np.float32)
else:
roll = float(self.robot_data_.imu_data_[2])
pitch = float(self.robot_data_.imu_data_[1])
yaw = float(self.robot_data_.imu_data_[0])
q_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
q_xyzw = np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]], dtype=np.float32)
gravity = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.0, 0.0, -1.0], dtype=np.float32))
q_29 = self.robot_data_.get_joint_pos()
dq_29 = self.robot_data_.get_joint_vel()
q_23 = q_29[self.mujoco_control_indices_]
dq_23 = dq_29[self.mujoco_control_indices_]
proprio = np.concatenate(
[
self.robot_data_.get_angular_velocity(),
gravity,
walk_cmd,
(q_23 - self.default_angles_mujoco23_)[self.mujoco_to_policy_idx_],
dq_23[self.mujoco_to_policy_idx_],
np.clip(self.last_actions_, -self.clip_act_, self.clip_act_),
self.current_gait_,
],
axis=0,
).astype(np.float32)
if self._first_obs:
for i in range(self.num_hist_):
start = i * self.obs_size_
self.obs_history_[start:start + self.obs_size_] = proprio
self._first_obs = False
else:
self.obs_history_ = np.roll(self.obs_history_, -self.obs_size_)
self.obs_history_[-self.obs_size_:] = proprio
self.observations_ = np.clip(self.obs_history_, -self.clip_obs_, self.clip_obs_)
def compute_actions(self):
input_name = self.ort_session_.get_inputs()[0].name
input_data = self.observations_.reshape(1, -1).astype(np.float32)
outputs = self.ort_session_.run(None, {input_name: input_data})
self.actions_[:] = np.clip(outputs[0][0][: self.action_num_], -self.clip_act_, self.clip_act_)
def on_exit(self):
print(f"[{self.log_prefix}] exit")
def check_transition(self, flag: ControlFlag) -> FSMStateName:
if flag.fsm_state_command == "gotoSTOP":
return FSMStateName.STOP
if flag.fsm_state_command == "gotoZERO":
return FSMStateName.ZERO
if flag.fsm_state_command == "gotoWALKAMP":
return FSMStateName.WALKAMP
if flag.fsm_state_command == "gotoMYPOLICY":
return FSMStateName.MYPOLICY
if flag.fsm_state_command == "gotoXSIMRUN":
return FSMStateName.XSIMRUN
return None
@staticmethod
def euler_to_quaternion_scipy(roll, pitch, yaw, degrees=False):
r = Rotation.from_euler("xyz", [roll, pitch, yaw], degrees=degrees)
q_xyzw = r.as_quat()
return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]], dtype=np.float32)
@staticmethod
def quat_rotate_inverse_numpy(q_xyzw, v):
q_w = q_xyzw[3]
q_v = q_xyzw[:3]
a = v * (2.0 * q_w * q_w - 1.0)
b = np.cross(q_v, v) * (2.0 * q_w)
c = q_v * (2.0 * np.dot(q_v, v))
return a - b + c

55
Deploy_Tienkung/udp_loopback/README.md
View File

@@ -5,6 +5,7 @@
包含：
- `udp_keyboard_sender.py`：从终端读取按键，编码 UDP 报文并发送
- `udp_xbox_sender.py`：订阅 `/xbox_data`，把 Xbox 摇杆/按键转成 UDP 报文
- `udp_loopback_node.py`：接收 UDP 报文，解码事件并计算目标值
- `protocol.py`：自定义协议和状态结构
- `config/udp_loopback.yaml`：本地测试配置
@@ -40,6 +41,56 @@ cd /home/meiqi/tienkung/Deploy_Tienkung
python3 udp_loopback/udp_keyboard_sender.py
```
如果改成 Xbox 经 UDP 转发，则启动方式是：
1. 把 [dex_config.yaml](/home/meiqi/tienkung/Deploy_Tienkung/config/dex_config.yaml) 里的 `control_tool` 改成 `udp_loopback`
2. 启动控制节点：
```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 rl_control_node_sim.py
```
3. 启动 MuJoCo：
```bash
cd /home/meiqi/tienkung/xSIM_MUJOCO
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 scripts/simulator_view_asyn.py -m evt2
```
4. 启动手柄节点：
```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
ros2 run joy joy_node --ros-args -r joy:=/xbox_data
```
5. 启动 UDP Xbox 转发：
```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 udp_loopback/udp_xbox_sender.py
```
默认按键映射：
- `A -> mode_stride -> gotoWALKAMP`
- `X -> pose_home -> gotoZERO`
- `Y -> pose_hold -> gotoSTOP`
- `B -> mode_dash -> gotoMYPOLICY`
- `START -> trim_reset`
- 左摇杆 Y -> 连续前后速度
- 左摇杆 X -> 连续横移速度
- 右摇杆 X -> 连续转向速度
- 十字键左右 -> 高度增减
此时 UDP 接收结果会在接收侧被映射回现有 FSM 命令：
- `pose_home -> gotoZERO`
@@ -60,6 +111,10 @@ python3 udp_loopback/udp_keyboard_sender.py
- `sway_right`
- `spin_left`
- `spin_right`
- `set_surge`
- `set_sway`
- `set_spin`
- `set_lift`
- `lift_up`
- `lift_down`
- `trim_reset`

8
Deploy_Tienkung/udp_loopback/config/udp_loopback.yaml
View File

@@ -5,6 +5,14 @@ sender:
target_port: 31000
source_tag: local_keys
xbox_sender:
joy_topic: /xbox_data
source_tag: xbox_udp
deadzone: 0.10
analog_epsilon: 0.01
dpad_threshold: 0.50
trigger_pressed_threshold: -0.50
receiver:
listen_host: 127.0.0.1
listen_port: 31000

2
Deploy_Tienkung/udp_loopback/protocol.py
View File

@@ -10,7 +10,7 @@ from typing import Any, Dict
@dataclass
class InputEnvelope:
"""Small UDP payload carrying one encoded keyboard event."""
"""Small UDP payload carrying one encoded input event."""
seq_id: int
event_code: str

16
Deploy_Tienkung/udp_loopback/udp_fsm_controller.py
View File

@@ -177,6 +177,22 @@ class UDPFSMController:
self.motion_frame.spin_goal = min(
self.max_spin, self.motion_frame.spin_goal + self.spin_step
)
elif event_code == "set_surge":
self.motion_frame.surge_goal = max(
-self.max_surge, min(self.max_surge, packet.drive_value)
)
elif event_code == "set_sway":
self.motion_frame.sway_goal = max(
-self.max_sway, min(self.max_sway, packet.drive_value)
)
elif event_code == "set_spin":
self.motion_frame.spin_goal = max(
-self.max_spin, min(self.max_spin, packet.drive_value)
)
elif event_code == "set_lift":
self.motion_frame.lift_goal = max(
self.min_lift, min(self.max_lift, packet.drive_value)
)
elif event_code == "lift_up":
self.motion_frame.lift_goal = min(
self.max_lift, self.motion_frame.lift_goal + self.lift_step

253
Deploy_Tienkung/udp_loopback/udp_xbox_sender.py Normal file
View File

@@ -0,0 +1,253 @@
"""ROS2 Joy -> localhost UDP bridge for Xbox control."""
from __future__ import annotations
import socket
from pathlib import Path
from typing import Dict
import rclpy
from rclpy.node import Node
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import Joy
import yaml
try:
from .protocol import InputEnvelope
except ImportError: # pragma: no cover - direct script execution fallback
from protocol import InputEnvelope
class UDPXboxSender(Node):
"""Subscribe to Joy messages and forward them as UDP loopback events."""
def __init__(self) -> None:
super().__init__("udp_xbox_sender")
self.config: Dict[str, object] = {}
self.seq_id = 0
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.last_buttons: Dict[str, int] = {}
self.last_dpad_h = 0.0
self._load_config()
qos_profile = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST,
depth=10,
)
self.subscription = self.create_subscription(
Joy, self.joy_topic, self._joy_callback, qos_profile
)
self.get_logger().info(
f"Forwarding {self.joy_topic} -> udp://{self.target_host}:{self.target_port}"
)
self.get_logger().info(
"Buttons: A=WALKAMP X=ZERO Y=STOP B=MYPOLICY START=reset"
)
def destroy_node(self) -> bool:
if self.socket is not None:
self.socket.close()
return super().destroy_node()
def _load_config(self) -> None:
udp_config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
main_config_path = Path(__file__).resolve().parents[1] / "config" / "dex_config.yaml"
with udp_config_path.open("r", encoding="utf-8") as file:
udp_config = yaml.safe_load(file) or {}
with main_config_path.open("r", encoding="utf-8") as file:
main_config = yaml.safe_load(file) or {}
sender_cfg = udp_config.get("sender", {})
xbox_sender_cfg = udp_config.get("xbox_sender", {})
xbox_cfg = main_config.get("xbox", {})
self.target_host = sender_cfg.get("target_host", "127.0.0.1")
self.target_port = int(sender_cfg.get("target_port", 31000))
self.source_tag = xbox_sender_cfg.get("source_tag", "xbox_udp")
self.joy_topic = xbox_sender_cfg.get("joy_topic", "/xbox_data")
self.deadzone = float(xbox_sender_cfg.get("deadzone", 0.10))
self.analog_epsilon = float(xbox_sender_cfg.get("analog_epsilon", 0.01))
self.dpad_threshold = float(xbox_sender_cfg.get("dpad_threshold", 0.50))
self.trigger_pressed_threshold = float(
xbox_sender_cfg.get("trigger_pressed_threshold", -0.50)
)
self.forward_command_offset = float(
xbox_cfg.get("forward_command_offset", 0.0)
)
self.lateral_command_offset = float(
xbox_cfg.get("lateral_command_offset", 0.0)
)
self.rotation_command_offset = float(
xbox_cfg.get("rotation_command_offset", 0.0)
)
self.button_map = {
"a": 0,
"b": 1,
"x": 2,
"y": 3,
"lb": 4,
"rb": 5,
"select": 6,
"start": 7,
"home": 8,
}
self.axis_map = {
"lx": 0,
"ly": 1,
"l_trigger": 2,
"rx": 3,
"ry": 4,
"r_trigger": 5,
"dpad_h": 6,
"dpad_v": 7,
}
self._merge_mapping(self.button_map, xbox_cfg.get("button_map"))
self._merge_mapping(self.axis_map, xbox_cfg.get("axis_map"))
self._merge_mapping(self.button_map, xbox_sender_cfg.get("button_map"))
self._merge_mapping(self.axis_map, xbox_sender_cfg.get("axis_map"))
def _merge_mapping(self, target: Dict[str, int], override: object) -> None:
if not isinstance(override, dict):
return
for name, index in override.items():
if name in target:
try:
target[name] = int(index)
except (TypeError, ValueError):
pass
def _joy_callback(self, msg: Joy) -> None:
axes = list(msg.axes) + [0.0] * 16
buttons = list(msg.buttons) + [0] * 32
state = {
"a": self._button_value(buttons, "a"),
"b": self._button_value(buttons, "b"),
"x": self._button_value(buttons, "x"),
"y": self._button_value(buttons, "y"),
"start": self._button_value(buttons, "start"),
"home": self._button_value(buttons, "home"),
"lx": self._axis_value(axes, "lx"),
"ly": self._axis_value(axes, "ly"),
"rx": self._axis_value(axes, "rx"),
"l_trigger": self._axis_value(axes, "l_trigger"),
"dpad_h": self._axis_value(axes, "dpad_h"),
}
self._send_mode_events(state)
self._send_trim_event(state)
self._send_lift_events(state)
self._send_analog_events(state)
self.last_buttons = {
name: int(state[name]) for name in ("a", "b", "x", "y", "start", "home")
}
self.last_dpad_h = float(state["dpad_h"])
def _button_value(self, buttons: list[int], name: str) -> int:
index = self.button_map[name]
return int(buttons[index]) if index < len(buttons) else 0
def _axis_value(self, axes: list[float], name: str) -> float:
index = self.axis_map[name]
return float(axes[index]) if index < len(axes) else 0.0
def _send_mode_events(self, state: Dict[str, float]) -> None:
if self._rising_edge(state, "y"):
self._send_event("pose_hold", "y")
elif self._rising_edge(state, "x"):
self._send_event("pose_home", "x")
elif self._rising_edge(state, "a"):
self._send_event("mode_stride", "a")
elif self._rising_edge(state, "b"):
self._send_event("mode_dash", "b")
elif (
self._rising_edge(state, "home")
and state["l_trigger"] < self.trigger_pressed_threshold
):
self._send_event("mode_xrun", "home")
def _send_trim_event(self, state: Dict[str, float]) -> None:
if self._rising_edge(state, "start"):
self._send_event("trim_reset", "start")
def _send_lift_events(self, state: Dict[str, float]) -> None:
dpad_h = float(state["dpad_h"])
if dpad_h <= -self.dpad_threshold and self.last_dpad_h > -self.dpad_threshold:
self._send_event("lift_up", "dpad_left")
elif dpad_h >= self.dpad_threshold and self.last_dpad_h < self.dpad_threshold:
self._send_event("lift_down", "dpad_right")
def _send_analog_events(self, state: Dict[str, float]) -> None:
surge = self._compute_surge(state["ly"])
sway = self._cleanup_command(
self._apply_deadzone(state["lx"]) * -0.4 + self.lateral_command_offset
)
spin = self._cleanup_command(
self._apply_deadzone(state["rx"]) * -0.4 + self.rotation_command_offset
)
self._send_event("set_surge", "left_stick_y", surge)
self._send_event("set_sway", "left_stick_x", sway)
self._send_event("set_spin", "right_stick_x", spin)
def _compute_surge(self, ly: float) -> float:
ly = self._apply_deadzone(ly)
if ly >= 0.0:
value = ly * 0.8 + self.forward_command_offset
else:
value = ly * 0.5
return self._cleanup_command(value)
def _apply_deadzone(self, value: float) -> float:
if abs(value) < self.deadzone:
return 0.0
return float(value)
def _cleanup_command(self, value: float) -> float:
if abs(value) < self.analog_epsilon:
return 0.0
return float(value)
def _rising_edge(self, state: Dict[str, float], name: str) -> bool:
previous = int(self.last_buttons.get(name, 0))
return int(state[name]) == 1 and previous == 0
def _send_event(
self, event_code: str, key_name: str, drive_value: float = 1.0
) -> None:
envelope = InputEnvelope(
seq_id=self.seq_id,
event_code=event_code,
key_name=key_name,
drive_value=drive_value,
source_tag=self.source_tag,
)
self.seq_id += 1
self.socket.sendto(
envelope.encode(), (self.target_host, self.target_port)
)
def main(args: list[str] | None = None) -> None:
rclpy.init(args=args)
node = UDPXboxSender()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()