建图模式
mapping_localization.sh
source_with_nounset_guard /opt/ros/humble/setup.bash
reset_rtabmap_database_on_start "$RTABMAP_DATABASE_PATH"
echo ""
echo "============================================"
echo " RTABMAP建图 + Nav2导航"
echo "============================================"
echo ""
echo "RTAB-Map 数据库: $RTABMAP_DATABASE_PATH"
echo "启动前删除数据库: $RTABMAP_DELETE_DB_ON_START"
echo "RViz 配置: $MAP_RVIZ_CONFIG"
echo "机器人模型配置: $ROBOT_DESCRIPTION_CONFIG_FILE"
echo "Nav2 参数文件: $NAV2_PARAMS_FILE"
echo "Nav2 启动入口: $NAV2_LAUNCH_PACKAGE/$NAV2_LAUNCH_FILE"
echo "启用 robot_localization: $ENABLE_ROBOT_LOCALIZATION"
if [ "$ENABLE_ROBOT_LOCALIZATION" = "1" ]; then
echo "robot_localization 参数文件: $ROBOT_LOCALIZATION_PARAMS_FILE"
echo "视觉里程计话题: $RTABMAP_RGBD_ODOM_TOPIC"
echo "激光里程计话题: $RTABMAP_LASER_ODOM_TOPIC"
echo "融合里程计话题: $FUSED_ODOM_TOPIC"
fi
echo "启用 BlueSea 雷达: $ENABLE_BLUESEA_LIDAR"
if [ "$ENABLE_BLUESEA_LIDAR" = "1" ]; then
echo "BlueSea 双雷达 launch: $BLUESEA_LIDAR_LAUNCH_FILE"
echo "左雷达参数文件: $BLUESEA_LEFT_PARAMS_FILE"
echo "右雷达参数文件: $BLUESEA_RIGHT_PARAMS_FILE"
echo "雷达融合参数文件: $BLUESEA_SCAN_MERGER_PARAMS_FILE"
fi
echo "RTAB-Map 地面分割: $RTABMAP_GRID_GROUND_SEGMENTATION"
echo "RTAB-Map 订阅激光: $RTABMAP_SUBSCRIBE_SCAN"
echo "RTAB-Map 图像降采样: $RTABMAP_ODOM_IMAGE_DECIMATION"
echo "RTAB-Map 最大特征数: $RTABMAP_VIS_MAX_FEATURES"
echo "RTAB-Map 局部地图大小: $RTABMAP_ODOM_F2M_MAX_SIZE"
echo "RTAB-Map 里程计BA: $RTABMAP_ODOM_F2M_BUNDLE_ADJUSTMENT"
echo "RTAB-Map 建图位移更新阈值: $RTABMAP_RGBD_LINEAR_UPDATE"
echo "RTAB-Map 建图角度更新阈值: $RTABMAP_RGBD_ANGULAR_UPDATE"
echo "底盘控制脚本: $BASE_CONTROLLER_SCRIPT"
echo "终端日志目录: $TERMINAL_LOG_DIR"
echo ""
启动机器人模型与固定传感器TF
echo "1. 启动机器人模型与固定传感器TF"
launch_terminal "[1] Robot Model" "
${ROBOT_MODEL_PREFIX:+$ROBOT_MODEL_PREFIX }ros2 launch description diff_base.launch.py \
use_rviz:=false \
config_file:=$ROBOT_DESCRIPTION_CONFIG_FILE
"
启动 BlueSea 双雷达与 LaserScan 融合”
if [ "$ENABLE_BLUESEA_LIDAR" = "1" ]; then
wait_for_tf_transform "base_link" "lidar_left" 20 || true
wait_for_tf_transform "base_link" "lidar_right" 20 || true
echo "2. 启动 BlueSea 双雷达与 LaserScan 融合"
launch_terminal "[2] BlueSea LiDAR" "
${LIDAR_PREFIX:+$LIDAR_PREFIX }ros2 launch '$BLUESEA_LIDAR_LAUNCH_FILE' \
left_params_file:='$BLUESEA_LEFT_PARAMS_FILE' \
right_params_file:='$BLUESEA_RIGHT_PARAMS_FILE' \
enable_scan_merger:=true \
scan_merger_params_file:='$BLUESEA_SCAN_MERGER_PARAMS_FILE' \
use_robot_model:=false \
start_rviz:=false \
use_sim_time:=false
"
else
echo "2. 跳过 BlueSea 激光雷达，ENABLE_BLUESEA_LIDAR=$ENABLE_BLUESEA_LIDAR"
fi
启动深度相机
echo "3. 启动深度相机"
launch_terminal "[3] Depth Camera" "
${CAMERA_PREFIX:+$CAMERA_PREFIX }ros2 launch orbbec_camera gemini_330_series.launch.py \
camera_name:=camera \
publish_tf:=true \
enable_sync_host_time:=true \
time_domain:=global \
enable_color:=true \
enable_depth:=true \
enable_accel:=true \
enable_gyro:=true \
enable_sync_output_accel_gyro:=true \
enable_frame_sync:=true \
color_width:=640 \
color_height:=480 \
color_fps:=30 \
depth_width:=640 \
depth_height:=480 \
depth_fps:=30 \
align_mode:=HW \
depth_registration:=true \
enable_point_cloud:=false \
color_qos:=default \
depth_qos:=default \
color_camera_info_qos:=default \
depth_camera_info_qos:=default
"
sleep 3
启动底盘控制
if [ "$ENABLE_BASE_CONTROLLER" = "1" ]; then
echo "4. 启动底盘控制"
launch_terminal "[4] Base Controller" "
${BASE_CONTROLLER_PREFIX:+$BASE_CONTROLLER_PREFIX }python3 '$BASE_CONTROLLER_SCRIPT'
"
sleep 2
else
echo "4. 跳过底盘控制，ENABLE_BASE_CONTROLLER=$ENABLE_BASE_CONTROLLER"
fi
启动视觉里程计
echo "5. 启动视觉里程计"
launch_terminal "[5] RGBD Odometry" "
${RGBD_ODOM_PREFIX:+$RGBD_ODOM_PREFIX }ros2 run rtabmap_odom rgbd_odometry \
--Vis/MinInliers 12 \
--OdomF2M/BundleAdjustment false \
--Reg/Force3DoF true \
--ros-args \
-p frame_id:=base_link \
-p odom_frame_id:=odom \
-p publish_tf:=false \
-p wait_for_transform:=0.2 \
-p wait_imu_to_init:=true \
-p approx_sync:=true \
-p approx_sync_max_interval:=0.02 \
-p topic_queue_size:=30 \
-p sync_queue_size:=30 \
-p qos:=0 \
-p qos_camera_info:=0 \
-p qos_imu:=0 \
-r rgb/image:=/camera/color/image_raw \
-r depth/image:=/camera/depth/image_raw \
-r rgb/camera_info:=/camera/color/camera_info \
-r imu:=/imu/data \
-r odom:=$RTABMAP_RGBD_ODOM_TOPIC
"
sleep 2
启动激光ICP里程计
if [ "$ENABLE_BLUESEA_LIDAR" = "1" ]; then
echo "6. 启动激光ICP里程计"
launch_terminal "[6] Laser ICP Odometry" "
${LASER_ODOM_PREFIX:+$LASER_ODOM_PREFIX }ros2 run rtabmap_odom icp_odometry \
--Reg/Force3DoF true \
--Icp/PointToPlane false \
--Icp/MaxCorrespondenceDistance 0.3 \
--ros-args \
-p frame_id:=base_link \
-p odom_frame_id:=odom \
-p publish_tf:=false \
-p wait_for_transform:=0.2 \
-p wait_imu_to_init:=false \
-p scan_range_min:=0.2 \
-p scan_range_max:=15.0 \
-p scan_voxel_size:=0.05 \
-p qos:=1 \
-p qos_imu:=1 \
-r scan:=/scan \
-r imu:=/imu/data \
-r odom:=$RTABMAP_LASER_ODOM_TOPIC
"
sleep 2
else
echo "6. 跳过激光ICP里程计，ENABLE_BLUESEA_LIDAR=$ENABLE_BLUESEA_LIDAR"
fi
启动 robot_localization 融合视觉/激光里程计
if [ "$ENABLE_ROBOT_LOCALIZATION" = "1" ]; then
echo "7. 启动 robot_localization 融合视觉/激光里程计"
launch_terminal "[7] Robot Localization EKF" "
${ROBOT_LOCALIZATION_PREFIX:+$ROBOT_LOCALIZATION_PREFIX }ros2 run robot_localization ekf_node \
--ros-args \
--params-file '$ROBOT_LOCALIZATION_PARAMS_FILE' \
-r odometry/filtered:=$FUSED_ODOM_TOPIC
"
sleep 2
else
echo "7. 跳过 robot_localization，ENABLE_ROBOT_LOCALIZATION=$ENABLE_ROBOT_LOCALIZATION"
fi
启动RTABMAP建图
echo "8. 启动RTABMAP建图"
launch_terminal "[8] RTABMAP Mapping" "
${RTABMAP_PREFIX:+$RTABMAP_PREFIX }ros2 launch rtabmap_launch rtabmap.launch.py \
database_path:=$RTABMAP_DATABASE_PATH \
rtabmap_viz:=false \
rviz:=true \
rviz_cfg:=$MAP_RVIZ_CONFIG \
frame_id:=base_link \
visual_odometry:=false \
icp_odometry:=false \
vo_frame_id:=odom \
odom_topic:=$FUSED_ODOM_TOPIC \
publish_tf_map:=true \
publish_tf_odom:=false \
rgb_topic:=/camera/color/image_raw \
depth_topic:=/camera/depth/image_raw \
camera_info_topic:=/camera/color/camera_info \
imu_topic:=/imu/data \
wait_imu_to_init:=true \
approx_sync:=true \
approx_sync_max_interval:=0.02 \
subscribe_scan:=$RTABMAP_SUBSCRIBE_SCAN \
scan_topic:=/scan \
topic_queue_size:=30 \
sync_queue_size:=30 \
odom_args:='
--Vis/MinInliers 25
--OdomF2M/BundleAdjustment true
' \
args:='
--RGBD/LinearUpdate 0.05
--RGBD/AngularUpdate 0.03
--Optimizer/PriorsIgnored false
--Optimizer/GravitySigma 0.0
--Reg/Force3DoF true
--Grid/FromDepth false
--Grid/Sensor 0
--Grid/RangeMax 5.0
--Grid/RangeMin 0.2
--Grid/CellSize 0.05
--Grid/MaxGroundHeight $RTABMAP_GRID_MAX_GROUND_HEIGHT
--Grid/MinGroundHeight $RTABMAP_GRID_MIN_GROUND_HEIGHT
--Grid/MaxObstacleHeight $RTABMAP_GRID_MAX_OBSTACLE_HEIGHT
--Grid/NormalsSegmentation $RTABMAP_GRID_GROUND_SEGMENTATION
--Grid/GroundIsObstacle $RTABMAP_GRID_GROUND_IS_OBSTACLE
--Grid/MaxGroundAngle $RTABMAP_GRID_MAX_GROUND_ANGLE
--Grid/ClusterRadius 0.1
--Grid/MinClusterSize 10
--Grid/NoiseFilteringRadius 0.1
--Grid/NoiseFilteringMinNeighbors 8
--Grid/FlatObstacleDetected $RTABMAP_GRID_FLAT_OBSTACLE_DETECTED
--Grid/RayTracing true
--Grid/3D false
'
"
sleep 3
启动Nav2导航（边建图边导航，静态/动态障碍物避障)
echo "9. 启动Nav2导航（边建图边导航，静态/动态障碍物避障）"
launch_terminal "[9] Nav2 Navigation" "
${NAV2_PREFIX:+$NAV2_PREFIX }ros2 launch '$NAV2_LAUNCH_PACKAGE' '$NAV2_LAUNCH_FILE' \
use_sim_time:=false \
params_file:=$NAV2_PARAMS_FILE \
autostart:=true \
use_composition:=False \
use_respawn:=False \
log_level:=info
"
启动 Collision Monitor（/scan 紧急防撞）
echo "10. 启动 Collision Monitor（/scan 紧急防撞）"
launch_terminal "[10] Collision Monitor" "
${COLLISION_MONITOR_PREFIX:+$COLLISION_MONITOR_PREFIX }ros2 launch nav2_collision_monitor collision_monitor_node.launch.py \
use_sim_time:=false \
params_file:=$NAV2_PARAMS_FILE
"
无人车底盘
差速底盘
差速底盘（Differential Drive）是移动机器人里最经典、也是 ROS 生态最“默认假设”的一种运动模型。它的核心思想其实很简单：用左右两侧轮子的速度差，来控制机器人在平面上的运动与转向。
差速底盘的运动只用两个量描述：
v：线速度（forward/backward） ω：角速度（旋转）
👉 “通过左右轮速度差，控制机器人在平面上做前进 + 转向组合运动”
四轮车做差速通常有三种工程方案：
| 方案 | 本质 | 工程常见度 |
|---|---|---|
| 1️⃣ 左右双电机（推荐） | 左侧2轮同速 + 右侧2轮同速 | ⭐⭐⭐⭐⭐ |
| 2️⃣ 4电机独立控制 | 4轮全独立，再做“虚拟差速” | ⭐⭐⭐ |
| 3️⃣ 中央差速器（机械） | 类似汽车结构 | ⭐⭐ |
全向底盘
全向底盘（Omni-directional / Mecanum / Holonomic Drive）可以简单理解为：
🚗 机器人可以“往任何方向走”，包括横移、斜移、原地旋转，而不需要转弯半径
为什么它“看起来很高级”但工程很难？
❌ 1. 非常容易打滑
因为：
轮子本身就允许侧滑
地面不平 → 模型失真
❌ 2. odom 非常容易漂
RTAB-Map / EKF 会遇到：
x/y 同时误差
方向不稳定
❌ 3. 对地面要求高
地面 表现
光滑地板 很好
水泥地 一般
室外粗糙路面 很差
❌ 4. 控制更复杂
需要：
逆运动学矩阵
4轮解耦控制
轮速同步
diff_base.yaml 描述底盘结构
Z↑ (高度方向)
GPS (front mast, ~1.07m)
●
|
|
Camera (0.884m)
●
|
┌────────────────┴────────────────┐
| |
Lidar Left Lidar Right
(0.388, +0.285) (0.388, -0.285)
yaw +45° yaw -45°
\ /
\ /
\ /
┌──────── base_link ────────┐
| (0,0,0) |
| |
| wheels / chassis |
| |
└─────────┬──────────────────┘
|
|
Rear GPS
●
|
(-0.325, rear mast)
diff_base.launch.py
1️⃣ 📦 读取机器人配置（YAML）
2️⃣ 🔧 生成 URDF（Xacro → robot_description）
3️⃣ 🚀 启动 ROS 运行系统
YAML配置
↓
Python解析
↓
Xacro参数映射
↓
URDF(robot_description)
↓
robot_state_publisher
↓
TF树发布
↓
RViz显示
双激光雷达
dual_lidar.launch.py
双雷达系统集成编排层（System Orchestration Layer）
它不做算法，只负责：
- 启动两个雷达驱动
- 建立 TF 坐标关系
- 融合 scan
- 提供 RViz 可视化
- 可选加载机器人模型
dual_lidar.launch.py
│
├── 1️⃣ 参数层（LaunchArguments）
├── 2️⃣ 机器人模型层（robot_state_publisher）
├── 3️⃣ 传感器层（left / right lidar）
├── 4️⃣ 融合层（scan_merger）
└── 5️⃣ 可视化层（RViz）
controller_mqtt.py
一个“多输入源 + 四轮滑移底盘控制 + VIO闭环 + MQTT/串口输出”的ROS2底盘控制中间层
┌─────────────────────────────────────────────┐
│ 输入层 │
├─────────────────────────────────────────────┤
│ ROS cmd_vel (Twist) │
│ Web HTTP (Twist / RPM) │
│ MQTT 云控 (Twist) │
│ SundayRobot MQTT (Twist) │
│ Odometry / VIO (反馈) │
└───────────────┬─────────────────────────────┘
│
▼
┌─────────────────────────────────────────────┐
│ 多缓存状态层（核心） │
│---------------------------------------------│
│ _latest_ros_cmd (Twist) │
│ _latest_web_cmd (Twist) │
│ _latest_web_motor_rpm (RPM) │
│ _latest_mqtt_cmd (Twist) │
│ _latest_odom_twist (feedback) │
│ + 每个都有 timestamp │
└───────────────┬─────────────────────────────┘
│
▼
┌─────────────────────────────────────────────┐
│ 候选生成层（你图里缺的关键层） │
│---------------------------------------------│
│ Candidate A: ROS TWIST │
│ Candidate B: WEB TWIST │
│ Candidate C: WEB RPM ⭐（独立通道） │
│ Candidate D: MQTT TWIST │
│ │
│ ⚠ 注意：RPM不是Twist体系！ │
└───────────────┬─────────────────────────────┘
│
▼
┌─────────────────────────────────────────────┐
│ 隐式仲裁层 (_pick_fresh_control) │
│---------------------------------------------│
│ 1. 时间戳最新优先 │
│ 2. ROS 防抢占规则 │
│ 3. neutral过滤（避免0命令覆盖Nav） │
│ 4. Twist / RPM 分流选择 │
└───────────────┬─────────────────────────────┘
┌───────┴────────┐
▼ ▼
┌──────────────┐ ┌─────────────────┐
│ TWIST路径 │ │ RPM路径 │
└──────┬───────┘ └──────┬──────────┘
│ │
▼ ▼
┌──────────────────┐ ┌──────────────────┐
│ VIO PID闭环 │ │ 直接输出 │
│ _apply_speed_comp │ │ (skip kinematics) │
└──────┬───────────┘ └─────────┬────────┘
│ │
▼ ▼
┌─────────────────────────────────────────────┐
│ 四轮滑移运动学模型 │
│---------------------------------------------│
│ skid-steer kinematics │
│ slip_factor 修正 │
│ front_rear_ratio │
└───────────────┬─────────────────────────────┘
▼
┌─────────────────────────────────────────────┐
│ 输出层 │
├─────────────────────────────────────────────┤
│ MQTT wheel_speed │
│ 串口 JSON (FOC驱动板) │
│ 状态上报 MQTT │
└─────────────────────────────────────────────┘
rtabmap 视觉里程计和激光里程计
RTAB-Map 里的视觉里程计（VO）和激光里程计（LO），本质都是在做同一件事：估计相邻帧（或相邻扫描）之间的运动（6DoF pose）
| 项目 | 视觉里程计（VO） | 激光里程计（LO / ICP） |
|---|---|---|
| 输入 | RGB / RGB-D / Stereo 图像 | LiDAR 点云 |
| 核心信息 | 纹理 + 特征点 | 几何结构（距离） |
| 依赖环境 | 光照敏感 | 光照无关 |
| 计算方式 | 特征匹配 / PnP / 光流 | ICP / NDT / 点云配准 |
| 输出 | 相机位姿 | 传感器位姿 |
| 稳定性 | 中等（环境依赖大） | 高（结构稳定环境下） |
| 漂移 | 较明显 | 较小 |
| 适用场景 | 室内视觉丰富环境 | 室外/结构化空间 |
一个非常关键的误区
很多人以为：
“相机自带里程计”
❌ 错
正确是：
Gemini 335/336L
↓
ROS driver（orbbec_camera）
↓
RTAB-Map
├── VO（视觉）
└── LO（ICP）
👉 VO / LO 都是 RTAB-Map 做的，不是相机
0
次点赞
