diff --git a/ros2/README.md b/ros2/README.md index d0913c3..3a95b8f 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -24,8 +24,8 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。 - 关节空间和笛卡尔空间运动控制 - 完整的逆运动学和正运动学 - **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态 - - `z > -55mm`: J5=-100（闭合），z4=-100mm，工作范围 z ∈ [-190, 110]mm - - `z ≤ -55mm`: J5=81（张开），z4=55mm，工作范围 z ∈ [-345, -55]mm + - `z > -55mm`: UP（J5=-100，z4=-100），工作范围 z ∈ [-190, 110]mm + - `z ≤ -55mm`: DOWN（J5=81，z4=55），工作范围 z ∈ [-345, -55]mm - UDP 通信（与 ESP32） - 状态发布（10Hz） @@ -39,8 +39,8 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。 - 释放流程：移动 → 释放 → 回收 **相机旋转说明**： -- J5 > 0°（张开/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)` -- J5 < 0°（闭合/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)` +- J5 < 0°（闭合/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)` +- J5 > 0°（张开/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)` ## 🚀 快速开始 diff --git a/ros2/src/udp_teleop/README.md b/ros2/src/udp_teleop/README.md deleted file mode 100644 index 6e9a77c..0000000 --- a/ros2/src/udp_teleop/README.md +++ /dev/null @@ -1,165 +0,0 @@ -# udp_teleop — ROS 2 底盘 + 机械臂键盘 UDP 遥控 - -通过键盘实时控制底盘（差速驱动）和机械臂（6 电机），指令通过 **单一 UDP socket** 发送到设备端。 - -## 项目结构 - -``` -ros2/ -├── build/ # colcon 构建产物（自动生成） -├── install/ # colcon 安装产物（自动生成） -├── log/ # 构建日志（自动生成） -└── src/ - └── udp_teleop/ # ROS 2 包 - ├── config/ - │ └── params.yaml # 可配置参数 - ├── launch/ # launch 文件（预留） - ├── udp_teleop/ - │ ├── __init__.py - │ └── keyboard_control.py # 键盘遥控节点 - ├── resource/ - ├── test/ - ├── package.xml - ├── setup.cfg - └── setup.py -``` - -## 环境搭建 - -### 1. 安装 Conda - -使用 Miniconda 或 Anaconda。推荐 Miniconda： - -```bash -# 下载并安装 Miniconda -wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -bash Miniconda3-latest-Linux-x86_64.sh -``` - -### 2. 创建 ROS 2 Humble 环境 - -使用 robostack 频道安装 ROS 2 Humble Desktop 完整版： - -```bash -conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop -``` - -### 3. 安装构建工具 - -```bash -conda activate ros2_humble -conda install -c robostack-staging -c conda-forge \ - colcon-common-extensions \ - ros-humble-ament-cmake \ - python3-pip -``` - -### 4. 安装 Python 依赖 - -```bash -pip install pynput -``` - -### 5. 激活环境 - -每次使用前： - -```bash -conda activate ros2_humble -source /path/to/ros2/install/setup.bash # 首次构建后执行 -``` - -## 构建 - -```bash -cd ros2 -colcon build --symlink-install --packages-select udp_teleop -source install/setup.bash -``` - -> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。 - -## 运行 - -### 使用参数文件（推荐） - -```bash -ros2 run udp_teleop keyboard_control \ - --ros-args --params-file src/udp_teleop/config/params.yaml -``` - -### 命令行覆盖参数 - -```bash -ros2 run udp_teleop keyboard_control \ - --ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999 -``` - -## 按键映射 - -### 底盘控制 - -| 按键 | 功能 | -|------|------| -| `W` / `S` | 前进 / 后退 | -| `A` / `D` | 左移 / 右移 | -| `Q` / `E` | 左转 / 右转 | - -### 机械臂控制 - -| 按键 | 功能 | -|------|------| -| `↑` / `↓` | 升降高度（↑ 升高，↓ 降低） | -| `2` ~ `6` | 选择关节 J2 ~ J6 | -| `←` / `→` | 减小 / 增大当前关节角度 | - -> 底盘和机械臂可以**同时操控**。机械臂指令仅在按下机械臂相关按键后发送。 - -### 其他 - -| 按键 | 功能 | -|------|------| -| `Ctrl+C` | 退出并发送底盘停止指令 | - -## UDP 协议 - -### 底盘指令 - -``` -XYW::::XZHY\n -``` - -### 机械臂指令 - -``` -JXB:<高度>::::::0:0:EZHY\n -``` - -- 6 个电机：电机 1 控制高度，电机 2~6 对应关节 J2~J6 -- 末尾补零至 8 个值 - -## 参数配置 - -| 参数 | 默认值 | 说明 | -|------|--------|------| -| `udp_ip` | `127.0.0.1` | UDP 目标 IP 地址 | -| `udp_port` | `8888` | UDP 目标端口 | -| `chassis_linear_speed` | `100` | 底盘线速度 | -| `chassis_angular_speed` | `45` | 底盘角速度 | -| `arm_height_step` | `5` | 高度每步变化量 | -| `arm_joint_step` | `5` | 关节角度每步变化量 | -| `update_rate` | `0.05` | 控制循环周期（秒） | -| `stdin_hold_time` | `0.04` | 按键持续时间（秒），修复箭头键时序问题 | -| `debug_keys` | `false` | 是否在状态行显示当前按键 | -| `keyboard_backend` | `auto` | 键盘后端：`auto` / `stdin` / `pynput` / `win_poll` | - -## 键盘后端 - -| 后端 | 说明 | -|------|------| -| `auto` | 自动选择：Linux/macOS 用 `stdin`，Windows 用 `win_poll` | -| `stdin` | 基于终端原始输入，无需额外依赖，**需要交互终端** | -| `pynput` | 基于 pynput 库，跨平台，需要 `pip install pynput` | -| `win_poll` | Windows 专用，通过 Win32 API 轮询按键状态 | - -> `ros2 launch` 启动的子进程**没有交互终端**，使用 `stdin` 后端会报错。必须通过 `ros2 run` 在终端直接运行。 diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control.py b/ros2/src/udp_teleop/udp_teleop/arm_control.py index 1edeb09..24defdc 100644 --- a/ros2/src/udp_teleop/udp_teleop/arm_control.py +++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py @@ -51,8 +51,10 @@ J5_CLOSED = -100 DEFAULT_FIXED_J5 = J5_OPEN # Z4 值根据夹爪状态变化 -Z4_OPEN = 55 # 夹爪张开（J5=81） -Z4_CLOSED = -100 # 夹爪闭合（J5=-100） +# J5 = -100 (闭合) → 夹爪朝上 (UP) → z4 = -100 +# J5 = 81 (张开) → 夹爪朝下 (DOWN) → z4 = 55 +Z4_UP = -100 # 夹爪朝上（J5=-100，闭合） +Z4_DOWN = 55 # 夹爪朝下（J5=81，张开） GRIP_ANGLE = -5 RELEASE_ANGLE = 80 @@ -172,17 +174,17 @@ def normalize_angle_deg(angle_deg: float) -> float: def resolve_z4_from_j5(j5: int) -> float: """根据 J5 状态确定 z4 值 - - J5 > 0 (张开): z4 = 55mm - - J5 < 0 (闭合): z4 = -100mm + - J5 = -100 (闭合): 夹爪朝上，z4 = -100mm + - J5 = 81 (张开): 夹爪朝下，z4 = 55mm """ - return Z4_OPEN if j5 > 0 else Z4_CLOSED + return Z4_UP if j5 < 0 else Z4_DOWN def resolve_j5_from_z(z: float) -> int: """根据目标 z 坐标自动选择夹爪状态 - - z > -55: 使用闭合状态 (J5=-100, z4=-100) - - z <= -55: 使用张开状态 (J5=81, z4=55) + - z > -55: 使用朝上状态 (J5=-100, z4=-100) + - z <= -55: 使用朝下状态 (J5=81, z4=55) """ return J5_CLOSED if z > -55 else J5_OPEN @@ -606,9 +608,9 @@ class ArmControlNode(Node): # 解析 z4：优先级 up/down > 自动选择 if request.up: - z4 = Z4_CLOSED # 夹爪朝上，z4=-100 + z4 = Z4_UP # 夹爪朝上，z4=-100 elif request.down: - z4 = Z4_OPEN # 夹爪朝下，z4=55 + z4 = Z4_DOWN # 夹爪朝下，z4=55 else: # 自动选择：根据目标 z 坐标 j5_auto = resolve_j5_from_z(target_pose.z) @@ -623,7 +625,7 @@ class ArmControlNode(Node): j6 = self.current_state.j6 # 根据 z4 反推 J5（用于 UDP 命令） - j5 = J5_CLOSED if z4 == Z4_CLOSED else J5_OPEN + j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN # 逆运动学 math_state = inverse_kinematics( diff --git a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py index 8d4286d..53b59a2 100644 --- a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py +++ b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py @@ -286,20 +286,22 @@ class VisionGraspNode(Node): # 图像坐标到相机坐标系转换 # 相机水平安装，但会随 J5 状态旋转 180° - if j5 > 0: - # J5 张开（UP）：相机正向 + # J5 = -100 (闭合) → 夹爪朝上 (UP) + # J5 = 81 (张开) → 夹爪朝下 (DOWN) + if j5 < 0: + # J5 闭合（UP）：相机正向 # 图像 Y 向下 → 相机 -Y xc = x yc = -y zc = z - self.get_logger().info(f'相机正向（UP），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') + self.get_logger().info(f'J5={j5}° (UP)，相机正向，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') else: - # J5 闭合（DOWN）：相机旋转 180° + # J5 张开（DOWN）：相机旋转 180° # 图像 X,Y 都翻转（相对于 UP 状态） xc = -x yc = y zc = z - self.get_logger().info(f'相机旋转 180°（DOWN），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') + self.get_logger().info(f'J5={j5}° (DOWN)，相机旋转 180°，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') # 坐标变换：相机 → 基坐标系 target_x, target_y, target_z = camera_to_base(