fix(craic_localization): tune AMCL params and propagate lidar args across launch files

- 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
2026-06-20 14:13:54 +08:00
parent 13f26e2453
commit 74396f48a0
8 changed files with 137 additions and 14 deletions
--- 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 <X>` 记自定义点；
 `del <X>` 删除；`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 树连通）。
--- a/ros2/AGENTS.md
+++ 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/<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.
--- 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
--- 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(),
        ),

        # 可视化（标定 / 联调）
--- 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'),
--- 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',
--- 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(
--- 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: