FallenSigh
7144005be2
主要修改: - 坐标系统: z=0 为顶部改为底部，z 轴朝上 - 高度范围: d1 ∈ [-290, 0] mm (-290=底部, 0=顶部) - Z4 偏移: 80.0 (原 -80.0), Z4_OPEN=55, Z4_CLOSED=-100 - 运动学: 更新 tcp_to_joint4_center 和 forward_kinematics 的 z 坐标变换 - 夹爪逻辑: 更新 resolve_gripper_from_z 的高度判断范围 - 插值优化: 先完成末端运动，再处理 grip/release - 移除所有坐标转换中的负号，统一为正向 z 轴朝上系统
tools — 调试 & 测试工具
文件
| 文件 | 说明 |
|---|---|
udp_server.py |
UDP 回显服务器，监听 0.0.0.0:8888，用于测试 UDP 通讯协议 |
使用
UDP 回显服务器
启动服务器，接收来自 ESP32 或 ROS 2 键盘遥控节点的 UDP 指令并打印：
python tools/udp_server.py
发送测试指令验证协议格式：
# 底盘指令
echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
# 机械臂指令
echo 'JXB:-10:90:0:0:45:0:0:0:EZHY' | nc -u 127.0.0.1 8888
# 激光指令
echo 'LASERON' | nc -u 127.0.0.1 8888
协议测试流程
- 启动
python tools/udp_server.py - 修改 ROS 2 节点参数指向本机：
udp_ip:=127.0.0.1 - 运行 ROS 2 键盘遥控节点，观察服务器收到的指令
- 确认协议格式正确后，将目标 IP 改为实际设备地址
# 示例：ROS 2 键盘遥控 → 本地回显
ros2 run udp_teleop keyboard_control \
--ros-args -p udp_ip:=127.0.0.1 -p udp_port:=8888