From 74396f48a0c7c16c8b4252e3372a0466e68c793e Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Sat, 20 Jun 2026 14:13:54 +0800 Subject: [PATCH] fix(craic_localization): tune AMCL params and propagate lidar args across launch files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 --- docs/localization.md | 5 ++- ros2/AGENTS.md | 39 +++++++++++++++++++ ros2/src/craic_localization/config/amcl.yaml | 10 ++--- .../launch/bringup.launch.py | 23 +++++++++++ .../craic_localization/launch/lidar.launch.py | 10 ++--- .../launch/localization.launch.py | 31 ++++++++++++++- .../launch/mapping.launch.py | 31 ++++++++++++++- .../craic_localization/rviz/localization.rviz | 2 +- 8 files changed, 137 insertions(+), 14 deletions(-) create mode 100644 ros2/AGENTS.md diff --git a/docs/localization.md b/docs/localization.md index ec122fe..6b57f38 100644 --- a/docs/localization.md +++ b/docs/localization.md @@ -156,7 +156,7 @@ ros2 topic echo /amcl_pose # 位姿 + 协方差 ### 6.5 示教记录点位 ```bash # 终端1：定位栈(6.3) 终端2：遥控(6.6) 终端3：示教 -q +ros2 run craic_localization teach_points --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml ``` 交互命令：**回车/`r`** 记录当前预设点并前进；`p` 查看当前位姿；`name ` 记自定义点； `del ` 删除；`list` 列出；`skip`/`back` 跳过/回退；`save` 存盘；`q` 退出。 @@ -233,6 +233,9 @@ ros2 run udp_teleop keyboard_control --ros-args \ | 时间戳显示 ~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 树连通）。 diff --git a/ros2/AGENTS.md b/ros2/AGENTS.md new file mode 100644 index 0000000..27cbe3d --- /dev/null +++ b/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//` 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. diff --git a/ros2/src/craic_localization/config/amcl.yaml b/ros2/src/craic_localization/config/amcl.yaml index caae981..45d1a97 100644 --- a/ros2/src/craic_localization/config/amcl.yaml +++ b/ros2/src/craic_localization/config/amcl.yaml @@ -34,13 +34,13 @@ amcl: # ===== 激光模型（匹配 5×5m 边界）===== laser_model_type: "likelihood_field" - laser_max_range: 5.0 + laser_max_range: 8.0 # 4×4m 场地对角 ~5.7m，留余量，避免远墙光束被裁掉 laser_min_range: -1.0 max_beams: 500 - sigma_hit: 0.05 - z_hit: 0.5 - z_rand: 0.03 - laser_likelihood_max_dist: 1.5 + 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 diff --git a/ros2/src/craic_localization/launch/bringup.launch.py b/ros2/src/craic_localization/launch/bringup.launch.py index 24e8391..53982ad 100644 --- a/ros2/src/craic_localization/launch/bringup.launch.py +++ b/ros2/src/craic_localization/launch/bringup.launch.py @@ -30,11 +30,25 @@ def generate_launch_description(): 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( @@ -48,6 +62,15 @@ def generate_launch_description(): # 激光雷达 + 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(), ), # 可视化（标定 / 联调） diff --git a/ros2/src/craic_localization/launch/lidar.launch.py b/ros2/src/craic_localization/launch/lidar.launch.py index ea8a6d9..5c2ea12 100644 --- a/ros2/src/craic_localization/launch/lidar.launch.py +++ b/ros2/src/craic_localization/launch/lidar.launch.py @@ -4,12 +4,12 @@ YDLiDAR 驱动节点（ydlidar_ros2_driver_node）内部是普通 rclcpp::Node，启动即在 `scan` 话题持续发布 sensor_msgs/LaserScan，无需 lifecycle configure/activate 转换。 -静态外参为**占位值**（绕 z 轴 -90°，对齐 move_try；平移取 0），需在 P3 按 CRAIC 上 -激光雷达的实际安装位置标定。可通过 launch 参数 lidar_x/lidar_y/lidar_z/lidar_yaw 覆盖。 +静态外参默认值仅作 CRAIC 当前安装的初始猜测，需按实车标定。若 scan 形状对、但在地图里整体 +平移或轻微转角对不齐，优先调 `lidar_x/lidar_y/lidar_yaw`。可通过 launch 参数覆盖。 用法： ros2 launch craic_localization lidar.launch.py - ros2 launch craic_localization lidar.launch.py params_file:=/abs/path/lidar.yaml lidar_yaw:=-1.5707963 + ros2 launch craic_localization lidar.launch.py lidar_x:=0.03 lidar_y:=-0.01 lidar_yaw:=-3.14159 """ import os @@ -44,10 +44,10 @@ def generate_launch_description(): description='YDLiDAR 参数文件路径'), DeclareLaunchArgument('base_frame', default_value='base_footprint'), DeclareLaunchArgument('laser_frame', default_value='laser_frame'), - # 占位外参，P3 标定后覆盖 + # 外参默认值；按实车标定后可在 launch 命令中覆盖。 DeclareLaunchArgument('lidar_x', default_value='0.0'), DeclareLaunchArgument('lidar_y', default_value='0.0'), - DeclareLaunchArgument('lidar_z', 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'), diff --git a/ros2/src/craic_localization/launch/localization.launch.py b/ros2/src/craic_localization/launch/localization.launch.py index f4547d5..ca7c42f 100644 --- a/ros2/src/craic_localization/launch/localization.launch.py +++ b/ros2/src/craic_localization/launch/localization.launch.py @@ -40,6 +40,13 @@ def generate_launch_description(): 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'), @@ -52,9 +59,31 @@ def generate_launch_description(): 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)), + 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', diff --git a/ros2/src/craic_localization/launch/mapping.launch.py b/ros2/src/craic_localization/launch/mapping.launch.py index 759928c..31bf70d 100644 --- a/ros2/src/craic_localization/launch/mapping.launch.py +++ b/ros2/src/craic_localization/launch/mapping.launch.py @@ -29,12 +29,41 @@ def generate_launch_description(): 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)), + 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( diff --git a/ros2/src/craic_localization/rviz/localization.rviz b/ros2/src/craic_localization/rviz/localization.rviz index ee92fe8..ae7115e 100644 --- a/ros2/src/craic_localization/rviz/localization.rviz +++ b/ros2/src/craic_localization/rviz/localization.rviz @@ -100,7 +100,7 @@ Visualization Manager: Namespaces: {} Value: true Global Options: - Fixed Frame: odom + Fixed Frame: map Background Color: 48; 48; 48 Frame Rate: 30 Tools: