update README.md

- Tune AMCL: widen sigma_hit (0.05→0.2), z_hit (0.5→0.9), z_rand, likelihood_max_dist,
  laser_max_range (5.0→8.0) for better convergence on 4x4m field
- Propagate lidar_x/y/z/yaw/intensity/sample_rate/baudrate through bringup,
  localization, and mapping launch files so overrides reach the driver
- Fix default lidar_z (0.0→0.02) in lidar.launch.py
- Change rviz Fixed Frame odom→map to stop map jumping on AMCL corrections
- Fix broken teach_points command in docs/localization.md; add troubleshooting
  entries for AMCL convergence and rf2o drift
- Add ros2/AGENTS.md with ROS workspace build/style guidelines

.gitignore

@@ -44,3 +44,16 @@ Thumbs.db
 # ====================
 *.log
 *.tmp
+
+# ====================
+# Project Specific
+# ====================
+dataset/
+dataset.zip
+ros2.zip
+
+# 运行时状态文件
+tools/.udp_control_state.json
+.ros/udp_control_state.json
+
+.omo/

CLAUDE.md

@@ -0,0 +1,242 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## Project Overview
+
+CRAIC (Camera-Robot AI Control System) — Competition code for the China Robot and Artificial Intelligence Competition (中国机器人及人工智能大赛), Robot Task Challenge (Small Desktop Level).
+
+**Hardware**: ESP32-S3-WROOM-1-N16R8 with OV2640 camera, 6-DOF mechanical arm with Feettech SCS/STS serial servos.
+
+**Architecture**: Three-tier system communicating via UDP port 8888:
+1. **ESP32-S3 firmware** (`jxbeye/`) — Dual-core camera streaming + UDP command receiver
+2. **ROS 2 teleop** (`ros2/src/udp_teleop/`) — Keyboard control node for chassis and arm
+3. **Python tools** (`tools/`) — Standalone control scripts with inverse kinematics
+
+## Build & Run Commands
+
+### ESP32-S3 Firmware
+
+```bash
+cd jxbeye
+pio run -t upload          # Build and flash firmware
+pio device monitor         # Serial monitor (1000000 baud)
+```
+
+**First boot**: ESP32 creates AP `ESP32-S3-Camera` (password `12345678`), access web UI at `http://192.168.4.1`.
+
+**WiFi configuration via serial**: Send `WIFI:SSID:PASSWORD` to configure station mode.
+
+### ROS 2 Teleop
+
+```bash
+# Build (from ros2/ directory)
+conda activate ros2_humble
+colcon build --symlink-install --packages-select udp_teleop
+source install/setup.bash
+
+# Run keyboard control
+ros2 run udp_teleop keyboard_control \
+    --ros-args --params-file src/udp_teleop/config/params.yaml
+
+# Override target IP
+ros2 run udp_teleop keyboard_control \
+    --ros-args -p udp_ip:=192.168.4.1 -p udp_port:=8888
+```
+
+**Keyboard mappings**:
+- Chassis: W/S (forward/back), A/D (strafe), Q/E (rotate)
+- Arm: ↑/↓ (height), 2-6 (select joint), ←/→ (adjust angle)
+
+**Important**: Must use `ros2 run`, not `ros2 launch` — the `stdin` keyboard backend requires an interactive terminal.
+
+### Python Arm Control Tools
+
+```bash
+# Direct joint command with interpolation
+python tools/udp_control.py joints \
+    --height -100 --j2 10 --j3 20 --j4 30 \
+    --duration 1.0 --rate 20
+
+# Cartesian pose mode (uses inverse kinematics)
+python tools/udp_control.py pose \
+    --x 150 --y 50 --z -100 --phi 45 \
+    --duration 1.0
+
+# Dry run (print commands without sending)
+python tools/udp_control.py pose --x 200 --y 0 --z -50 --phi 0 --dry-run
+
+# Camera frame capture
+python tools/camera_capture.py --ip 192.168.4.1
+python tools/camera_capture.py --scan  # Auto-detect camera on subnet
+```
+
+### UDP Testing
+
+```bash
+# Start echo server
+python tools/udp_server.py
+
+# Send test commands
+echo 'XYW:100:0:0:XZHY' | nc -u 192.168.4.1 8888
+echo 'JXB:-100:10:20:30:0:0:0:0:EZHY' | nc -u 192.168.4.1 8888
+```
+
+## Architecture Details
+
+### ESP32-S3 Dual-Core Design
+
+- **Core 0**: Camera capture loop (OV2640 → JPEG)
+- **Core 1**: WiFi streaming (MJPEG HTTP server on port 80)
+- **AsyncUDP**: Non-blocking UDP command receiver runs on Core 1, handled via interrupt callbacks
+
+The dual-core split ensures camera capture never blocks on WiFi transmission. UDP commands are processed asynchronously and do not interfere with streaming.
+
+### UDP Protocol
+
+All commands are ASCII text ending with terminator (varies by command type):
+
+```
+# Chassis control (XYZ cartesian velocity)
+XYW:<X_speed>:<Y_speed>:<W_angular>:XZHY\n
+
+# Arm control (6 motors: height, J2-J6)
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+
+# Laser control
+LASERON\n
+LASEROFF\n
+
+# Serial passthrough (any payload with ZHY or \n terminator)
+<payload>ZHY\n
+```
+
+**Critical**: The ESP32 firmware parses based on terminator suffix, not command prefix.
+
+### Mechanical Arm Coordinate System & Kinematics
+
+**Coordinate frame**: Base frame with Z-axis pointing UP (not down). Origin at the bottom of the J1 linear slide.
+
+**Height coordinate (d1)**:
+- User-facing coordinate: `-290 mm` (bottom) to `0 mm` (top)
+- Physical meaning: vertical position of J2 relative to base origin
+- **Z-up convention**: positive d1 = higher position
+
+**Planar joints (J2, J3, J4)**:
+- All three rotate around vertical Z-axis in the XY plane
+- J2 is base rotation, J3/J4 are elbow/wrist
+- TCP yaw angle `phi = J2 + J3 + J4` (additive)
+
+**Geometry parameters** (see `docs/arm.md` for full derivation):
+- `L1 = 125 mm`: J2-J3 link length
+- `L2 = 125 mm`: J3-J4 link length  
+- `x4 = 110 mm`: J4-to-TCP horizontal offset
+- `z4 = 80 mm`: J4-to-TCP vertical offset (variable: 55mm when gripper down, -100mm when up)
+
+**Zero offsets**: Physical mechanical zero does not align with math zero (straight line):
+- `J2_zero = 3°`, `J3_zero = 7°`, `J4_zero = 25°`
+- UDP command angles = math angles + zero offsets
+
+**Inverse kinematics** (`tools/udp_control.py`):
+- Solves for joint angles given TCP pose `(x, y, z, phi)`
+- Uses standard 2-link planar arm solution (atan2-based)
+- Two solutions: `--elbow-up` vs default elbow-down
+- Validates workspace limits and singularity checks
+- See `docs/arm.md` for full mathematical derivation
+
+**State persistence**: `tools/.udp_control_state.json` caches last sent joint command. This enables smooth interpolated motion from previous position without re-homing. Use `--no-state-cache` to disable.
+
+### ROS 2 Keyboard Backend Selection
+
+The `udp_teleop` node supports three keyboard input backends:
+
+- `stdin` (default on Linux/macOS): Terminal raw mode, zero dependencies, **requires interactive terminal**
+- `pynput`: Cross-platform library, works in background
+- `win_poll`: Windows-specific Win32 API polling
+
+Backend auto-selected by platform. Override with `keyboard_backend` parameter in `config/params.yaml`.
+
+**Limitation**: `stdin` backend fails when launched via `ros2 launch` because child processes lack TTY. Always use `ros2 run` for interactive keyboard control.
+
+### Camera Capture Tool Auto-Detection
+
+`camera_capture.py` implements subnet scanning and ESP32 identification:
+
+1. Probes common DHCP IPs on local subnet (`x.x.x.1`, `x.x.x.100-110`, etc.)
+2. Verifies ESP32 by checking `/status` endpoint for JSON keys `capture_fps` and `has_client`
+3. Falls back to full subnet scan if not found in common range
+4. Connects to `/stream` MJPEG endpoint, parses multipart frames, extracts first valid JPEG (SOI `0xFFD8` to EOI `0xFFD9`)
+
+Use `--scan` to force full subnet scan, or `--ip` to skip detection.
+
+## Environment Setup
+
+### ROS 2 Humble via Conda (robostack)
+
+```bash
+# Create environment (one-time)
+conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
+conda activate ros2_humble
+conda install -c robostack-staging -c conda-forge colcon-common-extensions
+pip install pynput
+
+# Every session
+conda activate ros2_humble
+cd ros2
+source install/setup.bash  # After first colcon build
+```
+
+**Alternative**: Native apt installation on Ubuntu 22.04 — see `ros2/README.md`.
+
+### PlatformIO ESP32
+
+```bash
+pip install platformio
+cd jxbeye
+pio pkg install  # Install dependencies
+```
+
+**Board configuration** (`platformio.ini`):
+- Custom board definition: `esp32-s3-wroom-1-n16r8`
+- PSRAM: Octal mode (`board_build.psram_type = octal`)
+- Flash: 16MB QIO mode at 80MHz
+- Partition table: `default_16MB.csv`
+
+## Important Files
+
+### Configuration
+
+- `jxbeye/platformio.ini` — ESP32 build config (PSRAM settings critical)
+- `ros2/src/udp_teleop/config/params.yaml` — ROS node parameters (IP, speeds, steps)
+- `tools/.udp_control_state.json` — Cached arm joint state for interpolation
+
+### Documentation
+
+- `docs/arm.md` — Full inverse kinematics derivation with LaTeX equations
+- `README.md` — Project overview and quick start
+- `ros2/src/udp_teleop/README.md` — ROS package details and keyboard mappings
+
+### Core Implementations
+
+- `jxbeye/src/main.cpp` — ESP32 dual-core firmware entry point
+- `ros2/src/udp_teleop/udp_teleop/keyboard_control.py` — ROS keyboard node
+- `tools/udp_control.py` — Standalone arm controller with full IK solver
+- `tools/camera_capture.py` — MJPEG stream frame extractor
+
+## Common Tasks
+
+**Change arm geometry parameters**: Edit constants in `tools/udp_control.py` (`DEFAULT_L1`, `DEFAULT_L2`, `DEFAULT_X4`, `DEFAULT_Z4`) or pass as CLI args.
+
+**Modify joint limits**: Edit `DEFAULT_*_MIN/MAX` in `tools/udp_control.py` or use `--height-min`, `--j2-max`, etc.
+
+**Adjust interpolation smoothness**: Change `--duration` (total time) and `--rate` (Hz) in `udp_control.py`. Default is 1.0s at 20Hz = 20 steps.
+
+**Debug UDP protocol**: Use `tools/udp_server.py` as echo server, point ROS/tools at `127.0.0.1:8888` to inspect raw commands.
+
+**Test kinematics without hardware**: Use `--dry-run` flag with `udp_control.py` to print UDP commands without sending.
+
+**Verify IK correctness**: Use `--show-fk` flag to compute forward kinematics of the solved joint angles and compare to target pose.
+
+## Dataset
+
+The `dataset.zip` and `dataset/` directory contain competition-specific training data (exact format unknown from structure alone).

README.md

@@ -2,7 +2,7 @@
 
 > 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。
 
-ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机器人系统。
+ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取 + 激光 SLAM 定位导航的一体化机器人系统。
 
 ## 赛事信息
 
@@ -19,24 +19,6 @@ ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机
 | 参赛赛项 | 机器人任务挑战赛（小型桌面级） |
 | 官方网站 | [www.caairobot.com](https://www.caairobot.com) |
 
-## 项目结构
-
-```
-craic/
-├── jxbeye/                    # ESP32-S3 固件 (PlatformIO)
-│   ├── src/main.cpp           # 双核摄像头推流 + UDP 控制
-│   └── platformio.ini         # ESP32-S3-WROOM-1-N16R8 配置
-├── ros2/                      # ROS 2 控制系统
-│   ├── src/arm_control_msgs/  # 消息和服务定义
-│   └── src/udp_teleop/        # 机械臂控制和视觉抓取节点
-├── tools/                     # 独立工具脚本
-│   ├── udp_control.py         # 命令行机械臂控制（带逆运动学）
-│   ├── camera_to_base.py      # 相机坐标到基坐标变换
-│   └── camera_capture.py      # MJPEG 流采集
-└── docs/                      # 技术文档
-    └── arm.md                 # 机械臂运动学推导
-```
-
 ## 硬件
 
 | 组件 | 型号 |
@@ -44,34 +26,100 @@ craic/
 | 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM) |
 | 摄像头 | OV2640 (XGA 1024×768, JPEG) |
 | 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 |
+| 激光雷达 | YDLiDAR TminiPro (360°, 10Hz) |
+| 底盘 | 麦克纳姆轮底盘，LZUCAR 底盘 MCU（串口里程计） |
+| 激光 | 激光模块（LASERON / LASEROFF） |
+
+## 项目结构
+
+```
+craic/
+├── jxbeye/                         # ESP32-S3 固件 (PlatformIO)
+│   ├── src/main.cpp                # 双核：Core 0 摄像头采集，Core 1 WiFi推流 + UDP控制
+│   ├── platformio.ini              # 开发板与 PSRAM 配置
+│   └── lib/FTServo/                # 飞特串行舵机库
+├── ros2/                           # ROS 2 控制系统
+│   ├── build.sh                    # 一键编译脚本
+│   ├── src/arm_control_msgs/       # 机械臂消息与服务定义 (ament_cmake)
+│   │   ├── msg/                    # JointState, TCPPose
+│   │   └── srv/                    # MoveJoints, MovePose, GetPose, SetGripper
+│   ├── src/udp_teleop/             # UDP 遥控与机械臂控制 (ament_python)
+│   │   ├── udp_teleop/             # arm_control, vision_grasp, keyboard_control, box_detection_grasp
+│   │   ├── launch/                 # vision_grasp.launch.py（统一启动）
+│   │   ├── config/                 # arm / vision / keyboard / box_detection 参数
+│   │   └── models/                 # YOLO 模型权重 (box_detection.pt)
+│   ├── src/craic_localization/     # 定位与导航 (ament_python)
+│   │   ├── launch/                 # mapping / localization / lidar / bringup
+│   │   ├── config/                 # amcl / gmapping / lidar 参数
+│   │   └── rviz/                   # 定位专用 RViz 配置
+│   ├── src/rf2o_laser_odometry/    # 激光里程计 (ament_cmake, C++14)
+│   └── src/ydlidar_ros2_driver/    # YDLiDAR 驱动 (ament_cmake)
+├── tools/                          # 独立命令行工具（不依赖 ROS）
+│   ├── udp_control.py              # 机械臂控制（逆运动学 + 插值）
+│   ├── camera_to_base.py           # 相机→基座坐标系变换
+│   ├── camera_capture.py           # MJPEG 流帧采集
+│   ├── udp_server.py               # UDP 回显调试服务器
+│   └── README.md                   # 工具详细文档
+├── docs/                           # 技术文档
+│   ├── arm.md                      # 机械臂运动学推导
+│   ├── localization.md             # 定位系统完整文档
+│   └── box_detection_grasp.md      # 方框检测与自动抓取
+└── dataset/                        # 竞赛训练数据（黑方块图像 + 标定）
+```
 
 ## 核心功能
 
 ### 1. ESP32-S3 固件 (`jxbeye/`)
 
-- **双核架构**：Core 0 采集，Core 1 推流
+- **双核架构**：Core 0 采集 OV2640 JPEG，Core 1 WiFi 推流 + 异步 UDP 命令接收
-- **MJPEG 推流**：`http://<IP>` 实时查看
+- **MJPEG 推流**：`http://<IP>` 实时查看，`/stream` 端点供 OpenCV/YOLO 消费
-- **UDP 控制**：端口 8888，非阻塞异步处理
+- **UDP 控制**：端口 8888，非阻塞中断回调处理底盘 + 机械臂 + 激光指令
-- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置
+- **WiFi 配置**：首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），串口发送 `WIFI:SSID:PASSWORD` 切换 Station 模式
 
-### 2. ROS 2 机械臂控制 (`ros2/`)
+### 2. 机械臂控制与视觉抓取 (`ros2/src/udp_teleop/`)
 
-**arm_control 节点**：
+| 节点 | 功能 |
-- 关节空间和笛卡尔空间运动控制
+|------|------|
-- 完整逆运动学和正运动学
+| `arm_control` | 完整逆运动学/正运动学，关节空间 + 笛卡尔空间运动，4 个 ROS 服务，状态发布 (10Hz)，自动归零，动态 J5/z4 适配 |
-- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper`
+| `vision_grasp` | 相机坐标→基坐标变换，自动抓取/释放流程（松开→移动→夹取→回收），多线程服务调用 |
-- 状态发布（10Hz）
+| `box_detection_grasp` | YOLO 实时方框检测（MJPEG 流），单目深度估计，自动/手动模式，检测到即触发抓取 |
+| `keyboard_control` | 键盘 UDP 遥控（底盘 WASD/QE + 机械臂 ↑↓←→ + 关节选择 2-6），3 种输入后端 |
 
-**vision_grasp 节点**：
+**机械臂服务接口**：
-- 相机坐标到基坐标系自动转换
+```bash
-- 自动抓取：释放 → 移动 → 抓取 → 回收
+ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
-- 自动释放：移动 → 释放 → 回收
+    "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
+ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
+ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper "{grip: true}"
+```
 
-### 3. 工具脚本 (`tools/`)
+### 3. 激光 SLAM 定位与导航 (`ros2/src/craic_localization/`)
 
-- `udp_control.py` - 命令行机械臂控制（支持逆运动学）
+三个阶段式比赛流程：
-- `camera_to_base.py` - 坐标变换工具
+
-- `camera_capture.py` - 相机帧采集（支持自动扫描）
+| 阶段 | Launch 文件 | 功能 | TF 链 |
+|------|------------|------|-------|
+| P3 标定 | `bringup.launch.py` | 轮式里程计 + 激光驱动 + RViz | `odom → base_footprint → laser_frame` |
+| P4 建图 | `mapping.launch.py` | 激光 + rf2o 里程计 + slam_gmapping | `map → odom → base_footprint → laser_frame` |
+| P5 导航 | `localization.launch.py` | 激光 + rf2o + map_server + AMCL | `map → odom → base_footprint → laser_frame` |
+
+**定位系统节点**：
+
+| 节点 | 功能 |
+|------|------|
+| `chassis_odometry` | 串口读取 LZUCAR 底盘 MCU 轮式里程计（32 字节协议） |
+| `teach_points` | 交互式示教工具：回车记录 map 帧位姿，支持预设点位序列 (B_1..F_1)，自动存 YAML |
+| `navigate_to_point` | 全向 P 控制器自主导航到示教点，UDP XYW 指令闭环驱动，干运行/单次模式可选 |
+| `show_points` | RViz MarkerArray 可视化示教点（箭头 + 标签），实时更新 |
+
+### 4. 命令行工具 (`tools/`)
+
+| 工具 | 功能 |
+|------|------|
+| `udp_control.py` | 机械臂关节/笛卡尔空间控制，逆运动学，轨迹插值，状态缓存，干运行 |
+| `camera_to_base.py` | 相机→TCP→基座完整变换链，支持相机安装偏移与旋转 |
+| `camera_capture.py` | ESP32 MJPEG 流帧采集，自动子网扫描寻找相机 |
+| `udp_server.py` | UDP 回显服务器，用于调试协议通信 |
 
 ## 快速开始
 
@@ -83,8 +131,6 @@ pio run -t upload
 pio device monitor
 ```
 
-首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），访问 `http://192.168.4.1`。
-
 ### ROS 2 控制系统
 
 ```bash
@@ -92,124 +138,187 @@ pio device monitor
 conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
 conda activate ros2_humble
 
-# 编译
+# 编译（一键）
 cd ros2
-export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
+./build.sh
-export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
-export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
-
-colcon build --packages-select arm_control_msgs \
-  --cmake-args \
-    -DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
-    -DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
-    -DPython_LIBRARY=$PYTHON_LIBRARY
-
-colcon build --packages-select udp_teleop
 source install/setup.bash
 
-# 运行机械臂控制
+# 或按需编译指定包
+./build.sh --packages-select craic_localization udp_teleop
+```
+
+**启动机械臂控制**：
+```bash
+# 终端 1：机械臂控制器
 ros2 run udp_teleop arm_control \
     --ros-args --params-file src/udp_teleop/config/arm_control.yaml
 
-# 运行视觉抓取（新终端）
+# 终端 2：视觉抓取
 ros2 run udp_teleop vision_grasp \
     --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+
+# 或一键启动全部（含 YOLO 检测）
+ros2 launch udp_teleop vision_grasp.launch.py detection:=true auto_grasp:=true
+```
+
+**启动键盘遥控**：
+```bash
+ros2 run udp_teleop keyboard_control \
+    --ros-args --params-file src/udp_teleop/config/params.yaml
+```
+
+**启动定位与导航**：
+```bash
+# P4 建图
+ros2 launch craic_localization mapping.launch.py
+# 建图完成后保存
+ros2 run nav2_map_server map_saver_cli -f src/craic_localization/maps/craic
+
+# P5 定位 + 导航
+ros2 launch craic_localization localization.launch.py
+
+# 新终端：示教记录点位
+ros2 run craic_localization teach_points \
+    --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
+
+# 新终端：自主导航到示教点
+ros2 run craic_localization navigate_to_point
 ```
 
 ### 命令行工具
 
 ```bash
-# 机械臂控制（关节空间）
+# 机械臂控制
-python tools/udp_control.py joints \
+python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0
-    --height -100 --j2 10 --j3 20 --j4 30 --duration 2.0
+python tools/udp_control.py joints --height -100 --j2 10 --j3 20 --j4 30
-
-# 机械臂控制（笛卡尔空间）
-python tools/udp_control.py pose \
-    --x 200 --y 100 --z -100 --phi 45 --duration 2.0
 
 # 相机采集
 python tools/camera_capture.py --ip 192.168.4.1
 python tools/camera_capture.py --scan  # 自动扫描
+
+# UDP 调试
+python tools/udp_server.py
+echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
 ```
 
 ## UDP 协议
 
-所有指令通过 UDP 端口 `8888` 发送：
+所有指令通过 UDP 端口 `8888` 发送，ASCII 文本协议：
 
-```bash
+```
-# 底盘控制（麦克纳姆轮）
+# 底盘控制（麦克纳姆轮全向速度）
-XYW:<X>:<Y>:<W>:XZHY\n
+XYW:<X_speed>:<Y_speed>:<W_angular>:XZHY\n
 
-# 机械臂控制（6 轴）
+# 机械臂控制（6 轴角度 + 高度）
 JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
 
 # 激光控制
-LASERON / LASEROFF
+LASERON\n
+LASEROFF\n
 ```
 
+- 角度单位：度（已包含零点偏移）
+- 高度单位：mm
+- 速度单位：X/Y mm/s，W deg/s
+
 ## 使用示例
 
 ### ROS 服务调用
 
 ```bash
-# 查询位姿
+# 机械臂移动到目标位置
-ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
-
-# 移动到目标位置
 ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
     "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
 
+# 快速抓取
+ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
+    "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, grip: true, duration: 2.0}"
+
+# 查询位姿
+ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
+
 # 视觉抓取（发布相机坐标）
 ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
     "{x: 10.0, y: 5.0, z: 250.0}"
 ```
 
+### 定位导航工作流
+
+```bash
+# 1. 启动定位系统
+ros2 launch craic_localization localization.launch.py
+
+# 2. 键盘遥控到目标位置（另一终端）
+ros2 run udp_teleop keyboard_control \
+    --ros-args --params-file src/udp_teleop/config/params.yaml
+
+# 3. 示教记录点位（第三终端）
+ros2 run craic_localization teach_points \
+    --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
+# 交互命令：回车记录 → r 记录 → p 查看 → save 保存 → q 退出
+
+# 4. 自主导航到示教点
+ros2 run craic_localization navigate_to_point
+# 输入目标点名（如 B_1），机器人自动导航到位
+```
+
 ### Python 集成
 
 ```python
 import rclpy
 from geometry_msgs.msg import Point
 
-class VisionDetector:
+class VisionDetector(rclpy.node.Node):
     def __init__(self):
-        self.grasp_pub = self.create_publisher(Point, 'vision_grasp/grasp_target', 10)
+        super().__init__('vision_detector')
-    
+        self.grasp_pub = self.create_publisher(
+            Point, 'vision_grasp/grasp_target', 10)
+
     def on_detection(self, camera_x, camera_y, camera_z):
         msg = Point()
-        msg.x = camera_x
+        msg.x = camera_x; msg.y = camera_y; msg.z = camera_z
-        msg.y = camera_y
-        msg.z = camera_z
         self.grasp_pub.publish(msg)
 ```
 
-## 文档
-
-- **ROS 2 系统**：[ros2/README.md](ros2/README.md) - 完整的 ROS 节点文档
-- **运动学推导**：[docs/arm.md](docs/arm.md) - 机械臂逆运动学数学推导
-- **视觉标定**：[docs/vision_calibration_horizontal.md](docs/vision_calibration_horizontal.md) - 相机标定指南
-
 ## 坐标系说明
 
 **机械臂基坐标系**（Z 轴朝上）：
 - 原点：J1 线性滑轨底部
-- X 轴：基座正前方
+- X 轴：基座正前方，Y 轴：基座左侧，Z 轴：竖直向上
-- Y 轴：基座左侧
+- 单位：mm
-- Z 轴：竖直向上（高度）
 
 **相机坐标系**（水平安装）：
-- X 轴：右侧
+- X 轴：右侧，Y 轴：向下，Z 轴：正前方（光轴）
-- Y 轴：向下
+- 单位：mm
-- Z 轴：正前方（光轴）
 
-变换由 `camera_to_base.py` 和 `vision_grasp` 节点自动处理。
+**底盘坐标系**（ROS TF）：
+```
+map ──(AMCL)──> odom ──(rf2o)──> base_footprint ──(static)──> laser_frame
+                                                  ──(static)──> base_link
+```
+- `map`：全局固定坐标系（AMCL 定位输出）
+- `odom`：连续里程计坐标系（rf2o 激光里程计）
+- `base_footprint`：底盘投影中心
+- `laser_frame`：激光雷达安装位置（外参可调：`lidar_x`, `lidar_y`, `lidar_yaw`）
+
+## 文档
+
+| 文档 | 说明 |
+|------|------|
+| [docs/arm.md](docs/arm.md) | 机械臂逆运动学完整数学推导（2 连杆平面臂 + z4 偏移） |
+| [docs/localization.md](docs/localization.md) | 定位系统文档：SLAM 建图、AMCL 定位、示教导航、故障排查 |
+| [docs/box_detection_grasp.md](docs/box_detection_grasp.md) | YOLO 方框检测与自动抓取：配置、服务、深度估计原理 |
+| [ros2/README.md](ros2/README.md) | ROS 2 节点详细文档 |
+| [tools/README.md](tools/README.md) | 命令行工具完整参数与工作流示例 |
 
 ## 依赖
 
 | 环境 | 依赖 |
 |------|------|
 | ESP32 | PlatformIO, Arduino framework, esp32-camera |
-| ROS 2 | ROS 2 Humble (robostack), Python 3.12 |
+| ROS 2 | ROS 2 Humble, `slam_gmapping`, `nav2_amcl`, `nav2_map_server`, `nav2_lifecycle_manager`, `serial` |
-| 工具 | NumPy, OpenCV (可选) |
+| Python | NumPy, OpenCV, Ultralytics (YOLO), pynput (可选) |
+| 系统 | YDLidar-SDK, colcon, conda (robostack) |
 
 ## 许可
 

docs/box_detection_grasp.md

@@ -0,0 +1,290 @@
+# 方框检测与自动抓取节点
+
+基于 YOLO 模型检测方框，并自动调用视觉抓取节点完成抓取。
+
+## 功能特性
+
+- ✅ 实时 YOLO 方框检测
+- ✅ 自动/手动模式切换
+- ✅ 检测成功后自动停止识别
+- ✅ 自动发布抓取目标到 `/vision_grasp/grasp_target`
+- ✅ 3D 坐标估计（基于方框尺寸）
+
+## 快速开始
+
+### 1. 启动必要的节点
+
+```bash
+# 终端 1: 启动 arm_control 节点
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+
+# 终端 2: 启动 vision_grasp 节点
+ros2 run udp_teleop vision_grasp \
+    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+```
+
+### 2. 启动方框检测节点
+
+#### 自动模式（检测到方框后自动抓取）
+
+```bash
+ros2 run udp_teleop box_detection_grasp \
+    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+    -p auto_grasp:=true
+```
+
+#### 手动模式（仅检测，不自动抓取）
+
+```bash
+ros2 run udp_teleop box_detection_grasp \
+    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+    -p auto_grasp:=false
+```
+
+## 工作流程
+
+### 自动模式 (auto_grasp=true)
+
+1. 节点启动后立即开始实时检测
+2. 检测到方框后：
+   - 停止检测（避免重复）
+   - 计算 3D 坐标（相机坐标系，单位：mm）
+   - 发布到 `/vision_grasp/grasp_target`
+   - vision_grasp 节点接收并执行抓取
+3. 等待 10 秒后完成（可自定义）
+4. 节点保持运行但不再检测
+
+### 手动模式 (auto_grasp=false)
+
+- 节点启动后不检测
+- 通过服务触发检测：
+
+```bash
+# 启动检测
+ros2 service call /box_detection/start std_srvs/srv/Trigger
+
+# 停止检测
+ros2 service call /box_detection/stop std_srvs/srv/Trigger
+```
+
+## 服务接口
+
+### `/box_detection/start`
+
+启动方框检测。
+
+**类型**：`std_srvs/srv/Trigger`
+
+**示例**：
+```bash
+ros2 service call /box_detection/start std_srvs/srv/Trigger
+```
+
+**响应**：
+- `success: true` - 检测已启动
+- `success: false` - 检测已在运行或抓取进行中
+
+### `/box_detection/stop`
+
+停止方框检测。
+
+**类型**：`std_srvs/srv/Trigger`
+
+**示例**：
+```bash
+ros2 service call /box_detection/stop std_srvs/srv/Trigger
+```
+
+**响应**：
+- `success: true` - 检测已停止
+- `success: false` - 检测未运行
+
+## 使用场景
+
+### 场景 1：自动抓取流水线
+
+```bash
+# 启动自动模式
+ros2 run udp_teleop box_detection_grasp \
+    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+    -p auto_grasp:=true
+
+# 节点自动检测并抓取，无需人工干预
+```
+
+### 场景 2：手动触发抓取
+
+```bash
+# 终端 1: 启动手动模式
+ros2 run udp_teleop box_detection_grasp \
+    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+    -p auto_grasp:=false
+
+# 终端 2: 需要抓取时手动触发
+ros2 service call /box_detection/start std_srvs/srv/Trigger
+
+# 检测到方框后自动抓取，完成后停止
+# 需要再次抓取时，再次调用 start 服务
+ros2 service call /box_detection/start std_srvs/srv/Trigger
+```
+
+### 场景 3：紧急停止
+
+```bash
+# 在检测过程中紧急停止
+ros2 service call /box_detection/stop std_srvs/srv/Trigger
+```
+
+## 配置参数
+
+编辑 `config/box_detection_grasp.yaml`：
+
+```yaml
+box_detection_grasp:
+  ros__parameters:
+    # 相机流
+    stream_url: "http://192.168.4.1/stream"
+    
+    # 模型路径
+    model_path: "/path/to/model.pt"
+    
+    # 检测参数
+    confidence: 0.35        # 置信度阈值
+    imgsz: 768             # YOLO 输入尺寸
+    device: ""             # "cpu", "cuda", 或 "" (自动)
+    detection_rate: 10.0   # 检测频率 (Hz)
+    
+    # 方框尺寸（用于深度估计）
+    box_size_m: 0.03       # 方框边长 (米)
+    
+    # 相机参数
+    horizontal_fov_deg: 66.0  # 水平视场角
+    
+    # 调试
+    show_debug_window: false  # 显示检测窗口
+    
+    # 控制
+    auto_grasp: false      # 自动抓取开关
+```
+
+## 调试
+
+### 显示检测窗口
+
+```bash
+ros2 run udp_teleop box_detection_grasp \
+    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+    -p auto_grasp:=true \
+    -p show_debug_window:=true
+```
+
+### 监控话题
+
+```bash
+# 查看抓取目标
+ros2 topic echo /vision_grasp/grasp_target
+
+# 查看日志
+ros2 node list
+ros2 node info /box_detection_grasp
+```
+
+## 坐标系说明
+
+- **输入**：YOLO 检测框（图像像素坐标）
+- **输出**：相机坐标系 3D 坐标（单位：mm）
+  - X: 向右
+  - Y: 向下
+  - Z: 向前（深度）
+- **传递**：vision_grasp 节点自动转换到基坐标系
+
+## 深度估计原理
+
+使用透视投影原理：
+
+```
+Z = (实际尺寸 × 焦距) / 像素尺寸
+X = (u - cx) × Z / fx
+Y = (v - cy) × Z / fy
+```
+
+其中：
+- `box_size_m` = 方框实际边长（米）
+- `horizontal_fov_deg` = 相机水平视场角
+
+## 故障排除
+
+### 问题：无法连接到 ESP32 相机
+
+**解决**：
+1. 检查 ESP32 IP 地址：`ping 192.168.4.1`
+2. 浏览器访问：`http://192.168.4.1`
+3. 修改配置：`-p stream_url:=http://<IP>/stream`
+
+### 问题：检测不到方框
+
+**解决**：
+1. 降低置信度：`-p confidence:=0.25`
+2. 检查模型路径：`ls -lh src/udp_teleop/models/box_detection.pt`
+3. 启用调试窗口：`-p show_debug_window:=true`
+
+### 问题：深度估计不准确
+
+**解决**：
+1. 校准方框尺寸：`-p box_size_m:=0.03`
+2. 校准相机 FOV：`-p horizontal_fov_deg:=66.0`
+
+## 依赖
+
+- ROS 2 Humble
+- Python 3.8+
+- OpenCV (`pip install opencv-python`)
+- NumPy (`pip install numpy`)
+- Ultralytics YOLO (`pip install ultralytics`)
+
+## 示例：完整启动脚本
+
+```bash
+#!/bin/bash
+# 启动完整的方框检测与抓取系统
+
+# 确保在 ros2 目录
+cd ~/Dev/craic/ros2
+source install/setup.bash
+
+# 启动 arm_control
+gnome-terminal -- bash -c "
+    source install/setup.bash
+    ros2 run udp_teleop arm_control \
+        --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+    exec bash"
+
+sleep 2
+
+# 启动 vision_grasp
+gnome-terminal -- bash -c "
+    source install/setup.bash
+    ros2 run udp_teleop vision_grasp \
+        --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+    exec bash"
+
+sleep 2
+
+# 启动方框检测（自动抓取模式）
+gnome-terminal -- bash -c "
+    source install/setup.bash
+    ros2 run udp_teleop box_detection_grasp \
+        --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
+        -p auto_grasp:=true \
+        -p show_debug_window:=true
+    exec bash"
+
+echo "所有节点已启动！"
+```
+
+保存为 `launch_box_grasp.sh` 并运行：
+```bash
+chmod +x launch_box_grasp.sh
+./launch_box_grasp.sh
+```

docs/localization.md

@@ -0,0 +1,256 @@
+# CRAIC 定位系统文档
+
+激光 SLAM 建图 + AMCL 精准定位 + 示教记录点位。移植自 `lzu_robot` / `move_try`
+（同一赛事的参考实现），适配 CRAIC 麦轮底盘 + ESP32 + 机械臂平台。
+
+---
+
+## 1. 概述
+
+- **目标**：在已知（预先测量）的竞赛场地上对底盘做绝对、无累积漂移的精准定位，并提供示教记录关键点位的工具。
+- **方案**：YDLiDAR → `rf2o` 激光里程计提供连续 `odom`；`slam_gmapping` 建一次场地地图；`nav2 AMCL` 用激光匹配已知地图做全局校正。
+- **运行环境**：ROS 2 Humble（conda/robostack `ros2_humble`）。机器人端工作区在 `~/Desktop/ros2`（下文命令默认在此目录、且已 `source install/setup.bash`）。
+
+> 设计上原打算用**轮式里程计**做运动来源，但实测 CRAIC 底盘板上电后只短时上报里程计（见 §8），
+> 故改用 `rf2o`（激光里程计）提供 `odom→base`，与 `lzu_robot`/`move_try` 一致。轮式里程计节点保留，
+> 待底盘板问题解决后可用 EKF 融合提精度。
+
+---
+
+## 2. 系统架构
+
+### TF 树
+```
+map ──(amcl，激光匹配已知地图)──> odom
+odom ──(rf2o，来自连续 /scan)──────> base_footprint
+base_footprint ──(静态外参)────────> laser_frame      # 激光安装位，lidar_yaw 等参数
+base_footprint ──(静态恒等)────────> base_link        # 兼容 gmapping/Nav2 默认基准帧
+```
+
+### 数据流
+```
+YDLiDAR ──/dev/ttylzulaser──> ydlidar 驱动 ──> /scan(10Hz)
+/scan ──> rf2o ──> /odom + TF(odom→base_footprint)
+/scan + odom ──> slam_gmapping ──> /map + TF(map→odom)        # 建图阶段
+/scan + 已知地图 ──> amcl ──> TF(map→odom) = 绝对位姿            # 定位阶段
+底盘速度命令: keyboard_control ──UDP──> ESP32 ──UART──> 底盘板   # 与定位互不干扰
+轮式里程计(可选): 底盘板 ──/dev/ttylzucar──> chassis_odometry ──> /odom(独立测试用)
+```
+
+---
+
+## 3. 软件清单
+
+### 自建包 `ros2/src/craic_localization`
+| 文件 | 作用 |
+|---|---|
+| `craic_localization/chassis_odometry.py` | 轮式里程计节点（读 `/dev/ttylzucar`，发 `/odom`+TF）。**默认不参与建图/定位**，独立测试用 |
+| `craic_localization/teach_points.py` | 示教节点：记录 map 系下位姿存 YAML |
+| `craic_localization/navigate_to_point.py` | 导航节点：自动驱动到示教点（读 taught_points.yaml + AMCL 闭环 + UDP 发 XYW） |
+| `craic_localization/show_points.py` | 在 rviz 显示所有示教点位（MarkerArray 箭头+文字标签） |
+| `launch/lidar.launch.py` | YDLiDAR 驱动 + 静态外参(base_footprint→laser_frame、→base_link) |
+| `launch/bringup.launch.py` | 轮式里程计 + 激光 + rviz（传感器自检） |
+| `launch/mapping.launch.py` | 激光 + rf2o + slam_gmapping（建图） |
+| `launch/localization.launch.py` | 激光 + rf2o + map_server + amcl + lifecycle_manager（AMCL 定位） |
+| `config/lidar.yaml` | YDLiDAR TminiPro 参数（端口 `/dev/ttylzulaser`） |
+| `config/gmapping.yaml` | gmapping 建图参数（`/**` 通配键，base_footprint/odom/map） |
+| `config/amcl.yaml` | map_server + amcl 参数（omni 运动模型） |
+| `rviz/localization.rviz` | rviz 配置（TF/Map/LaserScan/Odometry） |
+| `maps/` | 建图存盘输出（`craic.pgm` + `craic.yaml`） |
+| `config/taught_points.yaml` | 示教输出（运行时生成） |
+
+### 移植包
+- `ros2/src/ydlidar_ros2_driver` —— 来自 `move_try`，依赖系统 `ydlidar_sdk`（已装在 conda env）。
+- `ros2/src/rf2o_laser_odometry` —— 来自 `lzu_robot`，激光里程计（ICRA'16）。
+
+### 外部依赖（Nav2，已装入 conda env）
+`nav2_amcl`(amcl)、`nav2_map_server`(map_server, map_saver_cli)、`nav2_lifecycle_manager`(lifecycle_manager)、`slam_gmapping`/`openslam_gmapping`。
+
+---
+
+## 4. 环境准备与编译
+
+一次性安装 Nav2 定位组件（若环境无）：
+```bash
+conda install -n ros2_humble -c robostack-staging -c conda-forge \
+    ros-humble-nav2-amcl ros-humble-nav2-map-server ros-humble-nav2-lifecycle-manager
+```
+
+编译（在工作区根目录）：
+```bash
+conda activate ros2_humble
+cd ~/Desktop/ros2
+colcon build --symlink-install --packages-select \
+    ydlidar_ros2_driver rf2o_laser_odometry craic_localization
+source install/setup.bash
+```
+
+> **开发机 ↔ 机器人同步**：代码在开发机维护，需同步到机器人 `~/Desktop/ros2` 后**重新 colcon build**。
+> 新增/改动包时务必确认源码已同步（尤其新包 `ydlidar_ros2_driver`、`rf2o_laser_odometry`）。
+
+---
+
+## 5. 硬件接口
+
+| 设备 | 接口 | 说明 |
+|---|---|---|
+| 底盘 MCU | `/dev/ttylzucar` @115200 | 上报 32 字节里程计包；命令通过 ESP32 转发，非直连 |
+| 激光雷达 | `/dev/ttylzulaser` @230400 | YDLiDAR TminiPro，发 `/scan` 10Hz |
+| 底盘命令 | UDP `192.168.4.1:8888` → ESP32 → UART | `XYW:<x>:<y>:<w>:XZHY`（麦轮速度） |
+
+**底盘里程计协议**（移植自 lzu seriallzucar）：32 字节，帧尾 `'L''Z''U'`；`packet[0]`=校验和=`sum(packet[1:30])%256`；
+`packet[1:29]`=`<fffffff>`=`x,y,yaw,4轮位置`(x/y 单位 mm)；`packet[29]`=模式字节，`'D'`=有效。
+打开串口后下发 `UP0LZU` 使能上报（参数 `enable_cmd`）。
+
+---
+
+## 6. 使用流程
+
+> 前置：每个终端都先 `cd ~/Desktop/ros2 && source install/setup.bash`。
+
+### 6.1 传感器自检（可选）
+```bash
+ros2 launch craic_localization bringup.launch.py
+```
+rviz 看 `/scan`、TF。用于核对激光朝向（调 `lidar_yaw` 等，见 §7）。
+
+### 6.2 建图
+```bash
+# 终端1：建图栈（激光 + rf2o + gmapping + rviz）
+ros2 launch craic_localization mapping.launch.py
+# 终端2：遥控慢速走遍场地（见 6.6）
+# 终端3：地图满意后存盘（gmapping 保持运行）
+mkdir -p src/craic_localization/maps
+ros2 run nav2_map_server map_saver_cli \
+    -f src/craic_localization/maps/craic \
+    --ros-args -p save_map_timeout:=10000.0
+```
+**走图要点**：慢、贴墙走遍每条边界、最后绕回起点；rviz Fixed Frame 设 `map`，墙应是单条清晰线、无重影。
+**起点即原点**：建图启动时机器人所在位置=map(0,0)、正前方=map +X。建议从固定起点开始。
+
+存盘得到 `craic.pgm` + `craic.yaml`。**确认 `craic.yaml` 含 `free_thresh`**（缺则 map_server 加载失败）：
+```bash
+tail -1 src/craic_localization/maps/craic.yaml   # 应为 free_thresh: 0.25
+# 若缺： printf '\nfree_thresh: 0.25\n' >> src/craic_localization/maps/craic.yaml
+```
+存盘后重编一次让默认地图安装：`colcon build --symlink-install --packages-select craic_localization`。
+> 地图四周大片灰色是 gmapping 预留画布（`xmin/xmax ±10` → 20m×20m），属正常，不影响 AMCL。
+
+### 6.3 定位（AMCL）
+```bash
+ros2 launch craic_localization localization.launch.py \
+    map:=$HOME/Desktop/ros2/src/craic_localization/maps/craic.yaml
+# 起点不在地图原点时给初值：加 init_x:= init_y:= init_yaw:=，或 rviz 工具栏 "2D Pose Estimate" 点
+```
+正常表现：rviz(Fixed Frame=`map`) 红色激光点贴合墙体，移动时始终咬住墙、位姿稳定不漂。
+
+### 6.4 定位精度测试
+```bash
+ros2 run tf2_ros tf2_echo map base_footprint    # 实时读 x/y/yaw
+ros2 topic echo /amcl_pose                       # 位姿 + 协方差
+```
+- **重复定位**：标记一物理点记位姿 → 开一圈回同点再记 → 差值 < 3cm 为佳。
+- **定长移动**：精确前进 1.00m，AMCL 报的位移误差应在几 cm 内。
+- (可选) rviz Add → PoseArray → `/particlecloud`，粒子收紧=收敛。
+
+### 6.5 示教记录点位
+```bash
+# 终端1：定位栈(6.3) 终端2：遥控(6.6) 终端3：示教
+ros2 run craic_localization teach_points --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
+```
+交互命令：**回车/`r`** 记录当前预设点并前进；`p` 查看当前位姿；`name <X>` 记自定义点；
+`del <X>` 删除；`list` 列出；`skip`/`back` 跳过/回退；`save` 存盘；`q` 退出。
+预设顺序 `B_1..B_6, C_1, D_1, E_1, F_1`（每记一点自动存盘）。记 A 区用 `name A_1`…，或改 `points` 参数。
+输出（map 系，米/弧度）：
+```yaml
+points:
+  B_1: {x: 1.234, y: 0.456, yaw: 1.571}
+  ...
+```
+**在 rviz 看示教点**：`localization.launch.py` 已自动启动 `show_points`（青色箭头=位置+朝向，白字=点名，话题 `/taught_points`，rviz 内置 MarkerArray 显示）。
+想在示教**过程中实时**看到新点，给定位 launch 传 `points_file:=<你的 taught_points.yaml 绝对路径>`，或单独运行：
+```bash
+ros2 run craic_localization show_points --ros-args \
+    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
+```
+
+### 6.6 自动导航到示教点
+读取 `taught_points.yaml`，用 AMCL 闭环把底盘开到指定点（麦轮 holonomic P 控制，经 UDP 发 XYW）。
+**首次务必先 dry_run 验证方向，再低速实测，手放 Ctrl+C（退出自动停车）。** 前置：定位栈(6.3) 在运行。
+```bash
+# ① 干跑：只打印命令不发，确认逻辑与方向符号
+ros2 run craic_localization navigate_to_point --ros-args \
+    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
+    -p dry_run:=true -p goal:=B_1
+# ② 交互选点（低速实测）
+ros2 run craic_localization navigate_to_point --ros-args \
+    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
+    -p max_linear:=40.0 -p max_angular:=20.0
+# ③ 一次性去某点 / 供上层任务程序用话题指定
+ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=C_1
+ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"
+```
+交互命令：输入点名→导航；`s` 停车；`q` 退出。到达 `pos_tolerance`(5cm)/`yaw_tolerance`(~3°) 容差内停车。
+**若实车方向相反**：翻转 `sign_x`/`sign_y`/`sign_w`（默认 -1，按 keyboard_control 推导）。如左右反了 `-p sign_x:=1`。
+
+### 6.7 遥控
+```bash
+ros2 run udp_teleop keyboard_control --ros-args \
+    --params-file src/udp_teleop/config/params.yaml \
+    -p chassis_linear_speed:=40 -p chassis_angular_speed:=20    # 建图建议降速
+```
+按键：`W/S` 前后、`A/D` 左右平移、`Q/E` 左右转；`Ctrl+C` 退出。须 `ros2 run`、交互终端。
+
+---
+
+## 7. 配置 / 关键参数
+
+**`lidar.launch.py`**：`lidar_x/y/z`、`lidar_yaw`(默认 -3.14159=-180°，使机体 X 轴对准底盘实际前进方向；若前后颠倒改 0.0)、
+`intensity`(true；基础 Tmini 无强度时改 false)、`sample_rate`(4)、`baudrate`(230400)、`base_frame`、`laser_frame`。
+
+**`localization.launch.py`**：`map`(地图 yaml)、`init_x/init_y/init_yaw`(初始位姿)、`use_rviz`。
+
+**`mapping.launch.py`**：`use_rviz`。范围 `xmin/xmax/ymin/ymax`(±10) 在 `config/gmapping.yaml`，可收小到 ±3 得更紧凑地图（需重新建图）。
+
+**`amcl.yaml`**：`odom_model_type: omni`(麦轮)、`base_frame_id: base_footprint`、`laser_max_range: 12.0`、`alpha1..5`(里程计噪声)。
+
+**`chassis_odometry`**（独立测试）：`port`、`enable_cmd`(UP0LZU)、`enable_cmd_period`(0=仅打开时发一次)、`require_mode_d`(true)。
+
+**`navigate_to_point`**：`points_file`、`goal`(一次性目标)、`max_linear`(60)/`max_angular`(30)、`kp_linear`(150)/`kp_angular`(40)、`pos_tolerance`(0.05)/`yaw_tolerance`(0.05)、`sign_x/sign_y/sign_w`(默认 -1，方向修正)、`goal_timeout`(30s)、`dry_run`、`udp_ip/udp_port`。
+
+---
+
+## 8. 故障排查
+
+| 现象 | 原因 | 处理 |
+|---|---|---|
+| 激光狂刷 `Check Sum X != Y` | 有第二个进程在读 `/dev/ttylzulaser`，字节流被撕裂 | `pgrep -af ydlidar`；`pkill -f ydlidar_ros2_driver_node`；保证单实例。若仍报：试 `intensity:=false`（Tmini 无强度） |
+| gmapping `"base_link" ... does not exist` | gmapping 硬编码基准帧 `base_link`，而 TF 树只有 base_footprint | 已修：`lidar.launch.py` 发 base_footprint→base_link 恒等 TF |
+| gmapping `Message Filter ... queue is full`、地图/位置不动 | `odom→base` TF 冻结：轮式里程计板上电后只短时上报 | 已修：改用 `rf2o` 从 /scan 连续提供 odom→base（见 §1） |
+| map_server 加载地图失败 | `craic.yaml` 缺 `free_thresh` | 补 `free_thresh: 0.25`（见 6.2） |
+| rviz 地图上一串绿箭头 | Odometry 显示保留历史(Keep) | 已改默认 `Keep:1`；或 rviz 里取消勾选 Odometry（不影响地图） |
+| 轮式里程计只在上电后几帧有数据 | 底盘板上电后停止上报（固有行为，UP0LZU 未能维持） | 建图/定位已不依赖它；如需启用试 `enable_cmd_period:=1.0` 保活，或确认主机串口 TX 已接、或经 ESP32 通道使能 |
+| 时间戳显示 ~2000 年 | 机器人系统时钟未对时 | 单机不影响；多机协同前用 NTP/RTC 对时 |
+| 导航时往错误方向开 / 原地打转 | XYW 速度符号与实车不一致 | 翻转 `sign_x/sign_y/sign_w`(默认 -1)；先 `dry_run:=true` 核对，再低速实测 |
+| rviz 里机体朝向/前进方向与遥控差 90° | 激光外参 `lidar_yaw` 未校准 | 设 `lidar_yaw=-180°`(前后颠倒则 0.0)；改后 **yaw 变了需重新示教**，建议重建图 |
+| rviz 里示教点/地图整体乱飘跳动(机器人/激光却平滑) | Fixed Frame 设成了 `odom`，map 帧内容随 AMCL 每次校正跳变 | rviz Fixed Frame 改 `map`(已设为默认) |
+| 平移一段再返回，回位坐标误差大(静止时稳定) | 激光里程计 rf2o 运动跟踪偏差 + 麦轮物理漂移；**非地图大小问题** | 慢速驱动；根治用轮式里程计+EKF(见 §9)；先做物理标记/对比 rf2o 与 AMCL 定位区分原因 |
+| 激光点云形状对、但整体偏离墙线，AMCL 不往墙上贴 | `sigma_hit` 太小(0.05)→偏差超几 cm 就无梯度，AMCL 无法纠正 | 调大 `sigma_hit`(0.2)、`z_hit`(0.9)、`laser_max_range`(≥场地对角线)、`laser_likelihood_max_dist`(2.0)；并给准初始位姿 |
+
+通用排查：`ros2 run tf2_ros tf2_echo map base_footprint`、`ros2 topic hz /scan /odom /map`、
+`ros2 run tf2_tools view_frames`（看 TF 树连通）。
+
+---
+
+## 9. 已知限制与后续
+
+- **激光外参标定**：`lidar_yaw` 已校准为 -180°，使机体 X 轴对准底盘实际前进方向（rviz 显示与遥控一致，导航默认符号 `sign_x/y/w=-1` 也随之正确）。平移 `lidar_x/y` 仍为 0 占位，需要更高位置精度可再标。
+  **改动 `lidar_yaw` 后**：机体朝向变了 —— 旧地图仍可定位（AMCL 自动补偿），但**示教点的 yaw 需重新记录**，建议重新建图让地图朝向也一致。
+- **轮式里程计**：底盘板“持续上报”问题待解；解决后可用 `robot_localization` EKF 融合 轮速 + rf2o 再喂 AMCL 提精度。
+- **地图坐标系**：`map(0,0)` = 建图起点。比赛复现时让机器人从同一物理起点开机，或用 “2D Pose Estimate” 给初值。
+
+---
+
+## 10. 参考来源
+- `lzu_robot/src/cgbot/cgbot/seriallzucar.py`（里程计协议）、`maps/amcl_config.yaml`、`src/slam_gmapping`、`src/rf2o_laser_odometry`、`src/ydlidar_ros2_driver-humble`
+- `move_try/src/ydlidar_ros2_driver/params/TminiPro.yaml`、`src/move_try`（同赛事场地/规划参考）

jxbeye/src/main.cpp

@@ -366,7 +366,17 @@ void setup() {
     Serial.print("🌐 IP:   "); Serial.println(WiFi.localIP());
     Serial.printf("🌐 访问：http://%s\n", WiFi.localIP().toString().c_str());
   } else {
-    Serial.println("\n❌ WiFi 连接失败，继续运行 (无网络)");
+    Serial.println("\n❌ STA 连接失败，切换到 AP 模式...");
+    WiFi.mode(WIFI_AP);
+    if (WiFi.softAP("ESP32-S3-Camera", "12345678")) {
+      Serial.println("✅ AP 模式已启动");
+      Serial.println("📶 SSID: ESP32-S3-Camera");
+      Serial.println("🔑 密码: 12345678");
+      Serial.print("🌐 IP:   "); Serial.println(WiFi.localIP());
+      Serial.printf("🌐 访问：http://%s\n", WiFi.localIP().toString().c_str());
+    } else {
+      Serial.println("❌ AP 启动失败，继续运行 (无网络)");
+    }
   }
   
   Serial.println("\n[3] 摄像头初始化 (SVGA)...");

ros2/AGENTS.md

@@ -0,0 +1,39 @@
+# Repository Guidelines
+
+## Project Structure & Module Organization
+This repository is a ROS 2 workspace. Keep hand-written code under `src/`; treat top-level `build/`, `install/`, and `log/` as generated output. Core packages include `src/arm_control_msgs` for ROS messages/services, `src/udp_teleop` for arm and vision Python nodes, `src/craic_localization` for localization/navigation Python nodes, and C++ sensor packages such as `src/rf2o_laser_odometry` and `src/ydlidar_ros2_driver`. Launch files live in package `launch/`, runtime parameters in `config/` or `params/`, and RViz assets in `rviz/`.
+
+## Build, Test, and Development Commands
+Use the workspace script for normal builds:
+```bash
+./build.sh
+source install/setup.bash
+```
+`build.sh` sources ROS 2 Humble and runs `colcon build --symlink-install`.
+
+For targeted builds, prefer:
+```bash
+colcon build --packages-select craic_localization udp_teleop
+colcon build --packages-select arm_control_msgs --cmake-args -DPython_EXECUTABLE=$CONDA_PREFIX/bin/python
+```
+Run nodes with `ros2 run`, for example:
+```bash
+ros2 run udp_teleop arm_control --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+```
+
+## Coding Style & Naming Conventions
+Python uses 4-space indentation, `snake_case` modules, and `snake_case` console script names such as `navigate_to_point`. ROS interfaces use `PascalCase` (`MovePose.srv`, `TCPPose.msg`). Launch files should end in `.launch.py`. Follow existing ROS 2 defaults: concise docstrings, parameter-driven behavior, and package-local config files. C++ packages build with C++14 and warnings enabled (`-Wall -Wextra -Wpedantic`); keep headers in `include/<package>/` and source in `src/`.
+
+## Testing Guidelines
+Run package tests before opening a PR:
+```bash
+colcon test --packages-select udp_teleop craic_localization
+colcon test-result --verbose
+```
+`udp_teleop/test/` currently contains ROS linter tests for `flake8` and `pep257`. Add new Python tests under `test/` with `test_*.py` names. For C++ packages, enable `BUILD_TESTING`-compatible tests when adding nontrivial logic.
+
+## Commit & Pull Request Guidelines
+Recent history follows Conventional Commits, often with scopes: `feat(craic_localization): ...`, `chore: ...`, `docs: ...`. Keep subjects imperative and under one line. PRs should describe affected packages, runtime impact, and exact validation commands. Include screenshots or RViz captures when changing visualization, launch behavior, or operator-facing workflows.
+
+## Generated Files & Configuration
+Do not commit edits inside generated `build/`, `install/`, or `log/` directories. Update source files instead, then rebuild. Keep machine-specific IPs, ports, and calibration values in package YAML files under `config/` or `params/`, and document any hardware assumptions in the PR.

ros2/README.md

@@ -23,6 +23,9 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 **功能**：
 - 关节空间和笛卡尔空间运动控制
 - 完整的逆运动学和正运动学
+- **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态
+  - `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）
 
@@ -31,9 +34,14 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 
 **功能**：
 - 相机坐标到基坐标系的自动转换
+- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换
 - 抓取流程：释放 → 移动 → 抓取 → 回收
 - 释放流程：移动 → 释放 → 回收
 
+**相机旋转说明**：
+- J5 < 0°（闭合/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)`
+- J5 > 0°（张开/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)`
+
 ## 🚀 快速开始
 
 ### 编译

ros2/build.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+set -e
+cd "$(dirname "$0")"
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install "$@"
+echo ""
+echo "Done. Source with: source install/setup.bash"

ros2/src/arm_control_msgs/srv/MovePose.srv

@@ -4,12 +4,14 @@ float64 y           # 目标 Y 坐标 (mm)
 float64 z           # 目标 Z 坐标 (mm)
 float64 phi         # 目标偏航角 (度)
 bool elbow_up       # 是否使用肘部向上解
-uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
+bool up             # 夹爪朝上（z4=-100）
-bool grip           # 是否抓取
+bool down           # 夹爪朝下（z4=55）
-bool release        # 是否释放
+bool grip           # 是否抓取（J6=-5）
+bool release        # 是否释放（J6=80）
 float64 duration    # 运动时长 (秒，0 表示使用默认值)
 
-# 夹爪状态常量
+# 已废弃：使用 up/down 代替
+uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
 uint8 GRIPPER_KEEP = 0
 uint8 GRIPPER_OPEN = 1
 uint8 GRIPPER_CLOSED = 2

ros2/src/craic_localization/config/amcl.yaml

@@ -0,0 +1,60 @@
+# P5 AMCL 定位参数 —— 改自 lzu_robot/maps/amcl_config.yaml
+# 麦轮底盘用 omni 运动模型；帧与 chassis_odometry / 激光一致。
+# 地图文件由 localization.launch.py 的 map:= 参数注入 map_server 的 yaml_filename。
+
+map_server:
+  ros__parameters:
+    use_sim_time: false
+    yaml_filename: ""        # 由 launch 的 map:= 覆盖
+    topic_name: "map"
+    frame_id: "map"
+
+amcl:
+  ros__parameters:
+    use_sim_time: false
+    # ===== 帧 / 话题 =====
+    odom_model_type: "omni"          # 麦轮全向；差速底盘改 "diff"
+    global_frame_id: "map"
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    scan_topic: "scan"
+
+    # ===== 运动模型噪声 =====
+    alpha1: 0.05
+    alpha2: 0.05
+    alpha3: 0.1
+    alpha4: 0.1
+    alpha5: 0.05
+
+    # ===== 粒子滤波 =====
+    min_particles: 500
+    max_particles: 1000
+    pf_err: 0.05
+    resample_interval: 5
+
+    # ===== 激光模型（匹配 5×5m 边界）=====
+    laser_model_type: "likelihood_field"
+    laser_max_range: 8.0            # 4×4m 场地对角 ~5.7m，留余量，避免远墙光束被裁掉
+    laser_min_range: -1.0
+    max_beams: 500
+    sigma_hit: 0.2                  # 关键：原 0.05 太尖锐，偏差>几cm 就无梯度、AMCL 拉不回来；0.2 加宽收敛域
+    z_hit: 0.9                      # 加大“命中”权重，更用力把激光贴到墙上
+    z_rand: 0.1
+    laser_likelihood_max_dist: 2.0  # 似然场范围，加宽收敛域
+
+    # ===== 更新策略 =====
+    update_min_d: 0.02
+    update_min_a: 0.03
+    transform_tolerance: 0.5
+
+    # ===== 恢复：小场地关闭随机粒子注入 =====
+    recovery_alpha_slow: 0.0
+    recovery_alpha_fast: 0.0
+
+    # ===== 初始位姿（按机器人在场地中的起始点设置；可被 launch 覆盖）=====
+    set_initial_pose: true
+    initial_pose:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+      yaw: 0.0

ros2/src/craic_localization/config/gmapping.yaml

@@ -0,0 +1,44 @@
+# gmapping(slam_gmapping) 建图参数 —— 为 4×4m 场地优化
+# 帧：base_frame=base_footprint, odom_frame=odom, map_frame=map
+# 地图缩减到 5×5m（场地 4×4m + 各 0.5m 余量），栅格分辨率 1cm
+/**:
+  ros__parameters:
+    base_frame: base_footprint
+    odom_frame: odom
+    map_frame: map
+    map_update_interval: 5.0
+    maxUrange: 4.0
+    maxRange: 5.0
+    delta: 0.01
+    particles: 50
+    linearUpdate: 0.1
+    angularUpdate: 0.2
+    temporalUpdate: 1.0
+    resampleThreshold: 0.5
+    minimum_score: 0.0
+    # 运动模型噪声
+    srr: 0.1
+    srt: 0.2
+    str: 0.1
+    stt: 0.2
+    # 扫描匹配
+    sigma: 0.05
+    kernelSize: 1
+    lstep: 0.05
+    astep: 0.05
+    iterations: 5
+    lsigma: 0.075
+    ogain: 3.0
+    lskip: 0
+    llsamplerange: 0.01
+    llsamplestep: 0.01
+    lasamplerange: 0.005
+    lasamplestep: 0.005
+    # 地图范围（米）—— 4×4m 场地 + 0.5m 余量
+    xmin: -0.5
+    xmax: 4.5
+    ymin: -0.5
+    ymax: 4.5
+    occ_thresh: 0.25
+    transform_publish_period: 0.05
+    use_sim_time: false

ros2/src/craic_localization/config/lidar.yaml

@@ -0,0 +1,24 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttylzulaser
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 4
+    intensity_bit: 8
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: true
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 12.0
+    range_min: 0.03
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/craic_localization/craic_localization/chassis_odometry.py

@@ -0,0 +1,272 @@
+#!/usr/bin/env python3
+"""底盘轮式里程计节点 (chassis_odometry)
+
+直连底盘 MCU (lzucar 板) 串口 `/dev/ttylzucar`，**只读**解析 32 字节里程计数据包，
+发布 nav_msgs/Odometry (/odom) 与 TF (odom -> base_footprint)。
+
+数据包协议（移植自 lzu_robot/src/cgbot/cgbot/seriallzucar.py，已上真机验证的同款底盘）：
+  - 帧尾标志 3 字节: 'L', 'Z', 'U'
+  - 整包 32 字节，以 'LZU' 结尾对齐
+  - packet[0]      = 校验和 = sum(packet[1:30]) % 256
+  - packet[1:29]   = <fffffff> = x, y, yaw, posFR, posFL, posBL, posBR
+                     (x/y 单位 mm，需 /1000 转米; yaw 单位弧度)
+  - packet[29]     = 模式字节, 'D' 表示航位推算有效
+  - packet[29:32]  = 帧尾 'L','Z','U'（对齐用）
+
+本节点不发送任何命令；既有底盘速度控制 (UDP->ESP32->UART) 不受影响，互不抢占串口。
+若后续需把底盘命令也迁到直连串口，应在此节点内合并发送逻辑（参考 seriallzucar 的 pack_floats_send）。
+"""
+
+import math
+import struct
+import time
+from threading import Lock, Thread
+
+import rclpy
+import serial
+from geometry_msgs.msg import Quaternion, TransformStamped
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from tf2_ros import TransformBroadcaster
+
+PACKET_SIZE = 32
+MODE_DEAD_RECKON = ord('D')
+
+
+def yaw_to_quaternion(yaw: float) -> Quaternion:
+    """绕 Z 轴偏航角 -> 四元数。"""
+    q = Quaternion()
+    q.x = 0.0
+    q.y = 0.0
+    q.z = math.sin(yaw / 2.0)
+    q.w = math.cos(yaw / 2.0)
+    return q
+
+
+class ChassisOdometry(Node):
+    def __init__(self) -> None:
+        super().__init__('chassis_odometry')
+
+        # ---- 参数 ----
+        self.port = self.declare_parameter('port', '/dev/ttylzucar').value
+        self.baudrate = int(self.declare_parameter('baudrate', 115200).value)
+        self.odom_frame_id = self.declare_parameter('odom_frame_id', 'odom').value
+        self.base_frame_id = self.declare_parameter('base_frame_id', 'base_footprint').value
+        self.odom_topic = self.declare_parameter('odom_topic', 'odom').value
+        self.publish_tf = bool(self.declare_parameter('publish_tf', True).value)
+        # 仅当模式字节为 'D'(航位推算有效) 时才发布；置 False 可在调试期放行所有包
+        self.require_mode_d = bool(self.declare_parameter('require_mode_d', True).value)
+        # 串口打开后下发的使能命令，让底盘板持续上报里程计；空字符串=不发。
+        # lzu_robot/seriallzucar.open() 与 move_try/serial_bridge 都在打开后发 "UP0LZU"，
+        # 缺它则底盘上电后只发几帧就停，odom TF 变陈旧、gmapping 丢弃所有 scan。
+        self.enable_cmd = str(self.declare_parameter('enable_cmd', 'UP0LZU').value)
+        # >0 时按该周期(秒)重发使能命令做保活；0=仅打开时发一次(与参考实现一致)
+        self.enable_cmd_period = float(self.declare_parameter('enable_cmd_period', 0.0).value)
+
+        # ---- 发布器 ----
+        self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 10)
+        self.tf_broadcaster = TransformBroadcaster(self) if self.publish_tf else None
+
+        # ---- 串口接收环形缓冲 ----
+        self.BUFFER_SIZE = 1024
+        self.buffer = bytearray(self.BUFFER_SIZE)
+        self.write_index = 0
+        self.buffer_lock = Lock()
+
+        # ---- 上一帧状态（用于差分求速度）----
+        self.prev_x = None
+        self.prev_y = None
+        self.prev_yaw = None
+        self.prev_t = None
+
+        # ---- 串口 + 接收线程 ----
+        self.ser = None
+        self._running = True
+        self._reader = Thread(target=self._receiver_thread, daemon=True)
+        self._reader.start()
+
+        # 可选：周期性重发使能命令做保活
+        if self.enable_cmd and self.enable_cmd_period > 0.0:
+            self.create_timer(self.enable_cmd_period, self._send_enable_cmd)
+
+        self.get_logger().info(
+            f'chassis_odometry 启动: port={self.port}@{self.baudrate} '
+            f'odom_frame={self.odom_frame_id} base_frame={self.base_frame_id} '
+            f'publish_tf={self.publish_tf}'
+        )
+
+    # ------------------------------------------------------------------ #
+    # 串口接收 / 重连
+    # ------------------------------------------------------------------ #
+    def _open_serial(self) -> bool:
+        try:
+            self.ser = serial.Serial(self.port, self.baudrate, timeout=1)
+            self.get_logger().info(f'串口已连接 {self.port}')
+            self._send_enable_cmd()
+            return True
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warning(f'串口打开失败 {self.port}: {exc}')
+            self.ser = None
+            return False
+
+    def _send_enable_cmd(self) -> None:
+        """下发里程计使能命令(默认 UP0LZU)；空命令或串口未开则跳过。"""
+        if not self.enable_cmd or self.ser is None or not self.ser.is_open:
+            return
+        try:
+            self.ser.write(self.enable_cmd.encode('utf-8'))
+            self.ser.flush()
+            self.get_logger().info(f'已下发里程计使能命令: {self.enable_cmd}')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warning(f'使能命令发送失败: {exc}')
+
+    def _receiver_thread(self) -> None:
+        while self._running:
+            try:
+                if self.ser is None or not self.ser.is_open:
+                    if not self._open_serial():
+                        time.sleep(1.0)
+                    continue
+                waiting = self.ser.in_waiting
+                if waiting > 0:
+                    self._write_to_buffer(self.ser.read(waiting))
+                else:
+                    time.sleep(0.005)
+            except serial.SerialException as exc:
+                self.get_logger().warning(f'串口中断，准备重连: {exc}')
+                self._close_serial()
+                time.sleep(1.0)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f'接收线程异常: {exc}')
+                self._close_serial()
+                time.sleep(1.0)
+
+    def _close_serial(self) -> None:
+        if self.ser is not None:
+            try:
+                self.ser.close()
+            except Exception:  # noqa: BLE001
+                pass
+        self.ser = None
+
+    # ------------------------------------------------------------------ #
+    # 帧解析（逐字节移植自 seriallzucar，保持已验证行为）
+    # ------------------------------------------------------------------ #
+    def _read_byte(self, index: int) -> int:
+        return self.buffer[(index + self.BUFFER_SIZE) % self.BUFFER_SIZE]
+
+    def _read_from_buffer(self, start_index: int, size: int) -> bytearray:
+        data = bytearray()
+        for i in range(size):
+            data.append(self.buffer[(start_index + i) % self.BUFFER_SIZE])
+        return data
+
+    def _write_to_buffer(self, data: bytes) -> None:
+        with self.buffer_lock:
+            for byte in data:
+                self.buffer[self.write_index] = byte
+                # 检测帧尾 'L','Z','U'
+                if (byte == ord('U')
+                        and self._read_byte(self.write_index - 1) == ord('Z')
+                        and self._read_byte(self.write_index - 2) == ord('L')):
+                    start_index = (self.write_index - PACKET_SIZE + self.BUFFER_SIZE) % self.BUFFER_SIZE
+                    packet = self._read_from_buffer(start_index, PACKET_SIZE)
+                    self._process_packet(packet)
+                self.write_index = (self.write_index + 1) % self.BUFFER_SIZE
+
+    def _process_packet(self, packet: bytearray) -> None:
+        # 校验和
+        if (sum(packet[1:30]) % 256) != packet[0]:
+            return
+        try:
+            x, y, yaw, _fr, _fl, _bl, _br = struct.unpack('<fffffff', packet[1:29])
+            mode = packet[29]
+        except struct.error:
+            return
+
+        if self.require_mode_d and mode != MODE_DEAD_RECKON:
+            return
+
+        x /= 1000.0  # mm -> m
+        y /= 1000.0
+        self._publish(x, y, yaw)
+
+    # ------------------------------------------------------------------ #
+    # 发布 Odometry + TF
+    # ------------------------------------------------------------------ #
+    def _publish(self, x: float, y: float, yaw: float) -> None:
+        now = self.get_clock().now()
+        stamp = now.to_msg()
+        quat = yaw_to_quaternion(yaw)
+
+        # 差分求速度（body 系）；首帧或 dt 过小时置零
+        vx = vy = wz = 0.0
+        t = now.nanoseconds * 1e-9
+        if self.prev_t is not None:
+            dt = t - self.prev_t
+            if dt > 1e-3:
+                dx = x - self.prev_x
+                dy = y - self.prev_y
+                dyaw = math.atan2(math.sin(yaw - self.prev_yaw),
+                                  math.cos(yaw - self.prev_yaw))
+                # 世界系位移旋到 body 系
+                vx = (dx * math.cos(yaw) + dy * math.sin(yaw)) / dt
+                vy = (-dx * math.sin(yaw) + dy * math.cos(yaw)) / dt
+                wz = dyaw / dt
+        self.prev_x, self.prev_y, self.prev_yaw, self.prev_t = x, y, yaw, t
+
+        odom = Odometry()
+        odom.header.stamp = stamp
+        odom.header.frame_id = self.odom_frame_id
+        odom.child_frame_id = self.base_frame_id
+        odom.pose.pose.position.x = x
+        odom.pose.pose.position.y = y
+        odom.pose.pose.position.z = 0.0
+        odom.pose.pose.orientation = quat
+        odom.twist.twist.linear.x = vx
+        odom.twist.twist.linear.y = vy
+        odom.twist.twist.angular.z = wz
+        # 平面机器人协方差: x,y,yaw 给小值, z/roll/pitch 不可观给大值
+        odom.pose.covariance[0] = 0.01    # x
+        odom.pose.covariance[7] = 0.01    # y
+        odom.pose.covariance[14] = 1e6    # z
+        odom.pose.covariance[21] = 1e6    # roll
+        odom.pose.covariance[28] = 1e6    # pitch
+        odom.pose.covariance[35] = 0.02   # yaw
+        odom.twist.covariance[0] = 0.01
+        odom.twist.covariance[7] = 0.01
+        odom.twist.covariance[35] = 0.02
+        self.odom_pub.publish(odom)
+
+        if self.tf_broadcaster is not None:
+            tf = TransformStamped()
+            tf.header.stamp = stamp
+            tf.header.frame_id = self.odom_frame_id
+            tf.child_frame_id = self.base_frame_id
+            tf.transform.translation.x = x
+            tf.transform.translation.y = y
+            tf.transform.translation.z = 0.0
+            tf.transform.rotation = quat
+            self.tf_broadcaster.sendTransform(tf)
+
+    def destroy_node(self) -> bool:
+        self._running = False
+        self._close_serial()
+        return super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ChassisOdometry()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/navigate_to_point.py

@@ -0,0 +1,253 @@
+#!/usr/bin/env python3
+"""导航程序 (navigate_to_point)：自动驱动麦轮底盘到示教记录的点位。
+
+- 目标点：读取 teach_points 生成的 taught_points.yaml（map 系 x,y,yaw）。
+- 反馈：TF map->base_footprint（= AMCL 实时定位），需 localization.launch.py 在运行。
+- 控制：holonomic P 控制器，把 map 系位姿误差投影到机体系，生成 (前进, 侧移, 转向)
+        速度，按 udp_teleop 的 XYW 约定经 UDP→ESP32 下发，到达容差内停车。
+
+速度符号默认按 udp_teleop/keyboard_control 推导：
+  前进 W→chassis_y 取负, 左移 A→chassis_x 取负, 左转(CCW) Q→chassis_w 取负
+  => sign_x = sign_y = sign_w = -1。若实车方向相反，翻转对应 sign 参数即可。
+
+用法：
+  # 交互选点
+  ros2 run craic_localization navigate_to_point --ros-args \
+      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
+  # 一次性去某点
+  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=B_1
+  # 运行中用话题指定（供上层任务程序调用）
+  ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"
+  # 干跑（只打印命令不发，安全验证逻辑/符号）
+  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p dry_run:=true
+
+安全：首次务必先 dry_run，再低速单点测试，手放 Ctrl+C（退出会自动发停车）。
+"""
+
+import math
+import socket
+import threading
+
+import rclpy
+import yaml
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from std_msgs.msg import String
+from tf2_ros import (
+    Buffer,
+    ConnectivityException,
+    ExtrapolationException,
+    LookupException,
+    TransformListener,
+)
+
+
+def quat_to_yaw(qx, qy, qz, qw):
+    return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+def norm_angle(a):
+    return math.atan2(math.sin(a), math.cos(a))
+
+
+def clamp(v, lo, hi):
+    return max(lo, min(hi, v))
+
+
+class NavigateToPoint(Node):
+    def __init__(self):
+        super().__init__('navigate_to_point')
+        p = self.declare_parameter
+        self.points_file = p('points_file', 'taught_points.yaml').value
+        self.udp_ip = p('udp_ip', '192.168.4.1').value
+        self.udp_port = int(p('udp_port', 8888).value)
+        self.map_frame = p('map_frame', 'map').value
+        self.base_frame = p('base_frame', 'base_footprint').value
+        self.control_rate = float(p('control_rate', 20.0).value)
+        self.max_linear = float(p('max_linear', 60.0).value)      # 麦轮线速度命令上限(同 keyboard ~100)
+        self.max_angular = float(p('max_angular', 30.0).value)    # 角速度命令上限(同 keyboard ~45)
+        self.kp_linear = float(p('kp_linear', 150.0).value)       # m -> 命令单位
+        self.kp_angular = float(p('kp_angular', 40.0).value)      # rad -> 命令单位
+        self.pos_tol = float(p('pos_tolerance', 0.05).value)      # 到位容差(米)
+        self.yaw_tol = float(p('yaw_tolerance', 0.05).value)      # 到位容差(弧度)
+        self.sign_x = float(p('sign_x', -1.0).value)              # 侧移符号(机体左为正)
+        self.sign_y = float(p('sign_y', -1.0).value)              # 前进符号(机体前为正)
+        self.sign_w = float(p('sign_w', -1.0).value)              # 转向符号(CCW 为正)
+        self.goal_timeout = float(p('goal_timeout', 30.0).value)
+        self.dry_run = bool(p('dry_run', False).value)
+        goal_arg = str(p('goal', '').value)
+
+        self.points = self._load_points()
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+        self._lock = threading.Lock()
+        self.goal_name = None
+        self.goal = None          # (x, y, yaw)
+        self.goal_deadline = None
+        self.one_shot = bool(goal_arg)
+        self.done = False         # one_shot 完成标志
+
+        self.create_subscription(String, 'goto', self._goto_cb, 10)
+        self.timer = self.create_timer(1.0 / self.control_rate, self._control_tick)
+
+        self.get_logger().info(
+            f'navigate_to_point 就绪 (UDP {self.udp_ip}:{self.udp_port}, dry_run={self.dry_run})')
+        if goal_arg:
+            self.set_goal(goal_arg)
+
+    # -------- 点位 / 目标 -------- #
+    def _load_points(self):
+        try:
+            with open(self.points_file, 'r', encoding='utf-8') as f:
+                data = yaml.safe_load(f) or {}
+            pts = dict(data.get('points', {}))
+            self.get_logger().info(f'载入 {len(pts)} 个点: {", ".join(pts) or "(空)"}')
+            return pts
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(f'读取点位文件失败 {self.points_file}: {exc}')
+            return {}
+
+    def set_goal(self, name):
+        pt = self.points.get(name)
+        if pt is None:
+            self.get_logger().warn(f'未知点位: {name}（可用: {", ".join(self.points)}）')
+            return False
+        with self._lock:
+            self.goal_name = name
+            self.goal = (float(pt['x']), float(pt['y']), float(pt.get('yaw', 0.0)))
+            self.goal_deadline = self.get_clock().now() + Duration(seconds=self.goal_timeout)
+        self.get_logger().info(
+            f'前往 {name}: x={self.goal[0]:.3f} y={self.goal[1]:.3f} yaw={self.goal[2]:.3f}')
+        return True
+
+    def _goto_cb(self, msg):
+        self.set_goal(msg.data.strip())
+
+    # -------- 反馈 / 下发 -------- #
+    def _lookup(self):
+        try:
+            t = self.tf_buffer.lookup_transform(self.map_frame, self.base_frame, Time())
+        except (LookupException, ConnectivityException, ExtrapolationException):
+            return None
+        tr = t.transform.translation
+        q = t.transform.rotation
+        return (tr.x, tr.y, quat_to_yaw(q.x, q.y, q.z, q.w))
+
+    def _send(self, cx, cy, cw):
+        cmd = f'XYW:{int(round(cx))}:{int(round(cy))}:{int(round(cw))}:XZHY\n'.encode()
+        if self.dry_run:
+            self.get_logger().info(f'[dry] {cmd.decode().strip()}')
+        else:
+            self.sock.sendto(cmd, (self.udp_ip, self.udp_port))
+
+    def _stop(self):
+        self._send(0, 0, 0)
+
+    # -------- 控制环 -------- #
+    def _control_tick(self):
+        with self._lock:
+            goal, deadline, name = self.goal, self.goal_deadline, self.goal_name
+        if goal is None:
+            return
+        if self.get_clock().now() > deadline:
+            self.get_logger().warn(f'{name} 超时未到达，停车')
+            self._finish()
+            return
+
+        pose = self._lookup()
+        if pose is None:
+            self._stop()
+            self.get_logger().warn('无定位 (map->base_footprint)，停车等待…',
+                                   throttle_duration_sec=2.0)
+            return
+
+        px, py, pyaw = pose
+        tx, ty, tyaw = goal
+        dx, dy = tx - px, ty - py
+        dist = math.hypot(dx, dy)
+        dyaw = norm_angle(tyaw - pyaw)
+
+        if dist < self.pos_tol and abs(dyaw) < self.yaw_tol:
+            self.get_logger().info(
+                f'到达 {name}（位置误差 {dist * 100:.1f}cm，朝向误差 {math.degrees(dyaw):.1f}°）')
+            self._finish()
+            return
+
+        # map 系误差投影到机体系（x 前, y 左, REP-103）
+        e_fwd = dx * math.cos(pyaw) + dy * math.sin(pyaw)
+        e_left = -dx * math.sin(pyaw) + dy * math.cos(pyaw)
+
+        v_fwd = self.kp_linear * e_fwd
+        v_left = self.kp_linear * e_left
+        # 限制平移合速度（防止 x+y 叠加超限）
+        speed = math.hypot(v_fwd, v_left)
+        if speed > self.max_linear:
+            s = self.max_linear / speed
+            v_fwd *= s
+            v_left *= s
+        # 已到位置容差内则只修朝向
+        if dist < self.pos_tol:
+            v_fwd = v_left = 0.0
+        w = clamp(self.kp_angular * dyaw, -self.max_angular, self.max_angular)
+
+        self.get_logger().info(
+            f'{name}: 距离 {dist * 100:.1f}cm, 朝向差 {math.degrees(dyaw):.1f}°',
+            throttle_duration_sec=0.5)
+
+        # 机体速度 -> XYW（侧移=chassis_x, 前进=chassis_y, 转向=chassis_w）
+        self._send(self.sign_x * v_left, self.sign_y * v_fwd, self.sign_w * w)
+
+    def _finish(self):
+        self._stop()
+        with self._lock:
+            self.goal = self.goal_name = self.goal_deadline = None
+        if self.one_shot:
+            self.done = True
+
+    def destroy_node(self):
+        try:
+            self._stop()
+        except Exception:  # noqa: BLE001
+            pass
+        return super().destroy_node()
+
+
+def interactive(node: NavigateToPoint, stop_flag: threading.Event):
+    while not stop_flag.is_set() and rclpy.ok():
+        try:
+            line = input(f'去哪个点? ({", ".join(node.points) or "无"} | q 退出) > ').strip()
+        except EOFError:
+            break
+        low = line.lower()
+        if low in ('q', 'quit', 'exit'):
+            stop_flag.set()
+            break
+        elif low in ('s', 'stop'):
+            node._finish()
+        elif line:
+            node.set_goal(line)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NavigateToPoint()
+    stop_flag = threading.Event()
+    if not node.one_shot:
+        threading.Thread(target=interactive, args=(node, stop_flag), daemon=True).start()
+    try:
+        while rclpy.ok() and not stop_flag.is_set() and not (node.one_shot and node.done):
+            rclpy.spin_once(node, timeout_sec=0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/show_points.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""在 rviz 显示所有示教点位：读 taught_points.yaml，发布 MarkerArray。
+
+每个点画：① 箭头 = 位置 + 朝向(yaw)；② 文字 = 点名。frame_id = map。
+周期性重读文件并发布，因此示教(teach_points)时新增/删除的点会实时反映到 rviz。
+
+rviz 里加 MarkerArray 显示、话题 /taught_points（localization.rviz 已内置该显示）。
+
+用法：
+  ros2 run craic_localization show_points --ros-args \
+      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
+"""
+
+import math
+import os
+
+import rclpy
+import yaml
+from geometry_msgs.msg import Quaternion
+from rclpy.node import Node
+from visualization_msgs.msg import Marker, MarkerArray
+
+
+def yaw_to_quat(yaw):
+    q = Quaternion()
+    q.z = math.sin(yaw / 2.0)
+    q.w = math.cos(yaw / 2.0)
+    return q
+
+
+class ShowPoints(Node):
+    def __init__(self):
+        super().__init__('show_points')
+        self.points_file = os.path.abspath(
+            self.declare_parameter('points_file', 'taught_points.yaml').value)
+        self.frame_id = self.declare_parameter('frame_id', 'map').value
+        self.rate = float(self.declare_parameter('rate', 1.0).value)
+
+        self.pub = self.create_publisher(MarkerArray, 'taught_points', 1)
+        self.timer = self.create_timer(1.0 / self.rate, self._tick)
+        self.get_logger().info(
+            f'show_points: {self.points_file} -> /taught_points (frame={self.frame_id})')
+
+    def _load(self):
+        try:
+            with open(self.points_file, 'r', encoding='utf-8') as f:
+                data = yaml.safe_load(f) or {}
+            return dict(data.get('points', {}))
+        except FileNotFoundError:
+            return {}
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'读取点位失败: {exc}', throttle_duration_sec=5.0)
+            return {}
+
+    def _tick(self):
+        pts = self._load()
+        now = self.get_clock().now().to_msg()
+        arr = MarkerArray()
+
+        # 先清除上一帧（与新增放同一条消息，rviz 一次性处理，无闪烁），自动反映删除
+        clear = Marker()
+        clear.header.frame_id = self.frame_id
+        clear.action = Marker.DELETEALL
+        arr.markers.append(clear)
+
+        for i, (name, p) in enumerate(pts.items()):
+            try:
+                x, y = float(p['x']), float(p['y'])
+                yaw = float(p.get('yaw', 0.0))
+            except (KeyError, TypeError, ValueError):
+                continue
+
+            # 箭头：位置 + 朝向
+            a = Marker()
+            a.header.frame_id = self.frame_id
+            a.header.stamp = now
+            a.ns = 'arrow'
+            a.id = i
+            a.type = Marker.ARROW
+            a.action = Marker.ADD
+            a.pose.position.x = x
+            a.pose.position.y = y
+            a.pose.position.z = 0.05
+            a.pose.orientation = yaw_to_quat(yaw)
+            a.scale.x = 0.25   # 杆长
+            a.scale.y = 0.04   # 杆径
+            a.scale.z = 0.04   # 头径
+            a.color.r, a.color.g, a.color.b, a.color.a = 0.1, 0.8, 1.0, 1.0  # 青色
+            arr.markers.append(a)
+
+            # 文字：点名
+            t = Marker()
+            t.header.frame_id = self.frame_id
+            t.header.stamp = now
+            t.ns = 'label'
+            t.id = i
+            t.type = Marker.TEXT_VIEW_FACING
+            t.action = Marker.ADD
+            t.pose.position.x = x
+            t.pose.position.y = y
+            t.pose.position.z = 0.22
+            t.pose.orientation.w = 1.0
+            t.scale.z = 0.12   # 字高
+            t.color.r, t.color.g, t.color.b, t.color.a = 1.0, 1.0, 1.0, 1.0
+            t.text = name
+            arr.markers.append(t)
+
+        self.pub.publish(arr)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ShowPoints()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/teach_points.py

@@ -0,0 +1,221 @@
+#!/usr/bin/env python3
+"""示教程序 (teach_points)：记录机器人在 map 坐标系下的位姿，存为 YAML 供导航复用。
+
+依赖定位栈在运行(localization.launch.py 提供 AMCL)，位姿来源为 TF: map -> base_footprint
+(= AMCL 实时定位结果)。用法是另开终端用 keyboard_control 把车开到目标点，回到本程序按回车记录。
+
+预设要记录的点(可用参数 points 覆盖)：B_1..B_6, C_1, D_1, E_1, F_1。
+也可随时用 `name <名字>` 记录任意自定义点。
+
+运行：
+  # 终端1：定位栈
+  ros2 launch craic_localization localization.launch.py map:=.../craic.yaml
+  # 终端2：遥控(开车到点)
+  ros2 run udp_teleop keyboard_control --ros-args --params-file src/udp_teleop/config/params.yaml
+  # 终端3：示教(记录)，建议把结果存到包的 config 下供导航读取
+  ros2 run craic_localization teach_points --ros-args \
+      -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
+
+交互命令：
+  回车 / r      记录"当前待记录点"的位姿，并前进到下一个
+  p             只打印当前位姿，不记录
+  name <X>      把当前位姿记录为自定义名字 X(已存在则覆盖)
+  del <X>       删除已记录的点 X
+  list          列出已记录的点
+  skip / back   跳过 / 回退当前待记录点
+  save          立即保存到文件
+  h             帮助
+  q             保存并退出
+"""
+
+import math
+import os
+import threading
+
+import rclpy
+import yaml
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from tf2_ros import (
+    Buffer,
+    ConnectivityException,
+    ExtrapolationException,
+    LookupException,
+    TransformListener,
+)
+
+DEFAULT_POINTS = ['B_1', 'B_2', 'B_3', 'B_4', 'B_5', 'B_6', 'C_1', 'D_1', 'E_1', 'F_1']
+
+
+def quat_to_yaw(qx, qy, qz, qw):
+    """四元数 -> 绕 Z 偏航角(弧度)。"""
+    return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+class TeachPoints(Node):
+    def __init__(self):
+        super().__init__('teach_points')
+        self.map_frame = self.declare_parameter('map_frame', 'map').value
+        self.base_frame = self.declare_parameter('base_frame', 'base_footprint').value
+        self.output_file = os.path.abspath(
+            self.declare_parameter('output_file', 'taught_points.yaml').value)
+        self.points = list(self.declare_parameter('points', DEFAULT_POINTS).value)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.recorded = {}  # name -> {x, y, yaw}
+        self._load_existing()
+
+    # -------- 位姿读取 -------- #
+    def lookup_pose(self):
+        """读取 map->base_frame 的最新变换，返回 ({x,y,yaw}, None) 或 (None, 错误信息)。"""
+        try:
+            t = self.tf_buffer.lookup_transform(
+                self.map_frame, self.base_frame, Time(),
+                timeout=Duration(seconds=1.0))
+        except (LookupException, ConnectivityException, ExtrapolationException) as exc:
+            return None, str(exc)
+        tr = t.transform.translation
+        q = t.transform.rotation
+        yaw = quat_to_yaw(q.x, q.y, q.z, q.w)
+        return {'x': round(tr.x, 4), 'y': round(tr.y, 4), 'yaw': round(yaw, 4)}, None
+
+    # -------- 文件读写 -------- #
+    def _load_existing(self):
+        if os.path.isfile(self.output_file):
+            try:
+                with open(self.output_file, 'r', encoding='utf-8') as f:
+                    data = yaml.safe_load(f) or {}
+                self.recorded = dict(data.get('points', {}))
+                if self.recorded:
+                    print(f'已载入已有记录 {len(self.recorded)} 个点：{self.output_file}')
+            except Exception as exc:  # noqa: BLE001
+                print(f'载入已有文件失败(忽略)：{exc}')
+
+    def save(self):
+        os.makedirs(os.path.dirname(self.output_file) or '.', exist_ok=True)
+        header = (
+            '# 示教记录的点位（map 坐标系；x,y 单位米，yaw 单位弧度）\n'
+            '# 由 craic_localization/teach_points 生成\n'
+        )
+        body = yaml.safe_dump({'points': self.recorded},
+                              default_flow_style=False, sort_keys=False,
+                              allow_unicode=True)
+        with open(self.output_file, 'w', encoding='utf-8') as f:
+            f.write(header + body)
+
+
+HELP = __doc__.split('交互命令：', 1)[1]
+
+
+def interactive(node: TeachPoints):
+    idx = 0
+    print('=' * 60)
+    print('  示教程序 teach_points —— 记录 map 系下位姿')
+    print(f'  输出文件: {node.output_file}')
+    print(f'  预设点: {", ".join(node.points)}')
+    print('  (输入 h 查看命令)')
+    print('=' * 60)
+
+    while rclpy.ok():
+        target = node.points[idx] if idx < len(node.points) else None
+        tag = target if target else '(预设已完，用 name <X> 记录自定义点)'
+        try:
+            line = input(f'[已记录 {len(node.recorded)}] 下一个: {tag} > ').strip()
+        except EOFError:
+            break
+        cmd = line.lower()
+
+        if cmd in ('q', 'quit', 'exit'):
+            node.save()
+            print(f'已保存到 {node.output_file}，退出。')
+            break
+
+        elif cmd in ('', 'r'):
+            if target is None:
+                print('  预设点已记录完。用 "name <X>" 记录自定义点，或 q 退出。')
+                continue
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败(定位栈在运行吗?): {err}')
+                continue
+            node.recorded[target] = pose
+            node.save()
+            print(f'  ✓ {target} = x:{pose["x"]} y:{pose["y"]} '
+                  f'yaw:{pose["yaw"]}rad ({math.degrees(pose["yaw"]):.1f}°)  [已存盘]')
+            idx += 1
+
+        elif cmd == 'p':
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败: {err}')
+            else:
+                print(f'  当前位姿 x:{pose["x"]} y:{pose["y"]} '
+                      f'yaw:{pose["yaw"]}rad ({math.degrees(pose["yaw"]):.1f}°)')
+
+        elif cmd.startswith('name '):
+            name = line[5:].strip()
+            if not name:
+                print('  用法: name <名字>')
+                continue
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败: {err}')
+                continue
+            node.recorded[name] = pose
+            node.save()
+            print(f'  ✓ {name} = x:{pose["x"]} y:{pose["y"]} '
+                  f'yaw:{pose["yaw"]}rad  [已存盘]')
+
+        elif cmd.startswith('del '):
+            name = line[4:].strip()
+            if node.recorded.pop(name, None) is not None:
+                node.save()
+                print(f'  已删除 {name}  [已存盘]')
+            else:
+                print(f'  没有名为 {name} 的记录')
+
+        elif cmd == 'list':
+            if not node.recorded:
+                print('  (空)')
+            for k, v in node.recorded.items():
+                print(f'    {k}: x:{v["x"]} y:{v["y"]} yaw:{v["yaw"]}')
+
+        elif cmd == 'skip':
+            idx += 1
+
+        elif cmd == 'back':
+            idx = max(0, idx - 1)
+
+        elif cmd == 'save':
+            node.save()
+            print(f'  已保存到 {node.output_file}')
+
+        elif cmd in ('h', 'help', '?'):
+            print(HELP)
+
+        else:
+            print('  未知命令，输入 h 查看帮助。')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TeachPoints()
+    # 后台 spin 以填充 TF 缓冲
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+    try:
+        interactive(node)
+    except KeyboardInterrupt:
+        node.save()
+        print(f'\n中断，已保存到 {node.output_file}')
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/launch/bringup.launch.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+"""定位栈总 bringup：轮式里程计 + 激光雷达 + (可选) rviz。
+
+组成完整 TF 树：odom ──(chassis_odometry)──> base_footprint ──(静态外参)──> laser_frame
+用于 P3 激光外参标定与传感器联调：在 rviz 里同时看 /odom 与 /scan，
+调 lidar_yaw/lidar_x/lidar_y 直到激光轮廓与实际场地吻合。
+
+用法：
+  ros2 launch craic_localization bringup.launch.py
+  ros2 launch craic_localization bringup.launch.py use_rviz:=false
+  ros2 launch craic_localization bringup.launch.py lidar_yaw:=-1.5707963 intensity:=true
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('craic_localization')
+    lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
+    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')
+
+    use_rviz = LaunchConfiguration('use_rviz')
+    chassis_port = LaunchConfiguration('chassis_port')
+    rviz_config = LaunchConfiguration('rviz_config')
+    lidar_x = LaunchConfiguration('lidar_x')
+    lidar_y = LaunchConfiguration('lidar_y')
+    lidar_z = LaunchConfiguration('lidar_z')
+    lidar_yaw = LaunchConfiguration('lidar_yaw')
+    intensity = LaunchConfiguration('intensity')
+    sample_rate = LaunchConfiguration('sample_rate')
+    baudrate = LaunchConfiguration('baudrate')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('chassis_port', default_value='/dev/ttylzucar'),
+        DeclareLaunchArgument('rviz_config', default_value=default_rviz),
+        DeclareLaunchArgument('lidar_x', default_value='0.0'),
+        DeclareLaunchArgument('lidar_y', default_value='0.0'),
+        DeclareLaunchArgument('lidar_z', default_value='0.02'),
+        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159'),
+        DeclareLaunchArgument('intensity', default_value='true'),
+        DeclareLaunchArgument('sample_rate', default_value='4'),
+        DeclareLaunchArgument('baudrate', default_value='230400'),
+
+        # 轮式里程计：odom -> base_footprint
+        Node(
+            package='craic_localization',
+            executable='chassis_odometry',
+            name='chassis_odometry',
+            output='screen',
+            parameters=[{'port': chassis_port}],
+        ),
+
+        # 激光雷达 + base_footprint -> laser_frame 静态外参（参数见 lidar.launch.py）
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(lidar_launch),
+            launch_arguments={
+                'lidar_x': lidar_x,
+                'lidar_y': lidar_y,
+                'lidar_z': lidar_z,
+                'lidar_yaw': lidar_yaw,
+                'intensity': intensity,
+                'sample_rate': sample_rate,
+                'baudrate': baudrate,
+            }.items(),
+        ),
+
+        # 可视化（标定 / 联调）
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

ros2/src/craic_localization/launch/lidar.launch.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+"""激光雷达 bringup：启动 YDLiDAR TminiPro 驱动 + 发布 base_footprint→laser_frame 静态外参。
+
+YDLiDAR 驱动节点（ydlidar_ros2_driver_node）内部是普通 rclcpp::Node，启动即在 `scan`
+话题持续发布 sensor_msgs/LaserScan，无需 lifecycle configure/activate 转换。
+
+静态外参默认值仅作 CRAIC 当前安装的初始猜测，需按实车标定。若 scan 形状对、但在地图里整体
+平移或轻微转角对不齐，优先调 `lidar_x/lidar_y/lidar_yaw`。可通过 launch 参数覆盖。
+
+用法：
+  ros2 launch craic_localization lidar.launch.py
+  ros2 launch craic_localization lidar.launch.py lidar_x:=0.03 lidar_y:=-0.01 lidar_yaw:=-3.14159
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.descriptions import ParameterValue
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('craic_localization')
+    default_params = os.path.join(pkg_share, 'config', 'lidar.yaml')
+
+    params_file = LaunchConfiguration('params_file')
+    lidar_x = LaunchConfiguration('lidar_x')
+    lidar_y = LaunchConfiguration('lidar_y')
+    lidar_z = LaunchConfiguration('lidar_z')
+    lidar_yaw = LaunchConfiguration('lidar_yaw')
+    base_frame = LaunchConfiguration('base_frame')
+    laser_frame = LaunchConfiguration('laser_frame')
+    # 决定数据包解析格式的关键参数，作为 launch 覆盖项暴露，便于排查 Check Sum 报错：
+    # 基础 Tmini 无强度 -> intensity:=false；TminiPro 有强度 -> intensity:=true
+    intensity = LaunchConfiguration('intensity')
+    sample_rate = LaunchConfiguration('sample_rate')
+    baudrate = LaunchConfiguration('baudrate')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('params_file', default_value=default_params,
+                              description='YDLiDAR 参数文件路径'),
+        DeclareLaunchArgument('base_frame', default_value='base_footprint'),
+        DeclareLaunchArgument('laser_frame', default_value='laser_frame'),
+        # 外参默认值；按实车标定后可在 launch 命令中覆盖。
+        DeclareLaunchArgument('lidar_x', default_value='0.0'),
+        DeclareLaunchArgument('lidar_y', default_value='0.0'),
+        DeclareLaunchArgument('lidar_z', default_value='0.02'),
+        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159',
+                              description='激光相对底盘偏航角(rad)。-180°=让机体X轴对准底盘实际前进(W键)方向；'
+                                          '若实测前后颠倒，改 0.0'),
+        # 数据包格式覆盖项（排查 Check Sum 报错用）
+        DeclareLaunchArgument('intensity', default_value='true',
+                              description='设备是否输出强度：基础 Tmini=false，TminiPro=true。不匹配会刷 Check Sum 报错'),
+        DeclareLaunchArgument('sample_rate', default_value='4',
+                              description='采样率(K)，Tmini 系列为 4'),
+        DeclareLaunchArgument('baudrate', default_value='230400'),
+
+        Node(
+            package='ydlidar_ros2_driver',
+            executable='ydlidar_ros2_driver_node',
+            name='ydlidar_ros2_driver_node',
+            output='screen',
+            emulate_tty=True,
+            parameters=[
+                params_file,
+                {
+                    'intensity': ParameterValue(intensity, value_type=bool),
+                    'sample_rate': ParameterValue(sample_rate, value_type=int),
+                    'baudrate': ParameterValue(baudrate, value_type=int),
+                },
+            ],
+            namespace='/',
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_tf_base_to_laser',
+            arguments=[
+                '--x', lidar_x, '--y', lidar_y, '--z', lidar_z,
+                '--yaw', lidar_yaw, '--pitch', '0.0', '--roll', '0.0',
+                '--frame-id', base_frame, '--child-frame-id', laser_frame,
+            ],
+        ),
+        # base_footprint -> base_link 恒等变换。
+        # slam_gmapping(本 build)不从参数读 base_frame，硬编码默认 base_link；
+        # 提供该帧让其默认值可解析。本工程 base_footprint 即底盘基准，两者重合。
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_tf_base_link',
+            arguments=[
+                '--x', '0.0', '--y', '0.0', '--z', '0.0',
+                '--yaw', '0.0', '--pitch', '0.0', '--roll', '0.0',
+                '--frame-id', base_frame, '--child-frame-id', 'base_link',
+            ],
+        ),
+    ])

ros2/src/craic_localization/launch/localization.launch.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+"""P5 精准定位：激光 + rf2o 激光里程计 + 已知地图 + AMCL。
+
+完整 TF 树：map ─(amcl)→ odom ─(rf2o)→ base_footprint ─(静态)→ laser_frame
+odom→base_footprint 由 rf2o 从 /scan 连续提供(不依赖底盘里程计板)，AMCL 用它做运动模型、
+激光匹配已知地图做校正，输出绝对、无漂移位姿。需要 P4 建好并存盘的地图。
+
+说明：底盘轮式里程计板上电后仅短时上报，暂不参与 odom→base；待其连续上报问题解决后，
+可用 robot_localization EKF 融合轮速 + rf2o 再喂 AMCL 提升鲁棒性。
+
+用法：
+  ros2 launch craic_localization localization.launch.py map:=/abs/path/maps/craic.yaml
+  # 起点不在地图原点时，rviz 用 "2D Pose Estimate" 给初值，或：
+  ros2 launch craic_localization localization.launch.py map:=... init_x:=0.3 init_y:=0.2 init_yaw:=1.57
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.descriptions import ParameterValue
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('craic_localization')
+    lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
+    amcl_params = os.path.join(pkg_share, 'config', 'amcl.yaml')
+    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')
+    default_map = os.path.join(pkg_share, 'maps', 'craic.yaml')
+
+    use_rviz = LaunchConfiguration('use_rviz')
+    map_yaml = LaunchConfiguration('map')
+    init_x = LaunchConfiguration('init_x')
+    init_y = LaunchConfiguration('init_y')
+    init_yaw = LaunchConfiguration('init_yaw')
+    show_points = LaunchConfiguration('show_points')
+    points_file = LaunchConfiguration('points_file')
+    lidar_x = LaunchConfiguration('lidar_x')
+    lidar_y = LaunchConfiguration('lidar_y')
+    lidar_z = LaunchConfiguration('lidar_z')
+    lidar_yaw = LaunchConfiguration('lidar_yaw')
+    intensity = LaunchConfiguration('intensity')
+    sample_rate = LaunchConfiguration('sample_rate')
+    baudrate = LaunchConfiguration('baudrate')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('map', default_value=default_map,
+                              description='地图 yaml 路径(P4 由 map_saver 存盘得到)'),
+        DeclareLaunchArgument('init_x', default_value='0.0'),
+        DeclareLaunchArgument('init_y', default_value='0.0'),
+        DeclareLaunchArgument('init_yaw', default_value='0.0'),
+        DeclareLaunchArgument('show_points', default_value='true'),
+        DeclareLaunchArgument('points_file',
+                              default_value=os.path.join(pkg_share, 'config', 'taught_points.yaml'),
+                              description='示教点位文件(rviz 显示)；想实时看示教就指向你的 taught_points.yaml'),
+        DeclareLaunchArgument('lidar_x', default_value='0.0',
+                              description='base_footprint -> laser_frame 平移 X (m)'),
+        DeclareLaunchArgument('lidar_y', default_value='0.0',
+                              description='base_footprint -> laser_frame 平移 Y (m)'),
+        DeclareLaunchArgument('lidar_z', default_value='0.02',
+                              description='base_footprint -> laser_frame 平移 Z (m)'),
+        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159',
+                              description='base_footprint -> laser_frame 偏航角 (rad)'),
+        DeclareLaunchArgument('intensity', default_value='true'),
+        DeclareLaunchArgument('sample_rate', default_value='4'),
+        DeclareLaunchArgument('baudrate', default_value='230400'),
+
+        # ===== 传感器 + 激光里程计(odom -> base_footprint) =====
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(lidar_launch),
+            launch_arguments={
+                'lidar_x': lidar_x,
+                'lidar_y': lidar_y,
+                'lidar_z': lidar_z,
+                'lidar_yaw': lidar_yaw,
+                'intensity': intensity,
+                'sample_rate': sample_rate,
+                'baudrate': baudrate,
+            }.items(),
+        ),
+        Node(
+            package='rf2o_laser_odometry',
+            executable='rf2o_laser_odometry_node',
+            name='rf2o_laser_odometry',
+            output='screen',
+            parameters=[{
+                'laser_scan_topic': '/scan',
+                'odom_topic': '/odom',
+                'publish_tf': True,
+                'base_frame_id': 'base_footprint',
+                'odom_frame_id': 'odom',
+                'init_pose_from_topic': '',
+                'freq': 10.0,
+            }],
+        ),
+
+        # ===== 地图服务 =====
+        Node(
+            package='nav2_map_server',
+            executable='map_server',
+            name='map_server',
+            output='screen',
+            parameters=[amcl_params, {'yaml_filename': map_yaml}],
+        ),
+
+        # ===== AMCL 定位 =====
+        Node(
+            package='nav2_amcl',
+            executable='amcl',
+            name='amcl',
+            output='screen',
+            parameters=[amcl_params, {
+                'set_initial_pose': True,
+                'initial_pose.x': ParameterValue(init_x, value_type=float),
+                'initial_pose.y': ParameterValue(init_y, value_type=float),
+                'initial_pose.yaw': ParameterValue(init_yaw, value_type=float),
+            }],
+        ),
+
+        # ===== 生命周期管理：自动 configure+activate map_server 与 amcl =====
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_localization',
+            output='screen',
+            parameters=[{
+                'use_sim_time': False,
+                'autostart': True,
+                'node_names': ['map_server', 'amcl'],
+            }],
+        ),
+
+        # ===== 示教点位可视化 =====
+        Node(
+            package='craic_localization',
+            executable='show_points',
+            name='show_points',
+            output='screen',
+            parameters=[{'points_file': points_file}],
+            condition=IfCondition(show_points),
+        ),
+
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', default_rviz],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

ros2/src/craic_localization/launch/mapping.launch.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+"""P4 建图：激光 + rf2o 激光里程计 + slam_gmapping(SLAM)。
+
+odom→base_footprint 由 rf2o(从连续 /scan 推算)提供，**不依赖底盘里程计板** ——
+该板上电后仅短时上报，会让 odom TF 冻结、gmapping 报 "queue is full" 丢弃所有 scan。
+本管线对齐 lzu_robot 可正常建图的做法（rf2o 提供 odom→base，gmapping 出图）。
+
+用法：
+  ros2 launch craic_localization mapping.launch.py
+  # 另开终端遥控走图；满意后存盘：
+  ros2 run nav2_map_server map_saver_cli -f <pkg>/maps/craic --ros-args -p save_map_timeout:=10000.0
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('craic_localization')
+    lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
+    gmapping_params = os.path.join(pkg_share, 'config', 'gmapping.yaml')
+    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')
+
+    use_rviz = LaunchConfiguration('use_rviz')
+    lidar_x = LaunchConfiguration('lidar_x')
+    lidar_y = LaunchConfiguration('lidar_y')
+    lidar_z = LaunchConfiguration('lidar_z')
+    lidar_yaw = LaunchConfiguration('lidar_yaw')
+    intensity = LaunchConfiguration('intensity')
+    sample_rate = LaunchConfiguration('sample_rate')
+    baudrate = LaunchConfiguration('baudrate')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('lidar_x', default_value='0.0',
+                              description='base_footprint -> laser_frame 平移 X (m)'),
+        DeclareLaunchArgument('lidar_y', default_value='0.0',
+                              description='base_footprint -> laser_frame 平移 Y (m)'),
+        DeclareLaunchArgument('lidar_z', default_value='0.02',
+                              description='base_footprint -> laser_frame 平移 Z (m)'),
+        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159',
+                              description='base_footprint -> laser_frame 偏航角 (rad)'),
+        DeclareLaunchArgument('intensity', default_value='true'),
+        DeclareLaunchArgument('sample_rate', default_value='4'),
+        DeclareLaunchArgument('baudrate', default_value='230400'),
+
+        # 激光 + 静态外参(base_footprint->laser_frame, base_footprint->base_link)
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(lidar_launch),
+            launch_arguments={
+                'lidar_x': lidar_x,
+                'lidar_y': lidar_y,
+                'lidar_z': lidar_z,
+                'lidar_yaw': lidar_yaw,
+                'intensity': intensity,
+                'sample_rate': sample_rate,
+                'baudrate': baudrate,
+            }.items(),
+        ),
+
+        # 激光里程计：从 /scan 连续输出 odom -> base_footprint（建图运动来源）
+        Node(
+            package='rf2o_laser_odometry',
+            executable='rf2o_laser_odometry_node',
+            name='rf2o_laser_odometry',
+            output='screen',
+            parameters=[{
+                'laser_scan_topic': '/scan',
+                'odom_topic': '/odom',
+                'publish_tf': True,
+                'base_frame_id': 'base_footprint',
+                'odom_frame_id': 'odom',
+                'init_pose_from_topic': '',
+                'freq': 10.0,
+            }],
+        ),
+
+        # SLAM：发布 /map 与 TF map -> odom
+        Node(
+            package='slam_gmapping',
+            executable='slam_gmapping',
+            name='slam_gmapping',
+            output='screen',
+            parameters=[gmapping_params],
+        ),
+
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', default_rviz],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

ros2/src/craic_localization/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>craic_localization</name>
+  <version>0.0.0</version>
+  <description>Wheel odometry + LiDAR localization for the CRAIC mobile base</description>
+  <maintainer email="fallensigh@gmail.com">fallensigh</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
+  <exec_depend>python3-yaml</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2/src/craic_localization/rviz/localization.rviz

@@ -0,0 +1,130 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1
+        - /LaserScan1
+        - /LaserScan1/Topic1
+        - /Odometry1
+      Splitter Ratio: 0.5
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Enabled: true
+      Reference Frame: <Fixed Frame>
+      Plane Cell Count: 20
+      Cell Size: 0.5
+      Color: 160; 160; 164
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Enabled: true
+      Show Names: true
+      Show Axes: true
+      Show Arrows: true
+      Marker Scale: 0.5
+      Update Interval: 0
+      Frame Timeout: 15
+      Value: true
+    - Class: rviz_default_plugins/Map
+      Name: Map
+      Enabled: true
+      Topic:
+        Value: /map
+        Depth: 1
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+      Color Scheme: map
+      Draw Behind: true
+      Alpha: 0.7
+      Value: true
+    - Class: rviz_default_plugins/LaserScan
+      Name: LaserScan
+      Enabled: true
+      Topic:
+        Value: /scan
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Filter size: 10
+      Style: Points
+      Size (m): 0.02
+      Size (Pixels): 3
+      Color Transformer: FlatColor
+      Color: 255; 30; 30
+      Decay Time: 0
+      Position Transformer: XYZ
+      Value: true
+    - Class: rviz_default_plugins/Odometry
+      Name: Odometry
+      Enabled: true
+      Topic:
+        Value: /odom
+        Depth: 10
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+      Keep: 1
+      Position Tolerance: 0.05
+      Angle Tolerance: 0.05
+      Shape:
+        Value: Arrow
+        Color: 30; 255; 30
+        Shaft Length: 0.3
+        Shaft Radius: 0.03
+        Head Length: 0.1
+        Head Radius: 0.06
+        Alpha: 1
+      Covariance:
+        Value: false
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Name: TaughtPoints
+      Enabled: true
+      Topic:
+        Value: /taught_points
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+      Namespaces: {}
+      Value: true
+  Global Options:
+    Fixed Frame: map
+    Background Color: 48; 48; 48
+    Frame Rate: 30
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Name: Current View
+      Distance: 8
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Pitch: 1.2
+      Yaw: 3.14
+      Target Frame: <Fixed Frame>
+    Saved: ~
+Window Geometry:
+  Height: 800
+  Width: 1200
+  Displays:
+    collapsed: false
+  Views:
+    collapsed: false

ros2/src/craic_localization/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/craic_localization
+[install]
+install_scripts=$base/lib/craic_localization

ros2/src/craic_localization/setup.py

@@ -0,0 +1,46 @@
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = 'craic_localization'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'config'),
+            glob('config/*.yaml')),
+        (os.path.join('share', package_name, 'launch'),
+            glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'rviz'),
+            glob('rviz/*.rviz')),
+        (os.path.join('share', package_name, 'urdf'),
+            glob('urdf/*')),
+        (os.path.join('share', package_name, 'maps'),
+            glob('maps/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='fallensigh',
+    maintainer_email='fallensigh@gmail.com',
+    description='Wheel odometry + LiDAR localization for the CRAIC mobile base',
+    license='MIT',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'chassis_odometry = craic_localization.chassis_odometry:main',
+            'teach_points = craic_localization.teach_points:main',
+            'navigate_to_point = craic_localization.navigate_to_point:main',
+            'show_points = craic_localization.show_points:main',
+        ],
+    },
+)

ros2/src/rf2o_laser_odometry/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.5)
+project(rf2o_laser_odometry)
+
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2  REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+
+set(EXECUTABLE_NAME "rf2o_laser_odometry_node")
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+add_executable(${EXECUTABLE_NAME} 
+    src/CLaserOdometry2DNode.cpp
+    src/CLaserOdometry2D.cpp)
+
+include_directories(
+  include include/
+)
+
+include_directories(  
+  ${Boost_INCLUDE_DIRS}
+  ##${EIGEN3_INCLUDE_DIRS}
+)
+
+set(DEPENDENCIES
+  "rclcpp"           
+  "std_msgs"         
+  "geometry_msgs" 
+  "tf2_geometry_msgs"
+  "tf2"
+  "tf2_ros"
+  "sensor_msgs"
+  "nav_msgs"
+  "eigen3_cmake_module"
+)
+
+##target_include_directories(${PROJECT_NAME} PUBLIC ${Eigen3_INCLUDE_DIRS})
+ament_target_dependencies(${EXECUTABLE_NAME} ${DEPENDENCIES})
+
+## Declare a cpp library
+add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp)
+
+## Declare a cpp executable
+ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES})
+
+install(TARGETS ${EXECUTABLE_NAME}
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY launch  
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()
+

ros2/src/rf2o_laser_odometry/LICENSE

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    {one line to give the program's name and a brief idea of what it does.}
+    Copyright (C) {year}  {name of author}
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    {project}  Copyright (C) {year}  {fullname}
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

ros2/src/rf2o_laser_odometry/README.md

@@ -0,0 +1,9 @@
+# rf2o_laser_odometry
+
+Estimation of 2D odometry based on planar laser scans. rf2o is a fast and precise method to estimate the planar motion of a lidar from consecutive range scans. Useful for mobile robots with inaccurate wheel odometry.
+
+For every scanned point we formulate the range flow constraint equation in terms of the sensor velocity, and minimize a robust function of the resulting geometric constraints to obtain the motion estimate. Conversely to traditional approaches, this method does not search for correspondences but performs dense scan alignment based on the scan gradients, in the fashion of dense 3D visual odometry.
+
+The very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precision, makes RF2O a suitable method for those robotic applications that require planar odometry.
+
+For a full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.uma.es/papersrepo/2016/2016_Jaimez_ICRA_RF2O.pdf

ros2/src/rf2o_laser_odometry/include/rf2o_laser_odometry/CLaserOdometry2D.hpp

@@ -0,0 +1,109 @@
+/** ****************************************************************************************
+ *  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
+ *  Refactored: Pure C++ algorithm class, NO rclcpp dependencies.
+ ******************************************************************************************** */
+#ifndef CLaserOdometry2D_H
+#define CLaserOdometry2D_H
+
+// ✅ [CLEANED] Removed all rclcpp headers. Only msg types and math libs remain.
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <vector>
+#include <cmath>
+
+namespace rf2o {
+
+template <typename T> inline T sign(const T x) { return x < T(0) ? -1 : 1; }
+
+template <typename Derived>
+inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& r) {
+    return std::atan2(r(1, 0), r(0, 0));
+}
+
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll, const T pitch, const T yaw) {
+    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
+    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
+    return (az * ay * ax).toRotationMatrix().matrix();
+}
+
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw) {
+    return matrixRollPitchYaw<T>(0, 0, yaw);
+}
+
+using Scalar = float;
+using Pose3d = Eigen::Isometry3d;
+using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
+using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;
+
+// ✅ [FIXED] No longer inherits rclcpp::Node. Pure C++ class.
+class CLaserOdometry2D 
+{
+public:
+    CLaserOdometry2D();
+
+    void init(const sensor_msgs::msg::LaserScan& scan,
+              const geometry_msgs::msg::Pose& initial_robot_pose);
+    
+    bool is_initialized();
+    
+    // ✅ [MODIFIED] Accepts external timestamp instead of using internal clock
+    bool odometryCalculation(const sensor_msgs::msg::LaserScan& scan, double current_time_sec);
+    
+    void setLaserPose(const Pose3d& laser_pose);
+    
+    const Pose3d& getIncrement() const;
+    const IncrementCov& getIncrementCovariance() const;
+    Pose3d& getPose();
+    const Pose3d& getPose() const;
+
+    bool verbose, module_initialized, first_laser_scan;
+    
+    // ✅ [MODIFIED] Replaced rclcpp::Time with plain double (seconds)
+    double last_odom_time_sec; 
+    double current_scan_time_sec;
+
+    // Internal Data Structures (Unchanged)
+    std::vector<Eigen::MatrixXf> range, range_old, range_inter, range_warped;
+    std::vector<Eigen::MatrixXf> xx, xx_inter, xx_old, xx_warped;
+    std::vector<Eigen::MatrixXf> yy, yy_inter, yy_old, yy_warped;
+    std::vector<Eigen::MatrixXf> transformations;
+    Eigen::MatrixXf range_wf, dtita, dt, rtita, normx, normy, norm_ang, weights;
+    Eigen::MatrixXi null;
+    Eigen::MatrixXf A, Aw, B, Bw;
+    MatrixS31 Var;
+    IncrementCov cov_odo;
+    float fps, fovh;
+    unsigned int cols, cols_i, width, ctf_levels, image_level, level, num_valid_range, iter_irls;
+    float g_mask[5];
+    double lin_speed, ang_speed;
+    MatrixS31 kai_abs_, kai_loc_, kai_loc_old_, kai_loc_level_;
+    Pose3d last_increment_, laser_pose_on_robot_, laser_pose_on_robot_inv_;
+    Pose3d laser_pose_, laser_oldpose_, robot_pose_, robot_oldpose_;
+    bool test;
+    std::vector<double> last_m_lin_speeds, last_m_ang_speeds;
+
+private:
+    void createImagePyramid();
+    void calculateCoord();
+    void performWarping();
+    void calculaterangeDerivativesSurface();
+    void computeNormals();
+    void computeWeights();
+    void findNullPoints();
+    void solveSystemOneLevel();
+    void solveSystemNonLinear();
+    bool filterLevelSolution();
+    void PoseUpdate();
+    void Reset(const Pose3d& ini_pose);
+};
+
+} /* namespace rf2o */
+#endif

ros2/src/rf2o_laser_odometry/include/rf2o_laser_odometry/CLaserOdometry2DNode.hpp

@@ -0,0 +1,50 @@
+#include "rf2o_laser_odometry/CLaserOdometry2D.hpp"
+
+#include <tf2/convert.h>
+#include <tf2/exceptions.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2/impl/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/utils.h>
+
+namespace rf2o {
+
+class CLaserOdometry2DNode : public rclcpp::Node
+{
+public:
+  CLaserOdometry2DNode();
+  void process();
+  void publish();
+  bool setLaserPoseFromTf();
+  bool scan_available();
+
+  // Params & vars
+  CLaserOdometry2D    rf2o_ref;
+  bool                publish_tf, new_scan_available;
+  double              freq;
+  std::string         laser_scan_topic;
+  std::string         odom_topic;
+  std::string         base_frame_id;
+  std::string         odom_frame_id;
+  std::string         init_pose_from_topic;
+
+  sensor_msgs::msg::LaserScan                     last_scan;
+  bool                                            GT_pose_initialized;
+  std::shared_ptr<tf2_ros::Buffer>                buffer_;
+  std::shared_ptr<tf2_ros::TransformListener>     tf_listener_;  
+  std::unique_ptr<tf2_ros::TransformBroadcaster>  odom_broadcaster;
+  nav_msgs::msg::Odometry                         initial_robot_pose;
+
+  // Subscriptions & Publishers
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr  laser_sub;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr      initPose_sub;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr         odom_pub;
+
+  // CallBacks
+  void LaserCallBack(const sensor_msgs::msg::LaserScan::SharedPtr new_scan);
+  void initPoseCallBack(const nav_msgs::msg::Odometry::SharedPtr new_initPose);
+};
+}
+

ros2/src/rf2o_laser_odometry/launch/rf2o_laser_odometry.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    return LaunchDescription([
+
+            Node(
+                package='rf2o_laser_odometry',
+                executable='rf2o_laser_odometry_node',
+                name='rf2o_laser_odometry',   
+                parameters=[{
+                    'laser_scan_topic' : '/scan',
+                    'odom_topic' : '/odom',
+                    'publish_tf' : True,
+                    'base_frame_id' : 'base_footprint',#base_footprint
+                    'odom_frame_id' : 'odom',
+                    'init_pose_from_topic' : '',
+                    'freq' : 10.0}],
+            ),
+    ])

ros2/src/rf2o_laser_odometry/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package>
+  <name>rf2o_laser_odometry</name>
+  <version>0.1.0</version>
+  <description>
+    Estimation of 2D odometry based on planar laser scans. Useful for mobile robots with innacurate base odometry.
+    For full description of the algorithm, please refer to:
+    Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016
+    Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
+  </description>
+
+  <maintainer email="jgmonroy@uma.es">Javier G. Monroy</maintainer>
+  <author email="marianojt@uma.es"> Mariano Jaimez</author>
+  <author email="jgmonroy@uma.es"> Javier G. Monroy</author>
+  <license>GPL v3</license> 
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>  
+  <build_depend>eigen</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>rclcpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>  
+  <run_depend>eigen</run_depend>
+  <run_depend>cmake_modules</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2/src/rf2o_laser_odometry/src/CLaserOdometry2D.cpp

@@ -0,0 +1,840 @@
+/** ****************************************************************************************
+*  This node presents a fast and precise method to estimate the planar motion of a lidar
+*  from consecutive range scans. It is very useful for the estimation of the robot odometry from
+*  2D laser range measurements.
+*  This module is developed for mobile robots with innacurate or inexistent built-in odometry.
+*  It allows the estimation of a precise odometry with low computational cost.
+*  For more information, please refer to:
+*
+*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
+*  Available at: http://mapir.uma.es/papersrepo/2016/2016_Jaimez_ICRA_RF2O.pdf
+*
+* Maintainer: Javier G. Monroy
+* MAPIR group: https://mapir.isa.uma.es
+*
+* Modifications: Jeremie Deray & (see contributons on github)
+******************************************************************************************** */
+
+#include "rf2o_laser_odometry/CLaserOdometry2D.hpp"
+
+namespace rf2o {
+
+
+// ✅ [FIXED] Constructor no longer calls Node("..."). Pure member initialization only.
+CLaserOdometry2D::CLaserOdometry2D() :
+  verbose(false),
+  module_initialized(false),
+  first_laser_scan(true),
+  last_increment_(Pose3d::Identity()),
+  laser_pose_on_robot_(Pose3d::Identity()),
+  laser_pose_on_robot_inv_(Pose3d::Identity()),
+  laser_pose_(Pose3d::Identity()),
+  laser_oldpose_(Pose3d::Identity()),
+  robot_pose_(Pose3d::Identity()),
+  robot_oldpose_(Pose3d::Identity())
+{
+}
+
+void CLaserOdometry2D::setLaserPose(const Pose3d& laser_pose) {
+    laser_pose_on_robot_     = laser_pose;
+    laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse();
+}
+
+bool CLaserOdometry2D::is_initialized() {
+    return module_initialized;
+}
+
+void CLaserOdometry2D::init(const sensor_msgs::msg::LaserScan& scan,
+                            const geometry_msgs::msg::Pose& initial_robot_pose) {
+    // ✅ [CLEANED] All RCLCPP_INFO logs removed completely.
+    
+    width = scan.ranges.size();         
+    cols = width;						            
+    fovh = std::abs(scan.angle_max - scan.angle_min);  
+    ctf_levels = 5;                     
+    iter_irls  = 5;                     
+
+    Pose3d robot_initial_pose = Pose3d::Identity();
+    robot_initial_pose = Eigen::Quaterniond(initial_robot_pose.orientation.w,
+                                            initial_robot_pose.orientation.x,
+                                            initial_robot_pose.orientation.y,
+                                            initial_robot_pose.orientation.z);
+    robot_initial_pose.translation()(0) = initial_robot_pose.position.x;
+    robot_initial_pose.translation()(1) = initial_robot_pose.position.y;
+
+    laser_pose_    = robot_initial_pose * laser_pose_on_robot_;
+    laser_oldpose_ = laser_oldpose_;
+
+    range_wf = Eigen::MatrixXf::Constant(1, width, 1);
+
+    transformations.resize(ctf_levels);
+    for (unsigned int i = 0; i < ctf_levels; i++)
+        transformations[i].resize(3, 3);
+
+    unsigned int s, cols_i;
+    const unsigned int pyr_levels = std::round(std::log2(round(float(width) / float(cols)))) + ctf_levels;
+    range.resize(pyr_levels); range_old.resize(pyr_levels); range_inter.resize(pyr_levels);
+    xx.resize(pyr_levels); xx_inter.resize(pyr_levels); xx_old.resize(pyr_levels); xx_warped.resize(pyr_levels);
+    yy.resize(pyr_levels); yy_inter.resize(pyr_levels); yy_old.resize(pyr_levels); yy_warped.resize(pyr_levels);
+    range_warped.resize(pyr_levels);
+
+    for (unsigned int i = 0; i < pyr_levels; i++) {
+        s = std::pow(2.f, int(i));
+        cols_i = std::ceil(float(width) / float(s));
+        range[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        range_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        range_inter[i].resize(1, cols_i);
+        xx[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        xx_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        yy[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        yy_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+        xx_inter[i].resize(1, cols_i); yy_inter[i].resize(1, cols_i);
+        if (cols_i <= cols) {
+            range_warped[i].resize(1, cols_i);
+            xx_warped[i].resize(1, cols_i);
+            yy_warped[i].resize(1, cols_i);
+        }
+    }
+
+    dt.resize(1, cols); dtita.resize(1, cols);
+    normx.resize(1, cols); normy.resize(1, cols); norm_ang.resize(1, cols); weights.resize(1, cols);
+    null = Eigen::MatrixXi::Constant(1, cols, 0);
+    cov_odo = IncrementCov::Zero();
+    fps = 1.f;		
+    num_valid_range = 0;
+
+    g_mask[0] = 1.f / 16.f; g_mask[1] = 0.25f; g_mask[2] = 6.f / 16.f;
+    g_mask[3] = g_mask[1]; g_mask[4] = g_mask[0];
+
+    kai_abs_     = MatrixS31::Zero();
+    kai_loc_old_ = MatrixS31::Zero();
+
+    module_initialized = true;
+    // ✅ [MODIFIED] Convert ROS timestamp to plain double seconds
+    last_odom_time_sec = scan.header.stamp.sec + scan.header.stamp.nanosec / 1e9;
+}
+
+const Pose3d& CLaserOdometry2D::getIncrement() const { return last_increment_; }
+const IncrementCov& CLaserOdometry2D::getIncrementCovariance() const { return cov_odo; }
+Pose3d& CLaserOdometry2D::getPose() { return robot_pose_; }
+const Pose3d& CLaserOdometry2D::getPose() const { return robot_pose_; }
+
+// ✅ [MODIFIED] Signature changed to accept external time
+bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::msg::LaserScan& scan, double current_time_sec) {
+    range_wf = Eigen::Map<const Eigen::MatrixXf>(scan.ranges.data(), width, 1);
+
+    double start_sec = current_time_sec; // ✅ [MODIFIED] No more get_clock()->now()
+
+    createImagePyramid();
+
+    for (unsigned int i=0; i<ctf_levels; i++) {
+        transformations[i].setIdentity();
+        level = i;
+        unsigned int s = std::pow(2.f,int(ctf_levels-(i+1)));
+        cols_i = std::ceil(float(cols)/float(s));
+        image_level = ctf_levels - i + std::round(std::log2(std::round(float(width)/float(cols)))) - 1;
+
+        if (i == 0) {
+            range_warped[image_level] = range[image_level];
+            xx_warped[image_level]    = xx[image_level];
+            yy_warped[image_level]    = yy[image_level];
+        } else {
+            performWarping();
+        }
+
+        calculateCoord();
+        findNullPoints();
+        calculaterangeDerivativesSurface();
+        computeWeights();
+
+        if (num_valid_range > 3)
+            solveSystemNonLinear();
+        else
+            continue;
+
+        if (!filterLevelSolution()) return false;
+    } 
+
+    // ✅ [CLEANED] Execution time log removed completely.
+    // If you need it later, use: printf("[RF2O] exec ms: %f\n", (current_time_sec - start_sec)*1000);
+
+    PoseUpdate();
+    return true;
+}
+
+
+/**
+ * Creates the Pyramid (multi-resolution) from the current laser range data
+*/
+void CLaserOdometry2D::createImagePyramid()
+{
+  // Push the scans back
+  range_old.swap(range);
+  xx_old.swap(xx);
+  yy_old.swap(yy);
+
+  // The number of levels of the pyramid does not always match the number of levels used
+  // in the odometry computation (because we sometimes want to finish with lower resolutions)
+  unsigned int pyr_levels = std::round(std::log2(std::round(float(width)/float(cols)))) + ctf_levels;
+
+  // Generate Pyramid levels
+  const float max_range_dif = 0.3f;
+  for (unsigned int i = 0; i<pyr_levels; i++)
+  {
+    unsigned int s = std::pow(2.f,int(i));
+    cols_i = std::ceil(float(width)/float(s));
+
+    const unsigned int i_1 = i-1;
+
+    // First level -> Filter (not downsampling);
+    if (i == 0)
+    {
+      for (unsigned int u = 0; u < cols_i; u++)
+      {
+        const float dcenter = range_wf(u);
+
+        // Inner pixels (avoid first and last points in the scan)
+        if ((u>1)&&(u<cols_i-2))
+        {
+          if (std::isfinite(dcenter) && dcenter > 0.f)
+          {
+            float sum = 0.f;
+            float weight = 0.f;
+
+            for (int l=-2; l<3; l++)
+            {
+              const float abs_dif = std::abs(range_wf(u+l)-dcenter);
+              if (abs_dif < max_range_dif)
+              {
+                const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
+                weight += aux_w;
+                sum += aux_w*range_wf(u+l);
+              }
+            }
+            range[i](u) = sum/weight;
+          }
+          else
+            range[i](u) = 0.f;
+        }
+
+        // Boundary points 
+        else
+        {
+          if (std::isfinite(dcenter) && dcenter > 0.f)
+          {
+            float sum = 0.f;
+            float weight = 0.f;
+
+            for (int l=-2; l<3; l++)
+            {
+              const int indu = u+l;
+              if ((indu>=0)&&(indu<int(cols_i)))
+              {
+                const float abs_dif = std::abs(range_wf(indu)-dcenter);
+                if (abs_dif < max_range_dif)
+                {
+                  const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
+                  weight += aux_w;
+                  sum += aux_w*range_wf(indu);
+                }
+              }
+            }
+            range[i](u) = sum/weight;
+          }
+          else
+            range[i](u) = 0.f;
+        }
+      }
+    }
+
+    // Second level and forth ->  Downsampling    
+    else
+    {
+      for (unsigned int u = 0; u < cols_i; u++)
+      {
+        const int u2 = 2*u;
+        const float dcenter = range[i_1](u2);
+
+        // Inner pixels (avoid first/last point in the scan)
+        if ((u>0)&&(u<cols_i-1))
+        {
+          if (dcenter > 0.f)
+          {
+            float sum = 0.f;
+            float weight = 0.f;
+
+            for (int l=-2; l<3; l++)
+            {
+              const float abs_dif = std::abs(range[i_1](u2+l)-dcenter);
+              if (abs_dif < max_range_dif)
+              {
+                const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
+                weight += aux_w;
+                sum += aux_w*range[i_1](u2+l);
+              }
+            }
+            range[i](u) = sum/weight;
+          }
+          else
+            range[i](u) = 0.f;
+
+        }
+
+        //Boundary
+        else
+        {
+          if (dcenter > 0.f)
+          {
+            float sum = 0.f;
+            float weight = 0.f;
+            const unsigned int cols_i2 = range[i_1].cols();
+
+
+            for (int l=-2; l<3; l++)
+            {
+              const int indu = u2+l;
+              if ((indu>=0)&&(indu<int(cols_i2)))
+              {
+                const float abs_dif = std::abs(range[i_1](indu)-dcenter);
+                if (abs_dif < max_range_dif)
+                {
+                  const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
+                  weight += aux_w;
+                  sum += aux_w*range[i_1](indu);
+                }
+              }
+            }
+            range[i](u) = sum/weight;
+          }
+          else
+            range[i](u) = 0.f;
+
+        }
+      }
+    }
+
+    // Calculate coordinates "xy" of the points
+    for (unsigned int u = 0; u < cols_i; u++)
+    {
+      if (range[i](u) > 0.f)
+      {
+        const float tita = -0.5*fovh + float(u)*fovh/float(cols_i-1);
+        xx[i](u) = range[i](u)*std::cos(tita);
+        yy[i](u) = range[i](u)*std::sin(tita);
+      }
+      else
+      {
+        xx[i](u) = 0.f;
+        yy[i](u) = 0.f;
+      }
+    }
+  }
+}
+
+
+void CLaserOdometry2D::calculateCoord()
+{
+  for (unsigned int u = 0; u < cols_i; u++)
+  {
+    if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f))
+    {
+      range_inter[image_level](u) = 0.f;
+      xx_inter[image_level](u)    = 0.f;
+      yy_inter[image_level](u)    = 0.f;
+    }
+    else
+    {
+      range_inter[image_level](u) = 0.5f*(range_old[image_level](u) + range_warped[image_level](u));
+      xx_inter[image_level](u)    = 0.5f*(xx_old[image_level](u)    + xx_warped[image_level](u));
+      yy_inter[image_level](u)    = 0.5f*(yy_old[image_level](u)    + yy_warped[image_level](u));
+    }
+  }
+}
+
+
+void CLaserOdometry2D::calculaterangeDerivativesSurface()
+{
+  //The gradient size ir reserved at the maximum size (at the constructor)
+
+  //Compute connectivity
+
+  //Defined in a different way now, without inversion
+  rtita = Eigen::MatrixXf::Constant(1, cols_i, 1.f);
+
+  for (unsigned int u = 0; u < cols_i-1; u++)
+  {
+    float dista = xx_inter[image_level](u+1) - xx_inter[image_level](u);
+    dista *= dista;
+    float distb = yy_inter[image_level](u+1) - yy_inter[image_level](u);
+    distb *= distb;
+    const float dist = dista + distb;
+
+    if (dist  > 0.f)
+      rtita(u) = std::sqrt(dist);
+  }
+
+  //Spatial derivatives
+  for (unsigned int u = 1; u < cols_i-1; u++)
+    dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)-
+                range_inter[image_level](u)) + rtita(u)*(range_inter[image_level](u) -
+        range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1));
+
+  dtita(0) = dtita(1);
+  dtita(cols_i-1) = dtita(cols_i-2);
+
+  //Temporal derivative
+  for (unsigned int u = 0; u < cols_i; u++)
+    dt(u) = fps*(range_warped[image_level](u) - range_old[image_level](u));
+
+
+  //Apply median filter to the range derivatives
+  //MatrixXf dtitamed = dtita, dtmed = dt;
+  //vector<float> svector(3);
+  //for (unsigned int u=1; u<cols_i-1; u++)
+  //{
+  //	svector.at(0) = dtita(u-1); svector.at(1) = dtita(u); svector.at(2) = dtita(u+1);
+  //	std::sort(svector.begin(), svector.end());
+  //	dtitamed(u) = svector.at(1);
+
+  //	svector.at(0) = dt(u-1); svector.at(1) = dt(u); svector.at(2) = dt(u+1);
+  //	std::sort(svector.begin(), svector.end());
+  //	dtmed(u) = svector.at(1);
+  //}
+
+  //dtitamed(0) = dtitamed(1);
+  //dtitamed(cols_i-1) = dtitamed(cols_i-2);
+  //dtmed(0) = dtmed(1);
+  //dtmed(cols_i-1) = dtmed(cols_i-2);
+
+  //dtitamed.swap(dtita);
+  //dtmed.swap(dt);
+}
+
+void CLaserOdometry2D::computeNormals()
+{
+  normx.setConstant(1, cols, 0.f);
+  normy.setConstant(1, cols, 0.f);
+  norm_ang.setConstant(1, cols, 0.f);
+
+  const float incr_tita = fovh/float(cols_i-1);
+  for (unsigned int u=0; u<cols_i; u++)
+  {
+    if (null(u) == 0.f)
+    {
+      const float tita = -0.5f*fovh + float(u)*incr_tita;
+      const float alfa = -std::atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita);
+      norm_ang(u) = tita + alfa;
+      if (norm_ang(u) < -M_PI)
+        norm_ang(u) += 2.f*M_PI;
+      else if (norm_ang(u) < 0.f)
+        norm_ang(u) += M_PI;
+      else if (norm_ang(u) > M_PI)
+        norm_ang(u) -= M_PI;
+
+      normx(u) = std::cos(tita + alfa);
+      normy(u) = std::sin(tita + alfa);
+    }
+  }
+}
+
+void CLaserOdometry2D::computeWeights()
+{
+  //The maximum weight size is reserved at the constructor
+  weights.setConstant(1, cols, 0.f);
+
+  //Parameters for error_linearization
+  const float kdtita = 1.f;
+  const float kdt = kdtita / (fps*fps);
+  const float k2d = 0.2f;
+
+  for (unsigned int u = 1; u < cols_i-1; u++)
+    if (null(u) == 0)
+    {
+      //							Compute derivatives
+      //-----------------------------------------------------------------------
+      const float ini_dtita = range_old[image_level](u+1) - range_old[image_level](u-1);
+      const float final_dtita = range_warped[image_level](u+1) - range_warped[image_level](u-1);
+
+      const float dtitat = ini_dtita - final_dtita;
+      const float dtita2 = dtita(u+1) - dtita(u-1);
+
+      const float w_der = kdt*(dt(u)*dt(u)) +
+          kdtita*(dtita(u)*dtita(u)) +
+          k2d*(std::abs(dtitat) + std::abs(dtita2));
+
+      weights(u) = std::sqrt(1.f/w_der);
+    }
+
+  const float inv_max = 1.f / weights.maxCoeff();
+  weights = inv_max*weights;
+}
+
+void CLaserOdometry2D::findNullPoints()
+{
+  //Size of null matrix is set to its maximum size (constructor)
+  num_valid_range = 0;
+
+  for (unsigned int u = 1; u < cols_i-1; u++)
+  {
+    if (range_inter[image_level](u) == 0.f)
+      null(u) = 1;
+    else
+    {
+      num_valid_range++;
+      null(u) = 0;
+    }
+  }
+}
+
+// Solves the system without considering any robust-function
+void CLaserOdometry2D::solveSystemOneLevel()
+{
+  A.resize(num_valid_range, 3);
+  B.resize(num_valid_range, 1);
+
+  unsigned int cont = 0;
+  const float kdtita = (cols_i-1)/fovh;
+
+  //Fill the matrix A and the vector B
+  //The order of the variables will be (vx, vy, wz)
+
+  for (unsigned int u = 1; u < cols_i-1; u++)
+    if (null(u) == 0)
+    {
+      // Precomputed expressions
+      const float tw = weights(u);
+      const float tita = -0.5*fovh + u/kdtita;
+
+      //Fill the matrix A
+      A(cont, 0) = tw*(std::cos(tita) + dtita(u)*kdtita*std::sin(tita)/range_inter[image_level](u));
+      A(cont, 1) = tw*(std::sin(tita) - dtita(u)*kdtita*std::cos(tita)/range_inter[image_level](u));
+      A(cont, 2) = tw*(-yy[image_level](u)*std::cos(tita) + xx[image_level](u)*std::sin(tita) - dtita(u)*kdtita);
+      B(cont, 0) = tw*(-dt(u));
+
+      cont++;
+    }
+
+  //Solve the linear system of equations using a minimum least squares method
+  Eigen::MatrixXf AtA, AtB;
+  AtA = A.transpose()*A;
+  AtB = A.transpose()*B;
+  Var = AtA.ldlt().solve(AtB);
+
+  //Covariance matrix calculation 	Cov Order -> vx,vy,wz
+  Eigen::MatrixXf res(num_valid_range,1);
+  res = A*Var - B;
+  cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
+
+  kai_loc_level_ = Var;
+}
+
+// Solves the system by considering the Cauchy M-estimator robust-function
+void CLaserOdometry2D::solveSystemNonLinear()
+{
+  A.resize(num_valid_range, 3); Aw.resize(num_valid_range, 3);
+  B.resize(num_valid_range, 1); Bw.resize(num_valid_range, 1);
+  unsigned int cont = 0;
+  const float kdtita = float(cols_i-1)/fovh;
+
+  //Fill the matrix A and the vector B
+  //The order of the variables will be (vx, vy, wz)
+
+  for (unsigned int u = 1; u < cols_i-1; u++)
+    if (null(u) == 0)
+    {
+      // Precomputed expressions
+      const float tw = weights(u);
+      const float tita = -0.5*fovh + u/kdtita;
+
+      //Fill the matrix A
+      A(cont, 0) = tw*(std::cos(tita) + dtita(u)*kdtita*std::sin(tita)/range_inter[image_level](u));
+      A(cont, 1) = tw*(std::sin(tita) - dtita(u)*kdtita*std::cos(tita)/range_inter[image_level](u));
+      A(cont, 2) = tw*(-yy[image_level](u)*std::cos(tita) + xx[image_level](u)*std::sin(tita) - dtita(u)*kdtita);
+      B(cont, 0) = tw*(-dt(u));
+
+      cont++;
+    }
+
+  //Solve the linear system of equations using a minimum least squares method
+  Eigen::MatrixXf AtA, AtB;
+  AtA = A.transpose()*A;
+  AtB = A.transpose()*B;
+  Var = AtA.ldlt().solve(AtB);
+
+  //Covariance matrix calculation 	Cov Order -> vx,vy,wz
+  Eigen::MatrixXf res(num_valid_range,1);
+  res = A*Var - B;
+  //cout << endl << "max res: " << res.maxCoeff();
+  //cout << endl << "min res: " << res.minCoeff();
+
+  ////Compute the energy
+  //Compute the average dt
+  float aver_dt = 0.f, aver_res = 0.f; unsigned int ind = 0;
+  for (unsigned int u = 1; u < cols_i-1; u++)
+    if (null(u) == 0)
+    {
+      aver_dt  += std::abs(dt(u));
+      aver_res += std::abs(res(ind++));
+    }
+  aver_dt /= cont; aver_res /= cont;
+  //    printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res);
+
+
+  const float k = 10.f/aver_dt; //200
+  //float energy = 0.f;
+  //for (unsigned int i=0; i<res.rows(); i++)
+  //	energy += log(1.f + mrpt::math::square(k*res(i)));
+  //printf("\n\nEnergy(0) = %f", energy);
+
+  //Solve iterative reweighted least squares
+  //===================================================================
+  for (unsigned int i=1; i<=iter_irls; i++)
+  {
+    cont = 0;
+
+    for (unsigned int u = 1; u < cols_i-1; u++)
+      if (null(u) == 0)
+      {
+        const float res_weight = std::sqrt(1.f/(1.f + ((k*res(cont))*(k*res(cont)))));
+
+        //Fill the matrix Aw
+        Aw(cont,0) = res_weight*A(cont,0);
+        Aw(cont,1) = res_weight*A(cont,1);
+        Aw(cont,2) = res_weight*A(cont,2);
+        Bw(cont)   = res_weight*B(cont);
+        cont++;
+      }
+
+    //Solve the linear system of equations using a minimum least squares method
+    AtA = Aw.transpose()*Aw;
+    AtB = Aw.transpose()*Bw;
+    Var = AtA.ldlt().solve(AtB);
+    res = A*Var - B;
+
+    ////Compute the energy
+    //energy = 0.f;
+    //for (unsigned int j=0; j<res.rows(); j++)
+    //	energy += log(1.f + mrpt::math::square(k*res(j)));
+    //printf("\nEnergy(%d) = %f", i, energy);
+  }
+
+  cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
+  kai_loc_level_ = Var;
+
+  //RCLCPP_INFO_STREAM(get_logger(), "[rf2o] COV_ODO:\n" << cov_odo);
+}
+
+void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/)
+{
+  //Set the initial pose
+  laser_pose_    = ini_pose;
+  laser_oldpose_ = ini_pose;
+
+  //readLaser(scan);
+  createImagePyramid();
+}
+
+/**
+ * Performs warping on the scan points of the current pyramid lvl(coarse2fine)
+*/
+void CLaserOdometry2D::performWarping()
+{
+  Eigen::Matrix3f acu_trans;
+
+  acu_trans.setIdentity();
+  for (unsigned int i=1; i<=level; i++)
+    acu_trans = transformations[i-1]*acu_trans;
+
+  Eigen::MatrixXf wacu = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
+
+  range_warped[image_level].setConstant(1, cols_i, 0.f);
+
+  const float cols_lim = float(cols_i-1);
+  const float kdtita = cols_lim/fovh;
+
+  for (unsigned int j = 0; j<cols_i; j++)
+  {
+    if (range[image_level](j) > 0.f)
+    {
+      //Transform point to the warped reference frame
+      const float x_w = acu_trans(0,0)*xx[image_level](j) + acu_trans(0,1)*yy[image_level](j) + acu_trans(0,2);
+      const float y_w = acu_trans(1,0)*xx[image_level](j) + acu_trans(1,1)*yy[image_level](j) + acu_trans(1,2);
+      const float tita_w = std::atan2(y_w, x_w);
+      const float range_w = std::sqrt(x_w*x_w + y_w*y_w);
+
+      //Calculate warping
+      const float uwarp = kdtita*(tita_w + 0.5*fovh);
+
+      //The warped pixel (which is not integer in general) contributes to all the surrounding ones
+      if (( uwarp >= 0.f)&&( uwarp < cols_lim))
+      {
+        const int uwarp_l = uwarp;
+        const int uwarp_r = uwarp_l + 1;
+        const float delta_r = float(uwarp_r) - uwarp;
+        const float delta_l = uwarp - float(uwarp_l);
+
+        //Very close pixel
+        if (std::abs(std::round(uwarp) - uwarp) < 0.05f)
+        {
+          range_warped[image_level]((int)round(uwarp)) += range_w;
+          wacu((int)std::round(uwarp)) += 1.f;
+        }
+        else
+        {
+          const float w_r = delta_l*delta_l;
+          range_warped[image_level](uwarp_r) += w_r*range_w;
+          wacu(uwarp_r) += w_r;
+
+          const float w_l = delta_r*delta_r;
+          range_warped[image_level](uwarp_l) += w_l*range_w;
+          wacu(uwarp_l) += w_l;
+        }
+      }
+    }
+  }
+
+  //Scale the averaged range and compute coordinates
+  for (unsigned int u = 0; u<cols_i; u++)
+  {
+    if (wacu(u) > 0.f)
+    {
+      const float tita = -0.5f*fovh + float(u)/kdtita;
+      range_warped[image_level](u) /= wacu(u);
+      xx_warped[image_level](u) = range_warped[image_level](u)*std::cos(tita);
+      yy_warped[image_level](u) = range_warped[image_level](u)*std::sin(tita);
+    }
+    else
+    {
+      range_warped[image_level](u) = 0.f;
+      xx_warped[image_level](u) = 0.f;
+      yy_warped[image_level](u) = 0.f;
+    }
+  }
+}
+
+bool CLaserOdometry2D::filterLevelSolution()
+{
+  //		Calculate Eigenvalues and Eigenvectors
+  //----------------------------------------------------------
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo);
+  if (eigensolver.info() != Eigen::Success)
+  {
+    // RCLCPP_WARN(get_logger(), "WARNING: Eigensolver couldn't find a solution. Pose is not updated");
+    return false;
+  }
+
+  //First, we have to describe both the new linear and angular speeds in the "eigenvector" basis
+  //-------------------------------------------------------------------------------------------------
+  Eigen::Matrix<float,3,3> Bii;
+  Eigen::Matrix<float,3,1> kai_b;
+  Bii = eigensolver.eigenvectors();
+
+  kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level_);
+
+  assert((kai_loc_level_).isApprox(Bii*kai_b, 1e-5) && "Ax=b has no solution." && __LINE__);
+
+  //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too
+  //-------------------------------------------------------------------------------------------------
+  MatrixS31 kai_loc_sub;
+
+  //Important: we have to substract the solutions from previous levels
+  Eigen::Matrix3f acu_trans;
+  acu_trans.setIdentity();
+  for (unsigned int i=0; i<level; i++)
+    acu_trans = transformations[i]*acu_trans;
+
+  kai_loc_sub(0) = -fps*acu_trans(0,2);
+  kai_loc_sub(1) = -fps*acu_trans(1,2);
+  if (acu_trans(0,0) > 1.f)
+    kai_loc_sub(2) = 0.f;
+  else
+  {
+    kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0));
+  }
+  kai_loc_sub += kai_loc_old_;
+
+  Eigen::Matrix<float,3,1> kai_b_old;
+  kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);
+
+  assert((kai_loc_sub).isApprox(Bii*kai_b_old, 1e-5) && "Ax=b has no solution." && __LINE__);
+
+  //Filter speed
+  const float cf = 15e3f*std::exp(-float(int(level))),
+              df = 0.05f*std::exp(-float(int(level)));
+
+  Eigen::Matrix<float,3,1> kai_b_fil;
+  for (unsigned int i=0; i<3; i++)
+  {
+    kai_b_fil(i) = (kai_b(i) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df);
+    //kai_b_fil_f(i,0) = (1.f*kai_b(i,0) + 0.f*kai_b_old_f(i,0))/(1.0f + 0.f);
+  }
+
+  //Transform filtered speed to local reference frame and compute transformation
+  Eigen::Matrix<float, 3, 1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);
+
+  assert((kai_b_fil).isApprox(Bii.inverse()*kai_loc_fil, 1e-5) && "Ax=b has no solution." && __LINE__);
+
+  //transformation
+  const float incrx = kai_loc_fil(0)/fps;
+  const float incry = kai_loc_fil(1)/fps;
+  const float rot   = kai_loc_fil(2)/fps;
+
+  transformations[level](0,0) = std::cos(rot);
+  transformations[level](0,1) = -std::sin(rot);
+  transformations[level](1,0) = std::sin(rot);
+  transformations[level](1,1) = std::cos(rot);
+  transformations[level](0,2) = incrx;
+  transformations[level](1,2) = incry;
+
+  return true;
+}
+
+
+void CLaserOdometry2D::PoseUpdate() {
+    Eigen::Matrix3f acu_trans;
+    acu_trans.setIdentity();
+    for (unsigned int i=1; i<=ctf_levels; i++)
+        acu_trans = transformations[i-1]*acu_trans;
+
+    kai_loc_(0) = fps*acu_trans(0,2);
+    kai_loc_(1) = fps*acu_trans(1,2);
+    if (acu_trans(0,0) > 1.f) kai_loc_(2) = 0.f;
+    else kai_loc_(2) = fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0));
+
+    float phi = rf2o::getYaw(laser_pose_.rotation());
+    kai_abs_(0) = kai_loc_(0)*std::cos(phi) - kai_loc_(1)*std::sin(phi);
+    kai_abs_(1) = kai_loc_(0)*std::sin(phi) + kai_loc_(1)*std::cos(phi);
+    kai_abs_(2) = kai_loc_(2);
+
+    laser_oldpose_ = laser_pose_;
+    Pose3d pose_aux_2D = Pose3d::Identity();
+    pose_aux_2D = rf2o::matrixYaw(double(kai_loc_(2)/fps));
+    pose_aux_2D.translation()(0) = acu_trans(0,2);
+    pose_aux_2D.translation()(1) = acu_trans(1,2);
+    laser_pose_ = laser_pose_ * pose_aux_2D;
+    last_increment_ = pose_aux_2D;
+
+    phi = rf2o::getYaw(laser_pose_.rotation());
+    kai_loc_old_(0) =  kai_abs_(0)*std::cos(phi) + kai_abs_(1)*std::sin(phi);
+    kai_loc_old_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi);
+    kai_loc_old_(2) =  kai_abs_(2);
+
+    // ✅ [CLEANED] All odom position/yaw logs removed completely.
+
+    robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_;
+
+    // ✅ [MODIFIED] Plain double arithmetic instead of rclcpp::Time operations
+    double time_inc_sec = current_scan_time_sec - last_odom_time_sec;
+    last_odom_time_sec = current_scan_time_sec;
+    
+    if (time_inc_sec > 1e-9) {
+        lin_speed = acu_trans(0,2) / time_inc_sec;
+        double ang_inc = rf2o::getYaw(robot_pose_.rotation()) - rf2o::getYaw(robot_oldpose_.rotation());
+        if (ang_inc > 3.14159) ang_inc -= 2*3.14159;
+        if (ang_inc < -3.14159) ang_inc += 2*3.14159;
+        ang_speed = ang_inc/time_inc_sec;
+    }
+    
+    robot_oldpose_ = robot_pose_;
+}
+} /* namespace rf2o */

ros2/src/rf2o_laser_odometry/src/CLaserOdometry2DNode.cpp

@@ -0,0 +1,185 @@
+/** ****************************************************************************************
+*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
+*  Adapted: Updated to work with refactored pure-C++ CLaserOdometry2D algorithm class.
+******************************************************************************************** */
+
+#include "rf2o_laser_odometry/CLaserOdometry2DNode.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // ✅ 确保包含此头文件以支持 tf2::convert
+
+using namespace rf2o;
+
+CLaserOdometry2DNode::CLaserOdometry2DNode(): Node("rf2o_laser_odometry_node")
+{
+  RCLCPP_INFO(get_logger(), "Initializing RF2O node...");
+
+  this->declare_parameter<std::string>("laser_scan_topic", "/scan");
+  this->get_parameter("laser_scan_topic", laser_scan_topic);
+  this->declare_parameter<std::string>("odom_topic", "/odom_rf2o");
+  this->get_parameter("odom_topic", odom_topic);
+  this->declare_parameter<std::string>("base_frame_id", "base_link");
+  this->get_parameter("base_frame_id", base_frame_id);
+  this->declare_parameter<std::string>("odom_frame_id", "odom");
+  this->get_parameter("odom_frame_id", odom_frame_id);
+  this->declare_parameter<bool>("publish_tf", true);
+  this->get_parameter("publish_tf", publish_tf);
+  this->declare_parameter<std::string>("init_pose_from_topic", "/base_pose_ground_truth");
+  this->get_parameter("init_pose_from_topic", init_pose_from_topic);
+  this->declare_parameter<double>("freq", 10.0);
+  this->get_parameter("freq", freq);
+
+  buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
+  odom_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+  odom_pub  = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic, 5);
+  laser_sub = this->create_subscription<sensor_msgs::msg::LaserScan>(
+      laser_scan_topic,
+      rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(),
+      std::bind(&CLaserOdometry2DNode::LaserCallBack, this, std::placeholders::_1));
+  
+  if (init_pose_from_topic != "") {
+    initPose_sub = this->create_subscription<nav_msgs::msg::Odometry>(
+        init_pose_from_topic,
+        rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(),
+        std::bind(&CLaserOdometry2DNode::initPoseCallBack, this, std::placeholders::_1));
+    GT_pose_initialized = false;
+  } else {
+    GT_pose_initialized = true;
+    initial_robot_pose.pose.pose.position.x = 0;
+    initial_robot_pose.pose.pose.position.y = 0;
+    initial_robot_pose.pose.pose.position.z = 0;
+    initial_robot_pose.pose.pose.orientation.w = 1; // ✅ 修正：初始四元数 w 应为 1
+  }
+
+  rf2o_ref.module_initialized = false;
+  rf2o_ref.first_laser_scan   = true;
+}
+
+void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::msg::LaserScan::SharedPtr new_scan)
+{
+  if (GT_pose_initialized) {
+    last_scan = *new_scan;
+    
+    // ✅ 关键修改：将 ROS 时间戳转换为 double 秒
+    rf2o_ref.current_scan_time_sec = 
+        static_cast<double>(last_scan.header.stamp.sec) + 
+        static_cast<double>(last_scan.header.stamp.nanosec) / 1e9;
+    
+    if (!rf2o_ref.first_laser_scan) {
+      for (unsigned int i = 0; i < rf2o_ref.width; i++)
+        rf2o_ref.range_wf(i) = new_scan->ranges[i];
+      new_scan_available = true;
+    } else {
+      setLaserPoseFromTf();
+      rf2o_ref.init(last_scan, initial_robot_pose.pose.pose);
+      rf2o_ref.first_laser_scan = false;
+    }
+  }
+}
+
+bool CLaserOdometry2DNode::setLaserPoseFromTf()
+{  
+  bool retrieved = false;  
+  geometry_msgs::msg::TransformStamped tf_laser;
+
+  try {
+    tf_laser = buffer_->lookupTransform(base_frame_id, last_scan.header.frame_id, tf2::TimePointZero);
+    retrieved = true;
+  } catch (const tf2::TransformException &ex) {
+    RCLCPP_ERROR(get_logger(), "%s", ex.what());
+  }
+
+  if (retrieved) {
+    tf2::Transform transform;
+    tf2::convert(tf_laser.transform, transform);
+    const tf2::Matrix3x3 &basis = transform.getBasis();
+    Eigen::Matrix3d R;
+    for(int r = 0; r < 3; r++)
+      for(int c = 0; c < 3; c++)
+        R(r,c) = basis[r][c];
+
+    Pose3d laser_tf(R);
+    const tf2::Vector3 &t = transform.getOrigin();
+    laser_tf.translation()(0) = t[0];
+    laser_tf.translation()(1) = t[1];
+    laser_tf.translation()(2) = t[2];
+    rf2o_ref.setLaserPose(laser_tf);
+  }
+  return retrieved;
+}
+
+bool CLaserOdometry2DNode::scan_available() {
+  return new_scan_available;
+}
+
+void CLaserOdometry2DNode::process()
+{
+  if (rf2o_ref.is_initialized() && scan_available()) {
+    // ✅ 关键修改：传入当前时间戳
+    double now_sec = this->now().seconds();
+    rf2o_ref.odometryCalculation(last_scan, now_sec);
+    publish();
+    new_scan_available = false;
+  } else {
+    RCLCPP_WARN(get_logger(), "Waiting for laser_scans....");
+  }
+}
+
+void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::msg::Odometry::SharedPtr new_initPose)
+{
+  if (!GT_pose_initialized) {
+    initial_robot_pose = *new_initPose;
+    GT_pose_initialized = true;
+  }
+}
+
+void CLaserOdometry2DNode::publish()
+{
+  tf2::Quaternion tf_quaternion;
+  tf_quaternion.setRPY(0.0, 0.0, rf2o::getYaw(rf2o_ref.robot_pose_.rotation()));
+  geometry_msgs::msg::Quaternion quaternion = tf2::toMsg(tf_quaternion);
+  
+  nav_msgs::msg::Odometry odom;
+  
+  // ✅ 关键修改：将 double 秒转换回 ROS 时间戳
+  odom.header.stamp.sec = static_cast<int32_t>(rf2o_ref.last_odom_time_sec);
+  odom.header.stamp.nanosec = static_cast<uint32_t>(
+      (rf2o_ref.last_odom_time_sec - odom.header.stamp.sec) * 1e9);
+      
+  odom.header.frame_id = odom_frame_id;
+  odom.pose.pose.position.x = rf2o_ref.robot_pose_.translation()(0);
+  odom.pose.pose.position.y = rf2o_ref.robot_pose_.translation()(1);
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = quaternion;
+  odom.child_frame_id = base_frame_id;
+  odom.twist.twist.linear.x = rf2o_ref.lin_speed;
+  odom.twist.twist.linear.y = 0.0;
+  odom.twist.twist.angular.z = rf2o_ref.ang_speed;
+  
+  odom_pub->publish(odom);
+
+  if (publish_tf) {
+    geometry_msgs::msg::TransformStamped odom_trans;
+    odom_trans.header.stamp = odom.header.stamp; // ✅ 复用已转换的时间戳
+    odom_trans.header.frame_id = odom_frame_id;
+    odom_trans.child_frame_id = base_frame_id;
+    odom_trans.transform.translation.x = rf2o_ref.robot_pose_.translation()(0);
+    odom_trans.transform.translation.y = rf2o_ref.robot_pose_.translation()(1);
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = quaternion;
+    odom_broadcaster->sendTransform(odom_trans);
+  }
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  auto myLaserOdomNode = std::make_shared<rf2o::CLaserOdometry2DNode>();
+  rclcpp::Rate rate(myLaserOdomNode->freq);
+
+  while (rclcpp::ok()) { 
+    rclcpp::spin_some(myLaserOdomNode);
+    myLaserOdomNode->process();
+    rate.sleep();
+  }
+  return 0;
+}

ros2/src/udp_teleop/README.md

@@ -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:<X速度>:<Y速度>:<W角速度>:XZHY\n
-```
-
-### 机械臂指令
-
-```
-JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>: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` 在终端直接运行。

ros2/src/udp_teleop/config/arm_control.yaml

@@ -34,5 +34,13 @@ arm_control:
     default_rate: 100.0      # 插值频率 (Hz)
     use_state_cache: true   # 是否使用状态缓存
 
+    # 启动复位
+    auto_reset_on_startup: true  # 启动时是否自动复位
+    reset_x: 200.0               # 复位位置 X (mm)
+    reset_y: 0.0                 # 复位位置 Y (mm)
+    reset_z: -285.0                # 复位位置 Z (mm)
+    reset_phi: 0.0               # 复位朝向 (度)
+    reset_duration: 3.0          # 复位运动时长 (秒)
+
     # 状态发布频率
     publish_rate: 10.0      # Hz

ros2/src/udp_teleop/config/box_detection_grasp.yaml

@@ -0,0 +1,26 @@
+box_detection_grasp:
+  ros__parameters:
+    # ESP32 相机流（由 camera_ip 拼接 http://<ip>/stream）
+    camera_ip: "192.168.4.1"
+
+    # YOLO 模型路径
+    model_path: "/home/fallensigh/Dev/craic/ros2/src/udp_teleop/models/box_detection.pt"
+
+    # 检测参数
+    confidence: 0.35          # 置信度阈值
+    imgsz: 768                # YOLO 输入尺寸
+    device: ""                # 空字符串=自动选择，可设为 "cpu" 或 "cuda"
+    detection_rate: 10.0      # 检测频率 (Hz)
+
+    # 方框尺寸（用于深度估计）
+    box_size_m: 0.03          # 方框边长 (米)
+
+    # 相机参数
+    horizontal_fov_deg: 66.0  # 水平视场角 (度)
+    max_frame_age: 0.5        # 最大帧延迟 (秒)
+
+    # 调试
+    show_debug_window: false  # 是否显示调试窗口
+
+    # 控制参数
+    auto_grasp: false         # 是否自动抓取（true=启动后自动检测并抓取）

ros2/src/udp_teleop/config/params.yaml

@@ -2,11 +2,11 @@ keyboard_udp_control:
   ros__parameters:
     udp_ip: "192.168.4.1"
     udp_port: 8888
-    chassis_linear_speed: 100
+    chassis_linear_speed: 250
-    chassis_angular_speed: 45
+    chassis_angular_speed: 100
     arm_height_step: 5
     arm_joint_step: 5
-    update_rate: 0.05
+    update_rate: 0.03
     debug_keys: false
     keyboard_backend: "auto"
-    stdin_hold_time: 0.04
+    stdin_hold_time: 0.08

ros2/src/udp_teleop/config/vision_grasp.yaml

@@ -5,7 +5,7 @@ vision_grasp:
     # 相机到 TCP 的变换参数
     cam_tx: 0.0      # X 平移 (mm)
     cam_ty: 0.0      # Y 平移 (mm)
-    cam_tz: 0.0      # Z 平移 (mm)
+    cam_tz: 30.0      # Z 平移 (mm)
     cam_roll: 0.0    # 绕 X 轴旋转 (度)
     cam_pitch: 0.0   # 绕 Y 轴旋转 (度)
     cam_yaw: 0.0     # 绕 Z 轴旋转 (度)

ros2/src/udp_teleop/launch/vision_grasp.launch.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+"""视觉抓取 bringup：机械臂控制 + 视觉抓取 + (可选) YOLO 方框检测。
+
+组件依赖：arm_control → vision_grasp → box_detection_grasp
+三个节点通过 ROS 服务通信，启动顺序由 lifecycle 自发处理。
+
+用法：
+  ros2 launch udp_teleop vision_grasp.launch.py
+  ros2 launch udp_teleop vision_grasp.launch.py udp_ip:=192.168.4.1 detection:=true
+  ros2 launch udp_teleop vision_grasp.launch.py detection:=true auto_grasp:=true
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('udp_teleop')
+
+    arm_params = os.path.join(pkg_share, 'config', 'arm_control.yaml')
+    vision_params = os.path.join(pkg_share, 'config', 'vision_grasp.yaml')
+    detection_params = os.path.join(pkg_share, 'config', 'box_detection_grasp.yaml')
+
+    detection = LaunchConfiguration('detection')
+    auto_grasp = LaunchConfiguration('auto_grasp')
+    udp_ip = LaunchConfiguration('udp_ip')
+    show_debug = LaunchConfiguration('show_debug')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('detection', default_value='false',
+                              description='是否启动 YOLO 方框检测节点'),
+        DeclareLaunchArgument('auto_grasp', default_value='false',
+                              description='检测到方框后自动抓取'),
+        DeclareLaunchArgument('udp_ip', default_value='192.168.4.1',
+                              description='ESP32 IP（UDP 控制 + 相机流共用）'),
+        DeclareLaunchArgument('show_debug', default_value='false',
+                              description='显示 YOLO 检测调试窗口'),
+
+        # ===== arm_control =====
+        Node(
+            package='udp_teleop',
+            executable='arm_control',
+            name='arm_control',
+            output='screen',
+            parameters=[arm_params, {'udp_ip': udp_ip}],
+        ),
+
+        # ===== vision_grasp =====
+        Node(
+            package='udp_teleop',
+            executable='vision_grasp',
+            name='vision_grasp',
+            output='screen',
+            parameters=[vision_params],
+        ),
+
+        # ===== box_detection_grasp (可选) =====
+        Node(
+            package='udp_teleop',
+            executable='box_detection_grasp',
+            name='box_detection_grasp',
+            output='screen',
+            parameters=[detection_params, {
+                'camera_ip': udp_ip,
+                'auto_grasp': auto_grasp,
+                'show_debug_window': show_debug,
+            }],
+            condition=IfCondition(detection),
+        ),
+    ])

ros2/src/udp_teleop/setup.py

@@ -15,6 +15,10 @@ setup(
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share', package_name, 'config'),
             glob('config/*.yaml')),
+        (os.path.join('share', package_name, 'launch'),
+            glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'models'),
+            glob('models/*.pt')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -31,7 +35,8 @@ setup(
         'console_scripts': [
             'keyboard_control = udp_teleop.keyboard_control:main',
             'arm_control = udp_teleop.arm_control:main',
-            'vision_grasp = udp_teleop.vision_grasp:main'
+            'vision_grasp = udp_teleop.vision_grasp:main',
+            'box_detection_grasp = udp_teleop.box_detection_grasp:main',
         ],
     },
 )

ros2/src/udp_teleop/udp_teleop/arm_control.py

@@ -50,6 +50,12 @@ J5_OPEN = 81
 J5_CLOSED = -100
 DEFAULT_FIXED_J5 = J5_OPEN
 
+# Z4 值根据夹爪状态变化
+# 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
 DEFAULT_FIXED_J6 = RELEASE_ANGLE
@@ -61,7 +67,7 @@ DEFAULT_ZERO_J4 = 25
 DEFAULT_L1 = 125.0
 DEFAULT_L2 = 125.0
 DEFAULT_X4 = 110.0
-DEFAULT_Z4 = 80.0
+DEFAULT_Z4 = 80.0  # 仅用于配置默认值，实际使用动态 z4
 
 DEFAULT_INTERP_DURATION = 1.0
 DEFAULT_INTERP_RATE = 20.0
@@ -165,13 +171,37 @@ def normalize_angle_deg(angle_deg: float) -> float:
     return normalized
 
 
-def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
+def resolve_z4_from_j5(j5: int) -> float:
-    """正运动学：关节角度 → TCP 位姿"""
+    """根据 J5 状态确定 z4 值
+
+    - J5 = -100 (闭合): 夹爪朝上，z4 = -100mm
+    - J5 = 81 (张开): 夹爪朝下，z4 = 55mm
+    """
+    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)
+    """
+    return J5_CLOSED if z > -55 else J5_OPEN
+
+
+def forward_kinematics(geometry: ArmGeometry, state: ArmMathState, z4: float) -> ArmPose:
+    """正运动学：关节角度 → TCP 位姿
+
+    Args:
+        geometry: 机械臂几何参数
+        state: 数学坐标系的关节状态
+        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
+    """
     theta2 = math.radians(state.theta2_deg)
     theta3 = math.radians(state.theta3_deg)
     theta4 = math.radians(state.theta4_deg)
     phi = theta2 + theta3 + theta4
-    
+
     j4_center_x = (
         geometry.l1 * math.cos(theta2) +
         geometry.l2 * math.cos(theta2 + theta3)
@@ -180,11 +210,11 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
         geometry.l1 * math.sin(theta2) +
         geometry.l2 * math.sin(theta2 + theta3)
     )
-    
+
     x = j4_center_x + geometry.x4 * math.cos(phi)
     y = j4_center_y + geometry.x4 * math.sin(phi)
-    z = state.d1 - geometry.z4
+    z = state.d1 - z4  # 使用动态 z4
-    
+
     return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
 
 
@@ -195,14 +225,25 @@ def inverse_kinematics(
     elbow_up: bool,
     j5: int,
     j6: int,
+    z4: float,
 ) -> ArmMathState:
-    """逆运动学：TCP 位姿 → 关节角度"""
+    """逆运动学：TCP 位姿 → 关节角度
+
+    Args:
+        geometry: 机械臂几何参数
+        pose: 目标 TCP 位姿
+        limits: 关节限位
+        elbow_up: 肘部朝上/朝下
+        j5: J5 角度
+        j6: J6 角度
+        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
+    """
     # 计算 J4 中心位置
     phi = math.radians(pose.phi_deg)
     j4_x = pose.x - geometry.x4 * math.cos(phi)
     j4_y = pose.y - geometry.x4 * math.sin(phi)
-    j4_z = pose.z + geometry.z4
+    j4_z = pose.z + z4  # 使用动态 z4
-    
+
     d1 = j4_z
     
     # 计算平面距离
@@ -352,6 +393,12 @@ class ArmControlNode(Node):
                 ('default_rate', DEFAULT_INTERP_RATE),
                 ('use_state_cache', True),
                 ('publish_rate', 10.0),
+                ('auto_reset_on_startup', True),
+                ('reset_x', 200.0),
+                ('reset_y', 0.0),
+                ('reset_z', -285.0),
+                ('reset_phi', 0.0),
+                ('reset_duration', 3.0),
             ]
         )
 
@@ -360,6 +407,14 @@ class ArmControlNode(Node):
         self.udp_port = self.get_parameter('udp_port').value
         self.publish_rate = self.get_parameter('publish_rate').value
 
+        # 复位参数
+        self.auto_reset_on_startup = self.get_parameter('auto_reset_on_startup').value
+        self.reset_x = self.get_parameter('reset_x').value
+        self.reset_y = self.get_parameter('reset_y').value
+        self.reset_z = self.get_parameter('reset_z').value
+        self.reset_phi = self.get_parameter('reset_phi').value
+        self.reset_duration = self.get_parameter('reset_duration').value
+
         # 机械臂几何参数
         self.geometry = ArmGeometry(
             l1=self.get_parameter('l1').value,
@@ -444,6 +499,65 @@ class ArmControlNode(Node):
         self.get_logger().info(f'机械臂控制节点已启动')
         self.get_logger().info(f'UDP 目标: {self.udp_ip}:{self.udp_port}')
 
+        # 自动复位
+        if self.auto_reset_on_startup:
+            self.get_logger().info('执行启动复位...')
+            import threading
+            threading.Thread(target=self._auto_reset, daemon=True).start()
+
+    def _auto_reset(self):
+        """自动复位线程"""
+        import time
+        time.sleep(1.0)  # 等待节点完全启动
+
+        try:
+            target_pose = ArmPose(
+                x=self.reset_x,
+                y=self.reset_y,
+                z=self.reset_z,
+                phi_deg=self.reset_phi
+            )
+
+            # 根据目标 z 坐标自动选择 J5 和 z4
+            j5 = resolve_j5_from_z(target_pose.z)
+            j6 = DEFAULT_FIXED_J6
+            z4 = resolve_z4_from_j5(j5)
+
+            # 逆运动学
+            math_state = inverse_kinematics(
+                geometry=self.geometry,
+                pose=target_pose,
+                limits=self.limits,
+                elbow_up=False,
+                j5=j5,
+                j6=j6,
+                z4=z4,
+            )
+
+            # 转换为命令状态
+            target_state = math_to_command_state(
+                math_state, self.zero_offsets, self.limits, j5, j6
+            )
+
+            # 生成插值轨迹
+            steps = max(1, int(self.reset_duration * self.default_rate))
+            trajectory = interpolate_command_states(
+                self.current_state, target_state, steps
+            )
+
+            # 执行运动
+            self.get_logger().info(f'复位到: ({self.reset_x:.1f}, {self.reset_y:.1f}, {self.reset_z:.1f}), phi={self.reset_phi:.1f}°')
+            for state in trajectory:
+                self.send_udp_commands([state], 0.0)
+                self.current_state = state
+                self.save_state()
+                time.sleep(1.0 / self.default_rate)
+
+            self.get_logger().info('✓ 复位完成')
+
+        except Exception as e:
+            self.get_logger().error(f'复位失败: {e}')
+
     def load_state(self):
         """从缓存加载状态"""
         if not self.use_state_cache or not STATE_FILE.exists():
@@ -565,14 +679,17 @@ class ArmControlNode(Node):
                 phi_deg=request.phi
             )
 
-            # 解析夹爪状态
+            # 解析 z4：优先级 up/down > 自动选择
-            if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
+            if request.up:
-                j5 = J5_OPEN
+                z4 = Z4_UP      # 夹爪朝上，z4=-100
-            elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
+            elif request.down:
-                j5 = J5_CLOSED
+                z4 = Z4_DOWN    # 夹爪朝下，z4=55
             else:
-                j5 = self.current_state.j5
+                # 自动选择：根据目标 z 坐标
+                j5_auto = resolve_j5_from_z(target_pose.z)
+                z4 = resolve_z4_from_j5(j5_auto)
 
+            # 解析夹爪开合（J6：grip/release）
             if request.grip:
                 j6 = GRIP_ANGLE
             elif request.release:
@@ -580,6 +697,9 @@ class ArmControlNode(Node):
             else:
                 j6 = self.current_state.j6
 
+            # 根据 z4 反推 J5（用于 UDP 命令）
+            j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN
+
             # 逆运动学
             math_state = inverse_kinematics(
                 geometry=self.geometry,
@@ -588,6 +708,7 @@ class ArmControlNode(Node):
                 elbow_up=request.elbow_up,
                 j5=j5,
                 j6=j6,
+                z4=z4,
             )
 
             # 转换为命令状态
@@ -628,19 +749,22 @@ class ArmControlNode(Node):
                 return response
 
             math_state = command_to_math_state(self.current_state, self.zero_offsets)
-            pose = forward_kinematics(self.geometry, math_state)
+
+            # 根据当前 J5 状态确定 z4
+            z4 = resolve_z4_from_j5(self.current_state.j5)
+            pose = forward_kinematics(self.geometry, math_state, z4)
 
             response.success = True
-            response.x = pose.x
+            response.x = float(pose.x)
-            response.y = pose.y
+            response.y = float(pose.y)
-            response.z = pose.z
+            response.z = float(pose.z)
-            response.phi = pose.phi_deg
+            response.phi = float(pose.phi_deg)
-            response.height = self.current_state.height
+            response.height = int(self.current_state.height)
-            response.j2 = self.current_state.j2
+            response.j2 = int(self.current_state.j2)
-            response.j3 = self.current_state.j3
+            response.j3 = int(self.current_state.j3)
-            response.j4 = self.current_state.j4
+            response.j4 = int(self.current_state.j4)
-            response.j5 = self.current_state.j5
+            response.j5 = int(self.current_state.j5)
-            response.j6 = self.current_state.j6
+            response.j6 = int(self.current_state.j6)
 
         except Exception as e:
             response.success = False
@@ -707,14 +831,15 @@ class ArmControlNode(Node):
 
             # 计算并发布 TCP 位姿
             math_state = command_to_math_state(self.current_state, self.zero_offsets)
-            pose = forward_kinematics(self.geometry, math_state)
+            z4 = resolve_z4_from_j5(self.current_state.j5)
+            pose = forward_kinematics(self.geometry, math_state, z4)
 
             pose_msg = TCPPose()
             pose_msg.header.stamp = self.get_clock().now().to_msg()
-            pose_msg.x = pose.x
+            pose_msg.x = float(pose.x)
-            pose_msg.y = pose.y
+            pose_msg.y = float(pose.y)
-            pose_msg.z = pose.z
+            pose_msg.z = float(pose.z)
-            pose_msg.phi = pose.phi_deg
+            pose_msg.phi = float(pose.phi_deg)
             self.pub_tcp_pose.publish(pose_msg)
 
         except Exception as e:

ros2/src/udp_teleop/udp_teleop/box_detection_grasp.py

@@ -0,0 +1,381 @@
+#!/usr/bin/env python3
+"""方框检测与自动抓取节点
+
+基于 YOLO 模型检测方框，并自动调用视觉抓取节点完成抓取。
+
+功能：
+1. 启动参数控制：auto_grasp=true 时自动抓取
+2. 实时检测方框
+3. 检测成功后停止识别
+4. 发布方框中心坐标到 /vision_grasp/grasp_target
+5. 等待抓取完成
+"""
+
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Point
+from std_srvs.srv import Trigger
+
+try:
+    import cv2
+except ImportError:
+    cv2 = None
+
+try:
+    import numpy as np
+except ImportError:
+    np = None
+
+try:
+    from ultralytics import YOLO
+except ImportError:
+    YOLO = None
+
+
+class MjpegFrameReader:
+    """持续读取 ESP32 MJPEG 流并保持最新帧"""
+
+    def __init__(self, url, logger, reconnect_delay=1.0):
+        self.url = url
+        self.logger = logger
+        self.reconnect_delay = reconnect_delay
+        self.lock = threading.Lock()
+        self.latest_frame = None
+        self.latest_stamp = None
+        self.running = False
+        self.thread = None
+        self.capture = None
+        self.last_log_time = 0.0
+
+    def start(self):
+        self.running = True
+        self.thread = threading.Thread(target=self._run, daemon=True)
+        self.thread.start()
+
+    def stop(self):
+        self.running = False
+        if self.thread is not None:
+            self.thread.join(timeout=2.0)
+        if self.capture is not None:
+            self.capture.release()
+            self.capture = None
+
+    def get_latest(self):
+        with self.lock:
+            if self.latest_frame is None:
+                return None, None
+            return self.latest_frame.copy(), self.latest_stamp
+
+    def _log_periodic(self, message, period=5.0):
+        now = time.monotonic()
+        if now - self.last_log_time >= period:
+            self.logger.warning(message)
+            self.last_log_time = now
+
+    def _open_capture(self):
+        cap = cv2.VideoCapture(self.url)
+        if not cap.isOpened():
+            cap.release()
+            return None
+
+        cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+        self.logger.info(f"已连接到 ESP32 流: {self.url}")
+        return cap
+
+    def _run(self):
+        while self.running:
+            if self.capture is None:
+                self.capture = self._open_capture()
+                if self.capture is None:
+                    self._log_periodic(f"无法打开 ESP32 流 {self.url}，重试中...")
+                    time.sleep(self.reconnect_delay)
+                    continue
+
+            ok, frame = self.capture.read()
+            if not ok or frame is None:
+                self._log_periodic("ESP32 流读取失败，重新连接中...")
+                self.capture.release()
+                self.capture = None
+                time.sleep(self.reconnect_delay)
+                continue
+
+            with self.lock:
+                self.latest_frame = frame
+                self.latest_stamp = time.monotonic()
+
+
+class BoxDetectionGraspNode(Node):
+    """方框检测与自动抓取节点"""
+
+    def __init__(self):
+        super().__init__('box_detection_grasp')
+
+        # 检查依赖
+        if cv2 is None:
+            raise RuntimeError("需要 OpenCV。安装: pip install opencv-python")
+        if np is None:
+            raise RuntimeError("需要 NumPy。安装: pip install numpy")
+        if YOLO is None:
+            raise RuntimeError("需要 ultralytics。安装: pip install ultralytics")
+
+        # 声明参数
+        self.declare_parameter('camera_ip', '192.168.4.1')
+        self.declare_parameter('model_path', '/home/fallensigh/Dev/craic/tmp/best.pt')
+        self.declare_parameter('confidence', 0.35)
+        self.declare_parameter('imgsz', 768)
+        self.declare_parameter('device', '')
+        self.declare_parameter('detection_rate', 10.0)
+        self.declare_parameter('box_size_m', 0.03)
+        self.declare_parameter('horizontal_fov_deg', 66.0)
+        self.declare_parameter('max_frame_age', 0.5)
+        self.declare_parameter('show_debug_window', False)
+        self.declare_parameter('auto_grasp', False)  # 是否自动抓取
+
+        # 获取参数
+        self.stream_url = f"http://{self.get_parameter('camera_ip').value}/stream"
+        self.model_path = self.get_parameter('model_path').value
+        self.confidence = float(self.get_parameter('confidence').value)
+        self.imgsz = int(self.get_parameter('imgsz').value)
+        self.device = str(self.get_parameter('device').value)
+        self.detection_rate = float(self.get_parameter('detection_rate').value)
+        self.box_size_m = float(self.get_parameter('box_size_m').value)
+        self.horizontal_fov_deg = float(self.get_parameter('horizontal_fov_deg').value)
+        self.max_frame_age = float(self.get_parameter('max_frame_age').value)
+        self.show_debug_window = bool(self.get_parameter('show_debug_window').value)
+        self.auto_grasp = bool(self.get_parameter('auto_grasp').value)
+
+        # 状态标志
+        self.detection_active = self.auto_grasp  # 如果 auto_grasp=true，自动开始检测
+        self.grasp_in_progress = False
+
+        # 发布器：发布到视觉抓取节点
+        self.grasp_target_pub = self.create_publisher(Point, '/vision_grasp/grasp_target', 10)
+
+        # 服务：外部触发检测
+        self.start_detection_srv = self.create_service(
+            Trigger,
+            'box_detection/start',
+            self.handle_start_detection
+        )
+        self.stop_detection_srv = self.create_service(
+            Trigger,
+            'box_detection/stop',
+            self.handle_stop_detection
+        )
+
+        # 启动相机流读取器
+        self.reader = MjpegFrameReader(self.stream_url, self.get_logger())
+        self.reader.start()
+
+        # 加载 YOLO 模型
+        self.get_logger().info(f'加载 YOLO 模型: {self.model_path}')
+        self.model = YOLO(self.model_path)
+        self.last_no_detection_log = 0.0
+
+        # 创建定时器
+        period = 1.0 / max(self.detection_rate, 0.1)
+        self.timer = self.create_timer(period, self.process_frame)
+
+        if self.auto_grasp:
+            self.get_logger().info('自动抓取模式已启动，开始实时检测方框...')
+        else:
+            self.get_logger().info('手动模式，等待外部触发检测')
+
+    def process_frame(self):
+        """处理最新帧"""
+        if not self.detection_active or self.grasp_in_progress:
+            return
+
+        frame, frame_stamp = self.reader.get_latest()
+        if frame is None:
+            return
+
+        # 检查帧是否过期
+        if frame_stamp is not None and time.monotonic() - frame_stamp > self.max_frame_age:
+            return
+
+        # YOLO 检测
+        predict_kwargs = {
+            'source': frame,
+            'imgsz': self.imgsz,
+            'conf': self.confidence,
+            'verbose': False,
+        }
+        if self.device:
+            predict_kwargs['device'] = self.device
+
+        results = self.model.predict(**predict_kwargs)
+        detection = self.select_best_box(results[0], frame)
+
+        if detection is None:
+            self.log_no_detection()
+            if self.show_debug_window:
+                cv2.imshow('box_detection', frame)
+                cv2.waitKey(1)
+            return
+
+        # 估计 3D 坐标
+        point = self.estimate_camera_point(detection, frame.shape)
+        if point is None:
+            return
+
+        # 检测成功！停止检测并触发抓取
+        self.get_logger().info(f'✓ 检测到方框: 相机坐标 ({point[0]*1000:.1f}, {point[1]*1000:.1f}, {point[2]*1000:.1f}) mm')
+        self.detection_active = False  # 停止检测
+        self.grasp_in_progress = True
+
+        if self.show_debug_window:
+            self.show_debug_frame(frame, detection, point)
+
+        # 发布抓取目标（转换为 mm）
+        msg = Point()
+        msg.x = point[0] * 1000.0  # m -> mm
+        msg.y = point[1] * 1000.0
+        msg.z = point[2] * 1000.0
+        self.grasp_target_pub.publish(msg)
+        self.get_logger().info('已发布抓取目标到 /vision_grasp/grasp_target')
+
+        # 在独立线程中等待抓取完成
+        threading.Thread(target=self._wait_for_grasp_completion, daemon=True).start()
+
+    def select_best_box(self, result, frame):
+        """选择最佳检测框"""
+        if result.boxes is None or len(result.boxes) == 0:
+            return None
+
+        best = None
+        best_conf = -1.0
+
+        for box in result.boxes:
+            conf = float(box.conf[0].item()) if box.conf is not None else 0.0
+            xyxy = box.xyxy[0].detach().cpu().numpy().astype(float)
+            x1, y1, x2, y2 = xyxy
+            w = max(0.0, x2 - x1)
+            h = max(0.0, y2 - y1)
+
+            if w < 2.0 or h < 2.0:
+                continue
+
+            if conf > best_conf:
+                best_conf = conf
+                best = {
+                    'xyxy': xyxy,
+                    'confidence': conf,
+                }
+
+        return best
+
+    def estimate_camera_point(self, detection, frame_shape):
+        """估计相机坐标系 3D 坐标（单位：米）"""
+        frame_h, frame_w = frame_shape[:2]
+        x1, y1, x2, y2 = detection['xyxy']
+        bbox_w = max(1.0, x2 - x1)
+        bbox_h = max(1.0, y2 - y1)
+        pixel_side = (bbox_w + bbox_h) * 0.5
+
+        # 相机内参
+        cx = frame_w * 0.5
+        cy = frame_h * 0.5
+        fov_rad = math.radians(max(1.0, min(self.horizontal_fov_deg, 179.0)))
+        fx = frame_w / (2.0 * math.tan(fov_rad * 0.5))
+        fy = fx
+
+        if fx <= 0.0 or fy <= 0.0:
+            self.get_logger().error('无效的相机内参')
+            return None
+
+        # 估计深度
+        focal = (fx + fy) * 0.5
+        z = self.box_size_m * focal / pixel_side
+
+        # 计算 3D 坐标
+        u = (x1 + x2) * 0.5
+        v = (y1 + y2) * 0.5
+        x = (u - cx) * z / fx
+        y = (v - cy) * z / fy
+
+        return x, y, z
+
+    def show_debug_frame(self, frame, detection, point):
+        """显示调试窗口"""
+        x1, y1, x2, y2 = detection['xyxy'].astype(int)
+        cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+        text = f"x={point[0]*1000:.1f} y={point[1]*1000:.1f} z={point[2]*1000:.1f} mm"
+        cv2.putText(
+            frame, text, (x1, max(20, y1 - 8)),
+            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA
+        )
+        cv2.imshow('box_detection', frame)
+        cv2.waitKey(1)
+
+    def log_no_detection(self):
+        """周期性记录未检测到目标"""
+        now = time.monotonic()
+        if now - self.last_no_detection_log >= 5.0:
+            self.get_logger().info('当前帧未检测到方框')
+            self.last_no_detection_log = now
+
+    def _wait_for_grasp_completion(self):
+        """等待抓取完成（独立线程）"""
+        self.get_logger().info('等待抓取完成...')
+        time.sleep(10.0)  # 等待抓取流程完成
+        self.grasp_in_progress = False
+        self.get_logger().info('抓取流程完成，节点已停止检测')
+
+    def handle_start_detection(self, request, response):
+        """处理启动检测服务"""
+        if self.detection_active:
+            response.success = False
+            response.message = '检测已在运行中'
+            return response
+
+        if self.grasp_in_progress:
+            response.success = False
+            response.message = '抓取流程正在进行中，请等待完成'
+            return response
+
+        self.detection_active = True
+        self.get_logger().info('✓ 启动方框检测')
+        response.success = True
+        response.message = '已启动检测'
+        return response
+
+    def handle_stop_detection(self, request, response):
+        """处理停止检测服务"""
+        if not self.detection_active:
+            response.success = False
+            response.message = '检测未运行'
+            return response
+
+        self.detection_active = False
+        self.get_logger().info('✓ 停止方框检测')
+        response.success = True
+        response.message = '已停止检测'
+        return response
+
+    def destroy_node(self):
+        """节点销毁时的清理"""
+        self.reader.stop()
+        if self.show_debug_window:
+            cv2.destroyAllWindows()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BoxDetectionGraspNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/udp_teleop/udp_teleop/keyboard_control.py

@@ -104,6 +104,7 @@ class KeyboardUdpControlNode(Node):
         self.old_terminal_settings = None
 
         self._stdin_buf = ""
+        self._tick_count = 0
 
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 
@@ -326,7 +327,9 @@ class KeyboardUdpControlNode(Node):
         if self.arm_active or self.is_arm_key_pressed(keys):
             self.sock.sendto(self.build_arm_cmd(), (self.udp_ip, self.udp_port))
 
-        self.print_status(keys)
+        self._tick_count += 1
+        if self._tick_count % 5 == 0:
+            self.print_status(keys)
 
     def print_status(self, keys):
         selected_joint = self.arm_selected_joint + 2

ros2/src/udp_teleop/udp_teleop/vision_grasp.py

@@ -71,10 +71,14 @@ def tcp_to_base(
     """TCP 坐标系 → 机械臂基坐标系（水平相机版本）"""
     phi = math.radians(tcp_phi_deg)
 
+    # 旋转矩阵：TCP → 基坐标系
+    # TCP X → 基坐标 -Y
+    # TCP Y → 基坐标 -Z
+    # TCP Z → 基坐标 X
     R_tcp_to_base = np.array([
-        [-math.sin(phi), 0, math.cos(phi)],
+        [-math.sin(phi), 0,  math.cos(phi)],   # X_base
-        [ math.cos(phi), 0, math.sin(phi)],
+        [-math.cos(phi), 0, -math.sin(phi)],   # Y_base (修正：添加负号)
-        [0,             -1, 0]
+        [0,             -1,  0]                # Z_base
     ])
 
     P_tcp = np.array([xt, yt, zt])
@@ -183,8 +187,8 @@ class VisionGraspNode(Node):
         self.get_logger().info('  - /vision_grasp/grasp_target (geometry_msgs/Point)')
         self.get_logger().info('  - /vision_grasp/release_target (geometry_msgs/Point)')
 
-    def get_current_tcp_pose(self) -> Tuple[float, float, float, float]:
+    def get_current_tcp_pose(self) -> Tuple[float, float, float, float, int]:
-        """查询当前 TCP 位姿"""
+        """查询当前 TCP 位姿（包括 J5 状态）"""
         req = GetPose.Request()
         future = self.get_pose_cli.call_async(req)
 
@@ -202,7 +206,7 @@ class VisionGraspNode(Node):
         if not result.success:
             raise RuntimeError(f"获取 TCP 位姿失败: {result.message}")
 
-        return result.x, result.y, result.z, result.phi
+        return result.x, result.y, result.z, result.phi, result.j5
 
     def move_to(self, x: float, y: float, z: float, phi: float,
                 duration: float, grip: bool = False, release: bool = False) -> bool:
@@ -280,18 +284,31 @@ class VisionGraspNode(Node):
     def _execute_grasp_thread(self, x: float, y: float, z: float):
         """在独立线程中执行抓取流程"""
         try:
-            # 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z)
+            # 获取当前 TCP 位姿（包括 J5 状态）
-            xc = x
+            tcp_x, tcp_y, tcp_z, tcp_phi, j5 = self.get_current_tcp_pose()
-            yc = -y
+            self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°, j5={j5}°')
-            zc = z
 
-            self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+            # 图像坐标到相机坐标系转换
+            # 相机水平安装，但会随 J5 状态旋转 180°
+            # J5 = -100 (闭合) → 夹爪朝上 (UP) → 相机正向
+            # J5 = 81 (张开) → 夹爪朝下 (DOWN) → 相机旋转 180°
 
-            # 获取当前 TCP 位姿
+            if j5 < 0:
-            tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose()
+                # J5 闭合（UP）：相机正向
-            self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°')
+                # 图像 Y 向下 → 相机 -Y
+                xc = x
+                yc = -y
+                zc = z
+                self.get_logger().info(f'J5={j5}° (UP)，相机正向，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+            else:
+                # J5 张开（DOWN）：相机旋转 180°
+                # 图像 X,Y 都翻转（相对于 UP 状态）
+                xc = -x
+                yc = y
+                zc = z
+                self.get_logger().info(f'J5={j5}° (DOWN)，相机旋转 180°，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
 
-            # 坐标变换
+            # 坐标变换：相机 → 基坐标系
             target_x, target_y, target_z = camera_to_base(
                 xc, yc, zc,
                 tcp_x, tcp_y, tcp_z, tcp_phi,
@@ -323,7 +340,7 @@ class VisionGraspNode(Node):
         """在独立线程中执行释放流程"""
         try:
             # 获取当前 TCP 位姿（用于获取 phi）
-            _, _, _, tcp_phi = self.get_current_tcp_pose()
+            _, _, _, tcp_phi, _ = self.get_current_tcp_pose()
 
             # 执行释放流程
             self.execute_release(x, y, z, tcp_phi)

ros2/src/ydlidar_ros2_driver/CMakeLists.txt

@@ -0,0 +1,88 @@
+# Copyright(c) 2020 eaibot limited.
+cmake_minimum_required(VERSION 3.5)
+project(ydlidar_ros2_driver C CXX)
+
+##################ros2#############################################
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+####################find package#####################################
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rmw REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
+############## YDLIDAR SDK START#####################################
+#find ydlidar_sdk package
+find_package(ydlidar_sdk REQUIRED)
+############## YDLIDAR SDK END#####################################
+
+#Include directories
+include_directories(
+  ${PROJECT_SOURCE_DIR}
+  ${PROJECT_SOURCE_DIR}/src
+  ${YDLIDAR_SDK_INCLUDE_DIRS})
+
+#link library directories
+link_directories(${YDLIDAR_SDK_LIBRARY_DIRS})
+
+#---------------------------------------------------------------------------------------
+# generate excutable and add libraries
+#---------------------------------------------------------------------------------------
+add_executable(${PROJECT_NAME}_node
+    src/${PROJECT_NAME}_node.cpp)
+#---------------------------------------------------------------------------------------
+# link libraries
+#--------------------------------------------------------------------------------------
+ament_target_dependencies(${PROJECT_NAME}_node
+    "rclcpp"
+    "sensor_msgs"
+    "visualization_msgs"
+    "geometry_msgs"
+    "std_srvs"
+    )
+
+target_link_libraries(${PROJECT_NAME}_node
+    ${YDLIDAR_SDK_LIBRARIES})
+
+#---------------------------------------------------------------------------------------
+# generate excutable and add libraries
+#---------------------------------------------------------------------------------------
+add_executable(${PROJECT_NAME}_client
+    src/${PROJECT_NAME}_client.cpp)
+#---------------------------------------------------------------------------------------
+# link libraries
+#--------------------------------------------------------------------------------------
+ament_target_dependencies(${PROJECT_NAME}_client
+    "rclcpp"
+    "sensor_msgs"
+    "visualization_msgs"
+    "geometry_msgs"
+    "std_srvs"
+    )
+
+#---------------------------------------------------------------------------------------
+# Install
+#---------------------------------------------------------------------------------------
+install(TARGETS
+    ${PROJECT_NAME}_node ${PROJECT_NAME}_client
+    DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch params startup config
+    DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+

ros2/src/ydlidar_ros2_driver/README.md

@@ -0,0 +1,187 @@
+![YDLIDAR](images/YDLidar.jpg  "YDLIDAR")
+# YDLIDAR ROS2 Driver
+
+ydlidar_ros2_driver is a new ros package, which is designed to gradually become the standard driver package for ydlidar devices in the ros2 environment.
+
+## How to [install ROS2](https://index.ros.org/doc/ros2/Installation)
+[ubuntu](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/)
+
+[windows](https://index.ros.org/doc/ros2/Installation/Dashing/Windows-Install-Binary/)
+
+## How to Create a ROS2 workspace
+[Create a workspace](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#create-a-workspace)
+
+
+## Compile & Install YDLidar SDK
+
+ydlidar_ros2_driver depends on YDLidar-SDK library. If you have never installed YDLidar-SDK library or it is out of date, you must first install YDLidar-SDK library. If you have installed the latest version of YDLidar-SDK, skip this step and go to the next step.
+
+1. Download or clone the [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) repository on GitHub.
+2. Compile and install the YDLidar-SDK under the ***build*** directory following `README.md` of YDLIDAR/YDLidar-SDK.
+
+## Clone ydlidar_ros2_driver
+
+1. Clone ydlidar_ros2_driver package for github : 
+
+   `git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver`
+
+2. Build ydlidar_ros2_driver package :
+
+   ```
+   cd ydlidar_ros2_ws
+   colcon build --symlink-install
+   ```
+   Note: install colcon [see](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#install-colcon)
+
+   ![CMAKE Finished](images/finished.png  "CMAKE Finished")
+
+   <font color=Red size=4>>Note: If the following error occurs, Please install  [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) first.</font>
+
+   ![CMAKE ERROR](images/cmake_error.png  "CMAKE ERROR")
+
+3. Package environment setup :
+
+   `source ./install/setup.bash`
+
+    Note: Add permanent workspace environment variables.
+    It's convenientif the ROS2 environment variables are automatically added to your bash session every time a new shell is launched:
+    ```
+    $echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc
+    $source ~/.bashrc
+    ```
+4. Confirmation
+    To confirm that your package path has been set, printenv the `grep -i ROS` variable.
+    ```
+    $ printenv | grep -i ROS
+    ```
+    You should see something similar to:
+        `OLDPWD=/home/tony/ydlidar_ros2_ws/install`
+
+5. Create serial port Alias [optional] 
+    ```
+	$chmod 0777 src/ydlidar_ros2_driver/startup/*
+	$sudo sh src/ydlidar_ros2_driver/startup/initenv.sh
+    ```
+    Note: After completing the previous operation, replug the LiDAR again.
+	
+## Configure LiDAR [paramters](params/ydlidar.yaml)
+```
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 9
+    abnormal_check_count: 4
+    resolution_fixed: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 64.0
+    range_min: 0.01
+    frequency: 10.0
+    invalid_range_is_inf: false
+```
+
+## Run ydlidar_ros2_driver
+
+##### Run ydlidar_ros2_driver using launch file
+
+The command format is : 
+
+ `ros2 launch ydlidar_ros2_driver [launch file].py`
+
+1. Connect LiDAR uint(s).
+   ```
+   ros2 launch ydlidar_ros2_driver ydlidar_launch.py 
+   ```
+   or 
+
+   ```
+   launch $(ros2 pkg prefix ydlidar_ros2_driver)/share/ydlidar_ros2_driver/launch/ydlidar.py 
+   ```
+2. RVIZ 
+   ```
+   ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py 
+   ```
+    ![View](images/view.png  "View")
+
+3. echo scan topic
+   ```
+   ros2 run ydlidar_ros2_driver ydlidar_ros2_driver_client or ros2 topic echo /scan
+   ```
+
+#####  Launch file introduction
+
+The driver offers users a wealth of options when using different launch file. The launch file directory    
+
+is `"ydlidar_ros2_ws/src/ydlidar_ros2_driver/launch"`. All launch files are listed as below : 
+
+| launch file               | features                                                     |
+| ------------------------- | ------------------------------------------------------------ |
+| ydlidar.py         | Connect to defualt paramters<br/>Publish LaserScan message on `scan` topic |
+| ydlidar_launch.py         | Connect ydlidar.yaml Lidar specified by configuration parameters<br/>Publish LaserScan message on `scan` topic |
+| ydlidar_launch_view.py         | Connect ydlidar.yaml Lidar specified by configuration parameters and setup RVIZ<br/>Publish LaserScan message on `scan` topic |
+
+
+
+## Publish Topic
+| Topic                | Type                    | Description                                      |
+|----------------------|-------------------------|--------------------------------------------------|
+| `scan`               | sensor_msgs/LaserScan   | 2D laser scan of the 0-angle ring                |
+
+## Subscribe Service
+| Service                | Type                    | Description                                      |
+|----------------------|-------------------------|--------------------------------------------------|
+| `stop_scan`          | std_srvs::Empty   | turn off lidar                                         |
+| `start_scan`         | std_srvs::Empty   | turn on lidar                                          |
+
+
+
+## Configure ydlidar_ros_driver internal parameter
+
+The ydlidar_ros2_driver internal parameters are in the launch file, they are listed as below :
+
+| Parameter name | Data Type | detail                                                       |
+| -------------- | ------- | ------------------------------------------------------------ |
+| port         | string | Set Lidar the serial port or IP address <br/>it can be set to `/dev/ttyUSB0`, `192.168.1.11`, etc. <br/>default: `/dev/ydlidar` |
+| frame_id     | string | Lidar TF coordinate system name. <br/>default: `laser_frame` |
+| ignore_array | string | LiDAR filtering angle area<br/>eg: `-90, -80, 30, 40` |
+| baudrate     | int | Lidar baudrate or network port. <br/>default: `230400` |
+| lidar_type     | int | Set lidar type <br/>0 -- TYPE_TOF<br/>1 -- TYPE_TRIANGLE<br/>2 -- TYPE_TOF_NET <br/>default: `1` |
+| device_type     | int | Set device type <br/>0 -- YDLIDAR_TYPE_SERIAL<br/>1 -- YDLIDAR_TYPE_TCP<br/>2 -- YDLIDAR_TYPE_UDP <br/>default: `0` |
+| sample_rate     | int | Set Lidar Sample Rate. <br/>default: `9` |
+| abnormal_check_count     | int | Set the number of abnormal startup data attempts. <br/>default: `4` |
+| fixed_resolution     | bool | Fixed angluar resolution. <br/>default: `true` |
+| reversion     | bool | Reversion LiDAR. <br/>default: `true` |
+| inverted     | bool | Inverted LiDAR.<br/>false -- ClockWise.<br/>true -- CounterClockWise  <br/>default: `true` |
+| auto_reconnect     | bool | Automatically reconnect the LiDAR.<br/>true -- hot plug. <br/>default: `true` |
+| isSingleChannel     | bool | Whether LiDAR is a single-channel.<br/>default: `false` |
+| intensity     | bool | Whether LiDAR has intensity.<br/>true -- G2 LiDAR.<br/>default: `false` |
+| support_motor_dtr     | bool | Whether the Lidar can be started and stopped by Serial DTR.<br/>default: `false` |
+| angle_min     | float | Minimum Valid Angle.<br/>default: `-180` |
+| angle_max     | float | Maximum Valid Angle.<br/>default: `180` |
+| range_min     | float | Minimum Valid range.<br/>default: `0.1` |
+| range_max     | float | Maximum Valid range.<br/>default: `16.0` |
+| frequency     | float | Set Scanning Frequency.<br/>default: `10.0` |
+| invalid_range_is_inf     | bool | Invalid Range is inf.<br/>true -- inf.<br/>false -- 0.0.<br/>default: `false` |
+More paramters details, see [here](details.md)
+
+## Contact EAI
+![Development Path](images/EAI.png)
+
+If you have any extra questions, please feel free to [contact us](http://www.ydlidar.cn/cn/contact)
+
+
+
+
+
+

ros2/src/ydlidar_ros2_driver/config/ydlidar.rviz

@@ -0,0 +1,177 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 617
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1012
+      Min Color: 0; 0; 0
+      Min Intensity: 1008
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05999999865889549
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: System Default
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        laser_frame:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          laser_frame:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5103975534439087
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.163581848144531
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 67
+  Y: 60

ros2/src/ydlidar_ros2_driver/details.md

@@ -0,0 +1,133 @@
+# ROS Paramters Table
+
+## Dataset
+<table>
+<tr><th>LIDAR      <th> Model  <th>  Baudrate <th>  SampleRate(K) <th> Range(m)  		<th>  Frequency(HZ) <th> Intenstiy(bit) <th> SingleChannel<th> voltage(V)
+<tr><th> F4        <td> 1	   <td>  115200   <td>   4            <td>  0.12~12         <td> 5~12           <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> S4        <td> 4	   <td>  115200   <td>   4            <td>  0.10~8.0        <td> 5~12 (PWM)     <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> S4B       <td> 4/11   <td>  153600   <td>   4            <td>  0.10~8.0        <td> 5~12(PWM)      <td> true(8)        <td> false    	  <td> 4.8~5.2
+<tr><th> S2        <td> 4/12   <td>  115200   <td>   3            <td>  0.10~8.0     	<td> 4~8(PWM)       <td> false          <td> true    	  <td> 4.8~5.2
+<tr><th> G4        <td> 5	   <td>  230400   <td>   9/8/4        <td>  0.28/0.26/0.1~16<td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> X4        <td> 6	   <td>  128000   <td>   5            <td>  0.12~10     	<td> 5~12(PWM)      <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> X2/X2L    <td> 6	   <td>  115200   <td>   3            <td>  0.10~8.0     	<td> 4~8(PWM)       <td> false          <td> true    	  <td> 4.8~5.2
+<tr><th> G4PRO     <td> 7	   <td>  230400   <td>   9/8/4        <td>  0.28/0.26/0.1~16<td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> F4PRO     <td> 8	   <td>  230400   <td>   4/6          <td>  0.12~12         <td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> R2        <td> 9	   <td>  230400   <td>   5            <td>  0.12~16         <td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G6        <td> 13     <td>  512000   <td>   18/16/8      <td>  0.28/0.26/0.1~25<td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G2A       <td> 14	   <td>  230400   <td>   5            <td>  0.12~12         <td> 5~12      	    <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G2        <td> 15     <td>  230400   <td>   5            <td>  0.28~16     	<td> 5~12      	    <td> true(8)        <td> false    	  <td> 4.8~5.2
+<tr><th> G2C       <td> 16	   <td>  115200   <td>   4            <td>  0.1~12        	<td> 5~12      	    <td> false      	<td> false    	  <td> 4.8~5.2
+<tr><th> G4B       <td> 17	   <td>  512000   <td>   10           <td>  0.12~16         <td> 5~12        	<td> true(10)       <td> false    	  <td> 4.8~5.2
+<tr><th> G4C       <td> 18	   <td>  115200   <td>   4            <td>  0.1~12		    <td> 5~12           <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G1        <td> 19	   <td>  230400   <td>   9            <td>  0.28~16         <td> 5~12      	    <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G5        <td> 20	   <td>  230400   <td>   9/8/4        <td>  0.28/0.26/0.1~16<td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> G7        <td> 21         <td>  512000   <td>   18/16/8      <td>  0.28/0.26/0.1~25<td> 5~12        	<td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> TX8    　 <td> 100	   <td>  115200   <td>   4            <td>  0.05~8      	<td> 4~8(PWM)       <td> false          <td> true      	  <td> 4.8~5.2
+<tr><th> TX20    　<td> 100	   <td>  115200   <td>   4            <td>  0.05~20      	<td> 4~8(PWM)       <td> false          <td> true     	  <td> 4.8~5.2
+<tr><th> TG15    　<td> 100	   <td>  512000   <td>   20/18/10     <td>  0.05~30      	<td> 3~16      	    <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> TG30    　<td> 101	   <td>  512000   <td>   20/18/10     <td>  0.05~30      	<td> 3~16      	    <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> TG50    　<td> 102	   <td>  512000   <td>   20/18/10     <td>  0.05~50      	<td> 3~16      	    <td> false          <td> false    	  <td> 4.8~5.2
+<tr><th> T15     　<td> 200	   <td>  8000     <td>   20           <td>  0.05~30      	<td> 5~35      	    <td> true           <td> false    	  <td> 4.8~5.2
+</table>
+
+## Baudrate Table
+
+| LiDAR                					| baudrate               | 
+|-----------------------------------------------|-----------------------|
+|F4/S2/X2/X2L/S4/TX8/TX20/G4C 		| 115200			|
+|X4                   					| 128000			|
+|S4B                         				| 153600			|
+|G1/G2/R2/G4/G5/G4PRO/F4PRO         	| 230400			|
+|G6/G7/TG15/TG30/TG50			 	| 512000			|
+|T5/T15			 	                | 8000 (network port)			|
+
+
+## SingleChannel Table
+
+| LiDAR                							| isSingleChannel       | 
+|-----------------------------------------------------------|-----------------------|
+|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C 	| false			|
+|S2/X2/X2L                   					| true			|
+|TG15/TG30/TG50                         		| false			|
+|TX8/TX20         							    | true			|
+|T5/T15        							    | false	(optional)		|
+
+
+## LidarType Table
+
+| LiDAR                									| lidar_type             | 
+|-----------------------------------------------------------------------|-----------------------|
+|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L 	| TYPE_TRIANGLE			|
+|TG15/TG30/TG50/TX8/TX20                   				| TYPE_TOF			|
+|T5/T15                 				                | TYPE_TOF_NET			|
+
+## DeviceType Table
+
+| LiDAR                									| lidar_type             | 
+|-----------------------------------------------------------------------|-----------------------|
+|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L 	| YDLIDAR_TYPE_SERIAL			|
+|TG15/TG30/TG50/TX8/TX20                   				| YDLIDAR_TYPE_SERIAL			|
+|T5/T15                 				                | YDLIDAR_TYPE_TCP			|
+
+
+## Sampling Rate Table
+
+| LiDAR                		| sample_rate             | 
+|-----------------------------|------------------------|
+|G4/G5/F4                   | 4,8,9			 |
+|F4PRO                   	| 4,6   	     |
+|G6/G7                      | 8,16,18		 |
+|G2/R2/X4         		    | 5				 |
+|G1 	                    | 9			 	 |
+|S4/S4B/G4C/TX8/TX20 	    | 4			 	 |
+|S2                    		| 3			 	 |
+|TG15/TG30/TG50             | 10,18,20		 |
+|T5/T15                     | 20		     |
+
+
+## Frequency Table
+
+| LiDAR                					| frequency             | 
+|-----------------------------------------------|------------------------|
+|G1/G2/R2/G6/G7/G4/G5/G4PRO/F4/F4PRO	| 5-12Hz			 |
+|S4/S4B/S2/TX8/TX20/X4 			        | Not Support		 |
+|TG15/TG30/TG50           			    | 3-16Hz			 |
+|T5/T15           			            | 5-35Hz			 |
+
+Note: For unsupported LiDARs, adjusting the scanning frequency requires external access to PWM speed control.
+
+## Reversion Table
+ <table>
+      <tr><th>LiDAR                           <th>reversion
+      <tr><th>G1/G2/G2A/G2C/F4/F4PRO/R2       <td>true
+      <tr><th>G4/G5/G4PRO/G4B/G4C/G6/G7       <td>true
+      <tr><th>TG15/TG30/TG50                  <td>true
+      <tr><th>T5/T15                          <td>true
+      <tr><th>S2/X2/X2L/X4/S4/S4B             <td>false
+      <tr><th>TX8/TX20                        <td>false
+  </table>
+
+## Intensity Table
+ <table>
+    <tr><th>LiDAR                                    <th>intensity
+      <tr><th>S4B/G2/G4B                             <td>true
+      <tr><th>G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7         <td>false
+      <tr><th>G1/G2A/G2C/R2                          <td>false
+      <tr><th>S2/X2/X2L/X4                           <td>false
+      <tr><th>TG15/TG30/TG50                         <td>false
+      <tr><th>TX8/TX20                               <td>false
+      <tr><th>T5/T15                                 <td>true
+      <tr><th>                                       <td>false
+  </table>
+
+## DTR Support Table
+
+ <table>
+      <tr><th>LiDAR                                  <th>support_motor_dtr
+      <tr><th>S4/S4B/S2/X2/X2L/X4                    <td>true
+      <tr><th>TX8/TX20                               <td>true
+      <tr><th>G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7         <td>false
+      <tr><th>G1/G2A/G2C/R2/G2/G4B                   <td>false
+      <tr><th>TG15/TG30/TG50                         <td>false
+      <tr><th>T5/T15                                 <td>false
+  </table>

ros2/src/ydlidar_ros2_driver/launch/ydlidar.py

@@ -0,0 +1,29 @@
+from launch.exit_handler import ignore_exit_handler, restart_exit_handler
+from ros2run.api import get_executable_path
+
+
+def launch(launch_descriptor, argv):
+    ld = launch_descriptor
+    package = 'ydlidar_ros2_driver'
+    ld.add_process(
+        cmd=[get_executable_path(package_name=package, executable_name='ydlidar_ros2_driver_node')],
+        name='ydlidar_ros2_driver_node',
+        exit_handler=restart_exit_handler,
+    )
+    package = 'tf2_ros'
+    ld.add_process(
+        # The XYZ/Quat numbers for base_link -> laser_frame are taken from the
+        # turtlebot URDF in
+        # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
+        cmd=[
+            get_executable_path(
+                package_name=package, executable_name='static_transform_publisher'),
+            '0', '0', '0.02',
+            '0', '0', '0', '1',
+            'base_link',
+            'laser_frame'
+        ],
+        name='static_tf_pub_laser',
+        exit_handler=restart_exit_handler,
+    )
+    return ld

ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch.py

@@ -0,0 +1,56 @@
+#!/usr/bin/python3
+# Copyright 2020, EAIBOT
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.actions import LogInfo
+
+import lifecycle_msgs.msg
+import os
+
+
+def generate_launch_description():
+    share_dir = get_package_share_directory('ydlidar_ros2_driver')
+    parameter_file = LaunchConfiguration('params_file')
+    node_name = 'ydlidar_ros2_driver_node'
+
+    params_declare = DeclareLaunchArgument('params_file',
+                                           default_value=os.path.join(
+                                               share_dir, 'params', 'TminiPro.yaml'),
+                                           description='FPath to the ROS2 parameters file to use.')
+
+    driver_node = LifecycleNode(package='ydlidar_ros2_driver',
+                                executable='ydlidar_ros2_driver_node',
+                                name='ydlidar_ros2_driver_node',
+                                output='screen',
+                                emulate_tty=True,
+                                parameters=[parameter_file],
+                                namespace='/',
+                                )
+    # tf2_node = Node(package='tf2_ros',
+    #                 executable='static_transform_publisher',
+    #                 name='static_tf_pub_laser',
+    #                 arguments=['0', '0', '0.00','0', '0', '0', '1','base_link','laser_frame'],
+    #                 )
+
+    return LaunchDescription([
+        params_declare,
+        driver_node,
+        # tf2_node,
+    ])

ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python3
+# Copyright 2020, EAIBOTd
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.actions import LogInfo
+
+import lifecycle_msgs.msg
+import os
+
+
+def generate_launch_description():
+    share_dir = get_package_share_directory('ydlidar_ros2_driver')
+    rviz_config_file = os.path.join(share_dir, 'config','ydlidar.rviz')
+    parameter_file = LaunchConfiguration('params_file')
+    node_name = 'ydlidar_ros2_driver_node'
+
+    params_declare = DeclareLaunchArgument('params_file',
+                                           default_value=os.path.join(
+                                               share_dir, 'params', 'TminiPro.yaml'),
+                                           description='FPath to the ROS2 parameters file to use.')
+
+    driver_node = LifecycleNode(package='ydlidar_ros2_driver',
+                                executable='ydlidar_ros2_driver_node',
+                                name='ydlidar_ros2_driver_node',
+                                output='screen',
+                                emulate_tty=True,
+                                parameters=[parameter_file],
+                                namespace='/',
+                                )
+    tf2_node = Node(package='tf2_ros',
+                    executable='static_transform_publisher',
+                    name='static_tf_pub_laser',
+                    arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'],
+                    )
+    # rviz2_node = Node(package='rviz2',
+    #                 executable='rviz2',
+    #                 name='rviz2',
+    #                 arguments=['-d', rviz_config_file],
+    #                 )
+
+    return LaunchDescription([
+        params_declare,
+        driver_node,
+        tf2_node,
+        #rviz2_node,
+    ])

ros2/src/ydlidar_ros2_driver/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>ydlidar_ros2_driver</name>
+  <version>1.0.1</version>
+  <description>
+    The ROS2 device driver for YDLIDAR LIDARS
+  </description>
+  <maintainer email="support@ydlidar.com">Tony</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2/src/ydlidar_ros2_driver/params/G1.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 9
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 16.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/G2.yaml

@@ -0,0 +1,24 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 4
+    intensity_bit: 10
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: true
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 16.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/G6.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 512000
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 18
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 25.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/GS2.yaml

@@ -0,0 +1,24 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 921600
+    lidar_type: 3
+    device_type: 0
+    sample_rate: 9
+    intensity_bit: 0
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 1.0
+    range_min: 0.025
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/TEA.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: 192.168.0.11
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 8090
+    lidar_type: 0
+    device_type: 1
+    sample_rate: 20
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: true
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 50.0
+    range_min: 0.01
+    frequency: 20.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/TG.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 512000
+    lidar_type: 0
+    device_type: 0
+    sample_rate: 20
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 50.0
+    range_min: 0.01
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/TminiPro.yaml

@@ -0,0 +1,24 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttylzulaser
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 4
+    intensity_bit: 8
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: true
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 12.0
+    range_min: 0.10
+    frequency: 10.0
+    invalid_range_is_inf: true

ros2/src/ydlidar_ros2_driver/params/X2.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 115200
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 3
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: true
+    intensity: false
+    support_motor_dtr: true
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 12.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/X4-Pro.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 128000
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 5
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: true
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 12.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/X4.yaml

@@ -0,0 +1,23 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 128000
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 5
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: true
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 12.0
+    range_min: 0.1
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/params/ydlidar.yaml

@@ -0,0 +1,24 @@
+ydlidar_ros2_driver_node:
+  ros__parameters:
+    port: /dev/ttyUSB0
+    frame_id: laser_frame
+    ignore_array: ""
+    baudrate: 230400
+    lidar_type: 1
+    device_type: 0
+    sample_rate: 9
+    intensity_bit: 0
+    abnormal_check_count: 4
+    fixed_resolution: true
+    reversion: true
+    inverted: true
+    auto_reconnect: true
+    isSingleChannel: false
+    intensity: false
+    support_motor_dtr: false
+    angle_max: 180.0
+    angle_min: -180.0
+    range_max: 64.0
+    range_min: 0.01
+    frequency: 10.0
+    invalid_range_is_inf: false

ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp

@@ -0,0 +1,42 @@
+/*
+ *  YDLIDAR SYSTEM
+ *  YDLIDAR ROS 2 Node Client
+ *
+ *  Copyright 2017 - 2020 EAI TEAM
+ *  http://www.eaibot.com
+ *
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include <math.h>
+
+#define RAD2DEG(x) ((x)*180./M_PI)
+
+static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) {
+  int count = scan->scan_time / scan->time_increment;
+  printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count);
+  printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min),
+         RAD2DEG(scan->angle_max));
+
+  for (int i = 0; i < count; i++) {
+    float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
+    printf("[YDLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]);
+  }
+}
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_client");
+
+  auto lidar_info_sub = node->create_subscription<sensor_msgs::msg::LaserScan>(
+                        "scan", rclcpp::SensorDataQoS(), scanCb);
+
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
+
+
+  return 0;
+}

ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp

@@ -0,0 +1,266 @@
+/*
+ *  YDLIDAR SYSTEM
+ *  YDLIDAR ROS 2 Node
+ *
+ *  Copyright 2017 - 2020 EAI TEAM
+ *  http://www.eaibot.com
+ *
+ */
+
+#ifdef _MSC_VER
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#endif
+
+#include "src/CYdLidar.h"
+#include <math.h>
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time_source.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include <vector>
+#include <iostream>
+#include <string>
+#include <signal.h>
+#include <limits>
+
+#define ROS2Verision "1.0.1"
+
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node");
+
+  RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str());
+
+  CYdLidar laser;
+  std::string str_optvalue = "/dev/ydlidar";
+  node->declare_parameter("port", str_optvalue);
+  node->get_parameter("port", str_optvalue);
+  ///lidar port
+  laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size());
+
+  ///ignore array
+  str_optvalue = "";
+  node->declare_parameter("ignore_array", str_optvalue);
+  node->get_parameter("ignore_array", str_optvalue);
+  laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size());
+
+  std::string frame_id = "laser_frame";
+  node->declare_parameter("frame_id", frame_id);
+  node->get_parameter("frame_id", frame_id);
+
+  //////////////////////int property/////////////////
+  /// lidar baudrate
+  int optval = 230400;
+  node->declare_parameter("baudrate", optval);
+  node->get_parameter("baudrate", optval);
+  laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
+  /// tof lidar
+  optval = TYPE_TRIANGLE;
+  node->declare_parameter("lidar_type", optval);
+  node->get_parameter("lidar_type", optval);
+  laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
+  /// device type
+  optval = YDLIDAR_TYPE_SERIAL;
+  node->declare_parameter("device_type", optval);
+  node->get_parameter("device_type", optval);
+  laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
+  /// sample rate
+  optval = 9;
+  node->declare_parameter("sample_rate", optval);
+  node->get_parameter("sample_rate", optval);
+  laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
+  /// abnormal count
+  optval = 4;
+  node->declare_parameter("abnormal_check_count", optval);
+  node->get_parameter("abnormal_check_count", optval);
+  laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
+
+  /// Intenstiy bit count
+  optval = 8;
+  node->declare_parameter("intensity_bit", optval);
+  node->get_parameter("intensity_bit", optval);
+  laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
+     
+  //////////////////////bool property/////////////////
+  /// fixed angle resolution
+  bool b_optvalue = false;
+  node->declare_parameter("fixed_resolution", b_optvalue);
+  node->get_parameter("fixed_resolution", b_optvalue);
+  laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
+  /// rotate 180
+  b_optvalue = true;
+  node->declare_parameter("reversion", b_optvalue);
+  node->get_parameter("reversion", b_optvalue);
+  laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
+  /// Counterclockwise
+  b_optvalue = true;
+  node->declare_parameter("inverted", b_optvalue);
+  node->get_parameter("inverted", b_optvalue);
+  laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
+  b_optvalue = true;
+  node->declare_parameter("auto_reconnect", b_optvalue);
+  node->get_parameter("auto_reconnect", b_optvalue);
+  laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
+  /// one-way communication
+  b_optvalue = false;
+  node->declare_parameter("isSingleChannel", b_optvalue);
+  node->get_parameter("isSingleChannel", b_optvalue);
+  laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
+  /// intensity
+  b_optvalue = false;
+  node->declare_parameter("intensity", b_optvalue);
+  node->get_parameter("intensity", b_optvalue);
+  laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
+  /// Motor DTR
+  b_optvalue = false;
+  node->declare_parameter("support_motor_dtr", b_optvalue);
+  node->get_parameter("support_motor_dtr", b_optvalue);
+  laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
+
+  //////////////////////float property/////////////////
+  /// unit: °
+  float f_optvalue = 180.0f;
+  node->declare_parameter("angle_max", f_optvalue);
+  node->get_parameter("angle_max", f_optvalue);
+  laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
+  f_optvalue = -180.0f;
+  node->declare_parameter("angle_min", f_optvalue);
+  node->get_parameter("angle_min", f_optvalue);
+  laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
+  /// unit: m
+  f_optvalue = 64.f;
+  node->declare_parameter("range_max", f_optvalue);
+  node->get_parameter("range_max", f_optvalue);
+  laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
+  f_optvalue = 0.1f;
+  node->declare_parameter("range_min", f_optvalue);
+  node->get_parameter("range_min", f_optvalue);
+  laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
+  /// unit: Hz
+  f_optvalue = 10.f;
+  node->declare_parameter("frequency", f_optvalue);
+  node->get_parameter("frequency", f_optvalue);
+  laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));
+
+  bool invalid_range_is_inf = false;
+  node->declare_parameter("invalid_range_is_inf", invalid_range_is_inf);
+  node->get_parameter("invalid_range_is_inf", invalid_range_is_inf);
+
+
+  bool ret = laser.initialize();
+  if (ret) {
+    ret = laser.turnOn();
+  } else {
+    RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError());
+  }
+  
+  auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS());
+  auto pc_pub = node->create_publisher<sensor_msgs::msg::PointCloud>("point_cloud", rclcpp::SensorDataQoS());
+  
+  auto stop_scan_service =
+    [&laser](const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+  std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
+  {
+    return laser.turnOff();
+  };
+
+  auto stop_service = node->create_service<std_srvs::srv::Empty>("stop_scan",stop_scan_service);
+
+  auto start_scan_service =
+    [&laser](const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+  std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
+  {
+    return laser.turnOn();
+  };
+
+  auto start_service = node->create_service<std_srvs::srv::Empty>("start_scan",start_scan_service);
+
+  rclcpp::WallRate loop_rate(20);
+
+  while (ret && rclcpp::ok()) {
+
+    LaserScan scan;//
+
+    if (laser.doProcessSimple(scan)) {
+
+      auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
+      auto pc_msg = std::make_shared<sensor_msgs::msg::PointCloud>();
+      scan_msg->header.stamp = node->now();//this->now();
+      //scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp);
+      //scan_msg->header.stamp.nanosec =  scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec);
+      scan_msg->header.frame_id = frame_id;
+      pc_msg->header = scan_msg->header;
+      scan_msg->angle_min = scan.config.min_angle;
+      scan_msg->angle_max = scan.config.max_angle;
+      scan_msg->angle_increment = scan.config.angle_increment;
+      scan_msg->scan_time = scan.config.scan_time;
+      scan_msg->time_increment = scan.config.time_increment;
+      scan_msg->range_min = scan.config.min_range;
+      scan_msg->range_max = scan.config.max_range;
+      
+      int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
+      const float invalid_range_value =
+        invalid_range_is_inf ? std::numeric_limits<float>::infinity() : 0.0f;
+      scan_msg->ranges.assign(size, invalid_range_value);
+      scan_msg->intensities.assign(size, 0.0f);
+
+      pc_msg->channels.resize(2);
+      int idx_intensity = 0;
+      pc_msg->channels[idx_intensity].name = "intensities";
+      int idx_timestamp = 1;
+      pc_msg->channels[idx_timestamp].name = "stamps";
+
+      for(size_t i=0; i < scan.points.size(); i++) {
+        int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
+        if(index >=0 && index < size) {
+	  if (scan.points[i].range >= scan.config.min_range) {
+            scan_msg->ranges[index] = scan.points[i].range;
+            scan_msg->intensities[index] = scan.points[i].intensity;
+	  }
+        }
+
+	if (scan.points[i].range >= scan.config.min_range &&
+             scan.points[i].range <= scan.config.max_range) {
+          geometry_msgs::msg::Point32 point;
+          point.x = scan.points[i].range * cos(scan.points[i].angle);
+          point.y = scan.points[i].range * sin(scan.points[i].angle);
+          point.z = 0.0;
+          pc_msg->points.push_back(point);
+          pc_msg->channels[idx_intensity].values.push_back(scan.points[i].intensity);
+          pc_msg->channels[idx_timestamp].values.push_back(i * scan.config.time_increment);
+        }
+
+      }
+
+      laser_pub->publish(*scan_msg);
+      pc_pub->publish(*pc_msg);
+
+    } else {
+      RCLCPP_ERROR(node->get_logger(), "Failed to get scan");
+    }
+    if(!rclcpp::ok()) {
+      break;
+    }
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+
+
+  RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping .......");
+  laser.turnOff();
+  laser.disconnecting();
+  rclcpp::shutdown();
+
+  return 0;
+}

ros2/src/ydlidar_ros2_driver/startup/initenv.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout",  SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules
+
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout",  SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules
+
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout",  SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules
+
+service udev reload
+sleep 2
+service udev restart
+

tools/.udp_control_state.json

@@ -1,8 +0,0 @@
-{
-  "height": -280,
-  "j2": -6,
-  "j3": 122,
-  "j4": -80,
-  "j5": -100,
-  "j6": 0
-}

tools/README.md

@@ -1,43 +1,327 @@
-# tools — 调试 & 测试工具
+# CRAIC 工具脚本
 
-## 文件
+独立的命令行工具，用于机械臂控制、相机采集和坐标变换。
 
-| 文件 | 说明 |
+## 工具列表
-|------|------|
-| `udp_server.py` | UDP 回显服务器，监听 `0.0.0.0:8888`，用于测试 UDP 通讯协议 |
 
-## 使用
+### 1. udp_control.py
+机械臂命令行控制工具（带完整逆运动学）。
 
-### UDP 回显服务器
+**功能**：
-
+- 关节空间运动控制
-启动服务器，接收来自 ESP32 或 ROS 2 键盘遥控节点的 UDP 指令并打印：
+- 笛卡尔空间运动控制（自动逆运动学）
+- 轨迹插值（平滑运动）
+- 状态缓存（连续运动）
 
+**使用**：
 ```bash
-python tools/udp_server.py
+# 关节空间控制
+python udp_control.py joints \
+    --height -100 --j2 10 --j3 20 --j4 30 \
+    --duration 2.0 --rate 20
+
+# 笛卡尔空间控制
+python udp_control.py pose \
+    --x 200 --y 100 --z -100 --phi 45 \
+    --duration 2.0
+
+# 抓取动作
+python udp_control.py pose \
+    --x 200 --y 100 --z -150 --phi 45 \
+    --grip --duration 1.0
+
+# 释放动作
+python udp_control.py pose \
+    --x 200 --y 100 --z -50 --phi 45 \
+    --release --duration 1.0
+
+# 干运行（查看命令但不发送）
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --dry-run
+
+# 显示正运动学验证
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --show-fk
 ```
 
-发送测试指令验证协议格式：
+**参数**：
+- `--ip` - ESP32 IP 地址（默认 192.168.4.1）
+- `--port` - UDP 端口（默认 8888）
+- `--duration` - 运动时长（秒，默认 1.0）
+- `--rate` - 插值频率（Hz，默认 20.0）
+- `--elbow-up` - 肘部朝上解（默认朝下）
+- `--no-state-cache` - 禁用状态缓存
 
+### 2. camera_to_base.py
+相机坐标系到机械臂基坐标系的变换工具。
+
+**功能**：
+- 完整的坐标变换链：相机 → TCP → 基坐标系
+- 支持相机安装偏移和旋转
+- 水平安装相机的特化实现
+
+**使用**：
 ```bash
-# 底盘指令
+# 基本变换（相机在 TCP 中心）
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0
+
+# 考虑相机偏移（相机不在 TCP 中心）
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0 \
+    --cam-offset 0 10 50 \
+    --cam-rotation 0 15 0
+```
+
+**参数**：
+- `--camera X Y Z` - 相机坐标系坐标
+- `--tcp X Y Z` - 当前 TCP 位置
+- `--phi` - 当前 TCP 偏航角（度）
+- `--cam-offset TX TY TZ` - 相机到 TCP 的平移
+- `--cam-rotation ROLL PITCH YAW` - 相机到 TCP 的旋转（度）
+
+### 3. camera_capture.py
+ESP32 MJPEG 流采集工具。
+
+**功能**：
+- 自动扫描局域网中的 ESP32 相机
+- 解析 MJPEG 流并提取单帧
+- 保存 JPEG 图像
+
+**使用**：
+```bash
+# 指定 IP
+python camera_capture.py --ip 192.168.4.1
+
+# 自动扫描
+python camera_capture.py --scan
+
+# 指定输出文件
+python camera_capture.py --ip 192.168.4.1 --output frame.jpg
+```
+
+### 4. udp_server.py
+UDP 回显测试服务器。
+
+**功能**：
+- 监听 UDP 端口
+- 打印接收到的所有消息
+- 用于调试 UDP 通信
+
+**使用**：
+```bash
+python udp_server.py
+
+# 指定端口
+python udp_server.py --port 9999
+```
+
+**测试**：
+```bash
+# 终端 1: 启动服务器
+python udp_server.py
+
+# 终端 2: 发送测试
 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
 ```
 
-### 协议测试流程
+## 坐标系说明
 
-1. 启动 `python tools/udp_server.py`
+### 机械臂基坐标系
-2. 修改 ROS 2 节点参数指向本机：`udp_ip:=127.0.0.1`
+
-3. 运行 ROS 2 键盘遥控节点，观察服务器收到的指令
+```
-4. 确认协议格式正确后，将目标 IP 改为实际设备地址
+    Z (上)
+    |
+    |
+    o----→ X (前)
+   /
+  /
+ Y (左)
+```
+
+- 原点：J1 滑轨底部
+- 单位：mm
+
+### 相机坐标系（水平安装）
+
+```
+       Y (下)
+        |
+        |
+        o----→ Z (前，光轴)
+       /
+      /
+    X (右)
+```
+
+- 原点：相机光心
+- 单位：mm
+
+### 图像坐标系
+
+```
+o----→ u (列，右)
+|
+|
+↓
+v (行，下)
+```
+
+- 原点：图像左上角
+- 单位：像素
+
+## 完整工作流示例
+
+### 1. 测试机械臂连接
 
 ```bash
-# 示例：ROS 2 键盘遥控 → 本地回显
+# 1. 启动回显服务器
-ros2 run udp_teleop keyboard_control \
+python udp_server.py
-    --ros-args -p udp_ip:=127.0.0.1 -p udp_port:=8888
+
+# 2. 测试 UDP（新终端）
+echo 'JXB:0:0:0:0:81:30:0:0:EZHY' | nc -u 127.0.0.1 8888
+
+# 3. 验证服务器收到消息
+```
+
+### 2. 机械臂运动测试
+
+```bash
+# 测试关节运动
+python udp_control.py joints --height -50 --j2 10 --j3 20 --j4 30 --duration 2.0
+
+# 测试笛卡尔运动
+python udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0
+
+# 测试抓取
+python udp_control.py pose --x 200 --y 100 --z -150 --phi 45 --grip --duration 1.0
+```
+
+### 3. 视觉抓取流程
+
+```bash
+# 步骤 1: 采集图像
+python camera_capture.py --ip 192.168.4.1 --output target.jpg
+
+# 步骤 2: 检测物体（假设得到像素坐标和宽度）
+# u=320, v=240, pixel_width=50
+# 使用相似三角形计算深度：depth = (real_width * focal_length) / pixel_width
+# 转换到相机坐标：x_cam, y_cam, z_cam
+
+# 步骤 3: 获取当前 TCP 位姿（从 ROS 或状态文件）
+# tcp_x=200, tcp_y=0, tcp_z=-120, phi=0
+
+# 步骤 4: 坐标变换
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0
+
+# 步骤 5: 移动到目标
+python udp_control.py pose --x 323.5 --y 229.6 --z -108.6 --phi 0 --duration 3.0
+
+# 步骤 6: 抓取
+python udp_control.py pose --x 323.5 --y 229.6 --z -158.6 --phi 0 --grip --duration 1.0
+
+# 步骤 7: 回收
+python udp_control.py pose --x 200 --y 0 --z -158.6 --phi 0 --duration 2.0
+```
+
+## 状态缓存
+
+`udp_control.py` 会将最后发送的关节状态保存到：
+```
+~/.ros/udp_control_state.json
+```
+
+这允许连续运动时从上一个位置平滑插值，而不是从原点跳跃。
+
+**禁用缓存**：
+```bash
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --no-state-cache
+```
+
+## UDP 协议
+
+所有工具通过 UDP 端口 8888 与 ESP32 通信。
+
+**机械臂命令格式**：
+```
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+```
+
+- 所有角度已包含零点偏移
+- 高度单位：mm
+- 角度单位：度
+
+**底盘命令格式**：
+```
+XYW:<X>:<Y>:<W>:XZHY\n
+```
+
+## 依赖
+
+```bash
+pip install numpy
+```
+
+可选（用于相机采集）：
+```bash
+pip install opencv-python requests
+```
+
+## 故障排查
+
+### 1. 无法连接 ESP32
+
+**检查**：
+```bash
+# 测试网络连接
+ping 192.168.4.1
+
+# 测试 UDP 端口
+echo 'test' | nc -u 192.168.4.1 8888
+```
+
+### 2. 逆运动学失败
+
+**错误**：`目标超出工作空间`
+
+**原因**：目标位置超出机械臂可达范围
+
+**解决**：
+- 检查目标是否在圆环内（半径 110-360mm）
+- 检查高度是否在范围内（-370 到 -80mm）
+
+### 3. 运动不平滑
+
+**解决**：
+- 增加 `--duration`（运动时间）
+- 增加 `--rate`（插值频率）
+- 确保状态缓存已启用
+
+### 4. 关节角度超限
+
+**错误**：`J2=-150 超出范围 [-110, 115]`
+
+**解决**：
+- 检查目标位姿是否合理
+- 尝试 `--elbow-up` 切换肘部解
+
+## 相关文档
+
+- **运动学推导**：`docs/arm.md` - 完整数学推导
+- **ROS 节点**：`ros2/README.md` - ROS 2 集成
+- **视觉标定**：`docs/vision_calibration_horizontal.md` - 相机标定
+
+## 开发
+
+这些工具是独立的 Python 脚本，不依赖 ROS。可以直接在任何 Python 3 环境中运行。
+
+**测试**：
+```bash
+# 测试逆运动学
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --dry-run --show-fk
+
+# 测试坐标变换
+python camera_to_base.py --camera 0 0 300 --tcp 200 0 -120 --phi 0
 ```

tools/camera_to_base.py

@@ -0,0 +1,329 @@
+#!/usr/bin/env python3
+"""相机坐标系到机械臂基坐标系的直接变换工具。
+
+使用场景：
+1. 已经通过其他方式获得了相机坐标系坐标
+2. 测试和验证坐标变换算法
+3. 调试机械臂控制逻辑
+
+输入：
+    - 相机坐标系坐标 (xc, yc, zc)，单位 mm
+    - 当前 TCP 位姿 (x, y, z, phi)
+
+输出：
+    - 基坐标系坐标 (xb, yb, zb)，单位 mm
+"""
+
+import argparse
+import math
+import sys
+import json
+from pathlib import Path
+from typing import Tuple
+
+import numpy as np
+
+
+def euler_to_rotation_matrix(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
+    """欧拉角转旋转矩阵（ZYX顺序）"""
+    roll = math.radians(roll_deg)
+    pitch = math.radians(pitch_deg)
+    yaw = math.radians(yaw_deg)
+
+    Rx = np.array([
+        [1, 0, 0],
+        [0, math.cos(roll), -math.sin(roll)],
+        [0, math.sin(roll), math.cos(roll)]
+    ])
+
+    Ry = np.array([
+        [math.cos(pitch), 0, math.sin(pitch)],
+        [0, 1, 0],
+        [-math.sin(pitch), 0, math.cos(pitch)]
+    ])
+
+    Rz = np.array([
+        [math.cos(yaw), -math.sin(yaw), 0],
+        [math.sin(yaw), math.cos(yaw), 0],
+        [0, 0, 1]
+    ])
+
+    return Rz @ Ry @ Rx
+
+
+def camera_to_tcp(
+    xc: float, yc: float, zc: float,
+    tx: float = 0.0, ty: float = 0.0, tz: float = 0.0,
+    roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0
+) -> Tuple[float, float, float]:
+    """相机坐标系 → TCP 坐标系
+
+    Args:
+        xc, yc, zc: 相机坐标系坐标 (mm)
+        tx, ty, tz: 相机到 TCP 的平移 (mm)
+        roll, pitch, yaw: 相机到 TCP 的旋转 (度)
+
+    Returns:
+        (xt, yt, zt): TCP 坐标系坐标 (mm)
+    """
+    R = euler_to_rotation_matrix(roll, pitch, yaw)
+    T = np.array([tx, ty, tz])
+    P_cam = np.array([xc, yc, zc])
+    P_tcp = R @ P_cam + T
+    return float(P_tcp[0]), float(P_tcp[1]), float(P_tcp[2])
+
+
+def tcp_to_base(
+    xt: float, yt: float, zt: float,
+    tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float
+) -> Tuple[float, float, float]:
+    """TCP 坐标系 → 机械臂基坐标系（水平相机版本）
+
+    坐标映射：
+        - TCP X (相机右) → 基坐标系 垂直于 phi 方向
+        - TCP Y (相机下) → 基坐标系 -Z 方向（向下）
+        - TCP Z (相机前) → 基坐标系 phi 方向
+
+    Args:
+        xt, yt, zt: TCP 坐标系坐标 (mm)
+        tcp_x, tcp_y, tcp_z: TCP 当前位置 (mm)
+        tcp_phi_deg: TCP 当前朝向角 (度)
+
+    Returns:
+        (xb, yb, zb): 基坐标系坐标 (mm)
+    """
+    phi = math.radians(tcp_phi_deg)
+
+    # 旋转矩阵：TCP → 基坐标系
+    R_tcp_to_base = np.array([
+        [-math.sin(phi), 0, math.cos(phi)],
+        [ math.cos(phi), 0, math.sin(phi)],
+        [0,             -1, 0]
+    ])
+
+    P_tcp = np.array([xt, yt, zt])
+    P_base_relative = R_tcp_to_base @ P_tcp
+
+    P_base = P_base_relative + np.array([tcp_x, tcp_y, tcp_z])
+
+    return float(P_base[0]), float(P_base[1]), float(P_base[2])
+
+
+def camera_to_base(
+    xc: float, yc: float, zc: float,
+    tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float,
+    cam_tx: float = 0.0, cam_ty: float = 0.0, cam_tz: float = 0.0,
+    cam_roll: float = 0.0, cam_pitch: float = 0.0, cam_yaw: float = 0.0
+) -> Tuple[float, float, float]:
+    """完整变换：相机坐标系 → 基坐标系
+
+    Args:
+        xc, yc, zc: 相机坐标系坐标 (mm)
+        tcp_x, tcp_y, tcp_z, tcp_phi_deg: TCP 当前位姿
+        cam_tx, cam_ty, cam_tz: 相机到 TCP 的平移
+        cam_roll, cam_pitch, cam_yaw: 相机到 TCP 的旋转
+
+    Returns:
+        (xb, yb, zb): 基坐标系坐标 (mm)
+    """
+    # 步骤 1: 相机 → TCP
+    xt, yt, zt = camera_to_tcp(xc, yc, zc, cam_tx, cam_ty, cam_tz,
+                                cam_roll, cam_pitch, cam_yaw)
+
+    # 步骤 2: TCP → 基坐标系
+    xb, yb, zb = tcp_to_base(xt, yt, zt, tcp_x, tcp_y, tcp_z, tcp_phi_deg)
+
+    return xb, yb, zb
+
+
+def load_tcp_pose_from_state() -> Tuple[float, float, float, float]:
+    """从状态缓存文件读取当前 TCP 位姿"""
+    state_file = Path("tools/.udp_control_state.json")
+
+    if not state_file.exists():
+        raise FileNotFoundError(
+            f"状态文件不存在: {state_file}\n"
+            "请先运行一次 udp_control.py 以初始化状态"
+        )
+
+    state = json.loads(state_file.read_text())
+
+    # 导入必要的模块计算正运动学
+    sys.path.insert(0, str(Path(__file__).parent))
+    from udp_control import (
+        forward_kinematics,
+        ArmGeometry,
+        ArmMathState,
+        DEFAULT_L1, DEFAULT_L2, DEFAULT_X4, DEFAULT_Z4,
+        DEFAULT_ZERO_J2, DEFAULT_ZERO_J3, DEFAULT_ZERO_J4
+    )
+
+    geometry = ArmGeometry(l1=DEFAULT_L1, l2=DEFAULT_L2, x4=DEFAULT_X4, z4=DEFAULT_Z4)
+    math_state = ArmMathState(
+        d1=state["height"],
+        theta2_deg=state["j2"] - DEFAULT_ZERO_J2,
+        theta3_deg=state["j3"] - DEFAULT_ZERO_J3,
+        theta4_deg=state["j4"] - DEFAULT_ZERO_J4,
+    )
+
+    pose = forward_kinematics(geometry, math_state)
+    return pose.x, pose.y, pose.z, pose.phi_deg
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(
+        description="相机坐标系到机械臂基坐标系的直接变换",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog="""
+示例用法:
+
+  1. 基本用法（手动指定 TCP 位姿）:
+     python %(prog)s --xc 10 --yc 5 --zc 250 \\
+         --tcp-x 150 --tcp-y 50 --tcp-z -100 --tcp-phi 45
+
+  2. 自动读取 TCP 位姿（从状态缓存）:
+     python %(prog)s --xc 10 --yc 5 --zc 250 --auto-tcp
+
+  3. 包含相机到 TCP 的变换:
+     python %(prog)s --xc 10 --yc 5 --zc 250 --auto-tcp \\
+         --cam-tx 20 --cam-ty 10 --cam-tz 5
+
+相机坐标系定义（水平安装）:
+  Xc: 向右
+  Yc: 向下
+  Zc: 向前（水平，光轴方向）
+
+机械臂基坐标系定义:
+  X, Y: 水平面
+  Z: 向上
+        """
+    )
+
+    # 相机坐标系输入
+    parser.add_argument("--xc", type=float, required=True,
+                       help="相机坐标系 X 坐标（向右，mm）")
+    parser.add_argument("--yc", type=float, required=True,
+                       help="相机坐标系 Y 坐标（向下，mm）")
+    parser.add_argument("--zc", type=float, required=True,
+                       help="相机坐标系 Z 坐标（向前，mm）")
+
+    # TCP 位姿
+    tcp_group = parser.add_mutually_exclusive_group(required=True)
+    tcp_group.add_argument("--auto-tcp", action="store_true",
+                          help="自动从状态缓存读取当前 TCP 位姿")
+    tcp_group.add_argument("--tcp-manual", action="store_true",
+                          help="手动指定 TCP 位姿（需要 --tcp-x/y/z/phi）")
+
+    parser.add_argument("--tcp-x", type=float,
+                       help="TCP X 坐标（mm）")
+    parser.add_argument("--tcp-y", type=float,
+                       help="TCP Y 坐标（mm）")
+    parser.add_argument("--tcp-z", type=float,
+                       help="TCP Z 坐标（mm）")
+    parser.add_argument("--tcp-phi", type=float,
+                       help="TCP 朝向角（度）")
+
+    # 相机到 TCP 的变换（可选）
+    parser.add_argument("--cam-tx", type=float, default=0.0,
+                       help="相机到 TCP 平移 X（mm，默认 0）")
+    parser.add_argument("--cam-ty", type=float, default=0.0,
+                       help="相机到 TCP 平移 Y（mm，默认 0）")
+    parser.add_argument("--cam-tz", type=float, default=0.0,
+                       help="相机到 TCP 平移 Z（mm，默认 0）")
+    parser.add_argument("--cam-roll", type=float, default=0.0,
+                       help="相机到 TCP 旋转 roll（度，默认 0）")
+    parser.add_argument("--cam-pitch", type=float, default=0.0,
+                       help="相机到 TCP 旋转 pitch（度，默认 0）")
+    parser.add_argument("--cam-yaw", type=float, default=0.0,
+                       help="相机到 TCP 旋转 yaw（度，默认 0）")
+
+    # 输出格式
+    parser.add_argument("--json", action="store_true",
+                       help="以 JSON 格式输出")
+    parser.add_argument("--quiet", action="store_true",
+                       help="只输出坐标，不输出说明信息")
+
+    args = parser.parse_args()
+
+    # 获取 TCP 位姿
+    if args.auto_tcp:
+        try:
+            tcp_x, tcp_y, tcp_z, tcp_phi = load_tcp_pose_from_state()
+            if not args.quiet:
+                print(f"从状态缓存读取 TCP 位姿: "
+                      f"({tcp_x:.3f}, {tcp_y:.3f}, {tcp_z:.3f}), phi={tcp_phi:.3f}°")
+        except Exception as e:
+            print(f"错误: 无法读取 TCP 位姿: {e}", file=sys.stderr)
+            return 1
+    else:
+        if not all([args.tcp_x is not None, args.tcp_y is not None,
+                   args.tcp_z is not None, args.tcp_phi is not None]):
+            print("错误: 使用 --tcp-manual 时必须指定 --tcp-x, --tcp-y, --tcp-z, --tcp-phi",
+                  file=sys.stderr)
+            return 1
+        tcp_x, tcp_y, tcp_z, tcp_phi = args.tcp_x, args.tcp_y, args.tcp_z, args.tcp_phi
+
+    # 执行变换
+    try:
+        xb, yb, zb = camera_to_base(
+            args.xc, args.yc, args.zc,
+            tcp_x, tcp_y, tcp_z, tcp_phi,
+            args.cam_tx, args.cam_ty, args.cam_tz,
+            args.cam_roll, args.cam_pitch, args.cam_yaw
+        )
+
+        # 输出结果
+        if args.json:
+            result = {
+                "input": {
+                    "camera": {"x": args.xc, "y": args.yc, "z": args.zc},
+                    "tcp": {"x": tcp_x, "y": tcp_y, "z": tcp_z, "phi": tcp_phi}
+                },
+                "output": {
+                    "base": {"x": xb, "y": yb, "z": zb}
+                }
+            }
+            print(json.dumps(result, indent=2))
+        elif args.quiet:
+            print(f"{xb:.3f} {yb:.3f} {zb:.3f}")
+        else:
+            print("=" * 70)
+            print("相机坐标系到基坐标系变换")
+            print("=" * 70)
+            print(f"\n输入（相机坐标系）:")
+            print(f"  X_cam = {args.xc:8.3f} mm  (向右)")
+            print(f"  Y_cam = {args.yc:8.3f} mm  (向下)")
+            print(f"  Z_cam = {args.zc:8.3f} mm  (向前，光轴)")
+
+            print(f"\nTCP 位姿:")
+            print(f"  X_tcp = {tcp_x:8.3f} mm")
+            print(f"  Y_tcp = {tcp_y:8.3f} mm")
+            print(f"  Z_tcp = {tcp_z:8.3f} mm")
+            print(f"  φ_tcp = {tcp_phi:8.3f} °")
+
+            if any([args.cam_tx, args.cam_ty, args.cam_tz,
+                   args.cam_roll, args.cam_pitch, args.cam_yaw]):
+                print(f"\n相机到 TCP 变换:")
+                print(f"  平移: ({args.cam_tx:.3f}, {args.cam_ty:.3f}, {args.cam_tz:.3f}) mm")
+                print(f"  旋转: roll={args.cam_roll:.3f}°, pitch={args.cam_pitch:.3f}°, yaw={args.cam_yaw:.3f}°")
+
+            print(f"\n输出（机械臂基坐标系）:")
+            print(f"  X_base = {xb:8.3f} mm")
+            print(f"  Y_base = {yb:8.3f} mm")
+            print(f"  Z_base = {zb:8.3f} mm")
+            print("=" * 70)
+
+            print(f"\n使用此坐标控制机械臂:")
+            print(f"  python tools/udp_control.py pose \\")
+            print(f"    --x {xb:.1f} --y {yb:.1f} --z {zb:.1f} \\")
+            print(f"    --phi {tcp_phi:.1f}")
+
+        return 0
+
+    except Exception as e:
+        print(f"错误: {e}", file=sys.stderr)
+        return 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/udp_control.py

@@ -45,7 +45,7 @@ DEFAULT_FIXED_J5 = J5_OPEN
 # ==== 抓取/释放（由 J6 控制）====
 # 请填写实际角度值：
 GRIP_ANGLE = -5      # TODO: 填写抓取时 J6 的角度
-RELEASE_ANGLE = 30   # TODO: 填写释放时 J6 的角度
+RELEASE_ANGLE = 80   # TODO: 填写释放时 J6 的角度
 DEFAULT_FIXED_J6 = RELEASE_ANGLE
 DEFAULT_ZERO_J2 = 3
 DEFAULT_ZERO_J3 = 7