ROS2 无人车学习

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 ""

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
"

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

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

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

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

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
"

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
"