基于Pixhawk和ROS搭建自主无人车（五）：SLAM导航篇

参考

• PX4 Autopilot User Guide
• ArduPilot Documentation

• 基于Pixhawk和ROS搭建自主无人车（文章链接汇总）

1. 硬件平台

2. 环境搭建

2.1 创建工作空间

$ cd
$ mkdir -p mav_ws/src
$ cd mav_ws
$ catkin_init_workspace

ROS命令 catkin_init_workspace 分析

2.2 安装 RPLiDAR 包

$ cd ~/mav_ws/src
$ git clone https://github.com/Slamtec/rplidar_ros.git

2.3 安装 Cartographer

• 先安装一些工具

  $ sudo apt-get update
  $ sudo apt-get install -y python-wstool python-rosdep ninja-build stow
  # noetic 则使用下行代码，上行默认为 melodic 版本
  $ sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow
  

• 使用 wstool 重新初始化工作区，然后合并 cartographer_ros.rosinstall 文件并获取依赖项的代码

  $ cd ~/mav_ws
  $ wstool init src
  $ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
  $ wstool update -t src
  

• 安装 proto3 和 abseil

  $ wget http://fishros.com/install -O fishros && bash fishros  # 根据提示选择 3 安装 rosdepc 
  $ src/cartographer/scripts/install_proto3.sh
  $ sudo rosdepc init
  $ rosdepc update
  # 进行下一步之前，先注释 cartographer 包下 package.xml 中该行代码：(<depend>libabsl-dev</depend>)  
  $ src/cartographer/scripts/install_abseil.sh
  $ rosdepc install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
  

• 安装 robot_pose_publisher（使用 TF 发布机器人相对于地图位置的节点）

  $ cd ~/mav_ws/src
  $ git clone https://github.com/GT-RAIL/robot_pose_publisher.git
  

• 创建 cartographer.launch 文件

  $ cd ~/mav_ws/src/cartographer_ros/cartographer_ros/launch
  $ gedit cartographer.launch
  

  <!-- cartographer.launch -->
  <launch>
     <param name="/use_sim_time" value="false" />
     <node name="cartographer_node"
           pkg="cartographer_ros"
           type="cartographer_node"
           args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename cartographer.lua"
           output="screen">
           <remap from="odom" to="/mavros/local_position/odom" />
           <remap from="imu" to="/mavros/imu/data" />
     </node>
     <node name="cartographer_occupancy_grid_node"
           pkg="cartographer_ros"
           type="cartographer_occupancy_grid_node" />
     <node name="robot_pose_publisher"
           pkg="robot_pose_publisher"
           type="robot_pose_publisher"
           respawn="false"
           output="screen" >
           <param name="is_stamped" type="bool" value="true"/>
           <remap from="robot_pose" to="/mavros/vision_pose/pose" />
     </node>
     <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />
  </launch>
  

• 创建 cartographer.lua 脚本文件

  $ cd ~/mav_ws/src/cartographer_ros/cartographer_ros/configuration_files
  $ gedit cartographer.lua
  

  include "map_builder.lua"
  include "trajectory_builder.lua"
  
  options = {
    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    map_frame = "map",
    tracking_frame = "base_link",
    published_frame = "base_link",
    odom_frame = "odom",
    provide_odom_frame = true,
    publish_frame_projected_to_2d = false,
    use_odometry = false,
    use_nav_sat = false,
    use_landmarks = false,
    num_laser_scans = 1,
    num_multi_echo_laser_scans = 0,
    num_subdivisions_per_laser_scan = 1,
    num_point_clouds = 0,
    lookup_transform_timeout_sec = 0.2,
    submap_publish_period_sec = 0.3,
    pose_publish_period_sec = 5e-3,
    trajectory_publish_period_sec = 30e-3,
    rangefinder_sampling_ratio = 1.,
    odometry_sampling_ratio = 1.,
    fixed_frame_pose_sampling_ratio = 1.,
    imu_sampling_ratio = 1.,
    landmarks_sampling_ratio = 1.,
  }
  
  MAP_BUILDER.use_trajectory_builder_2d = true
  
  TRAJECTORY_BUILDER_2D.min_range = 0.05
  TRAJECTORY_BUILDER_2D.max_range = 30
  TRAJECTORY_BUILDER_2D.missing_data_ray_length = 8.5
  TRAJECTORY_BUILDER_2D.use_imu_data = false
  TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.2
  TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 5
  TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
  TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
  TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1.
  TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 10
  TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
  -- for current lidar only 1 is good value
  TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
  
  TRAJECTORY_BUILDER_2D.min_z = -0.5
  TRAJECTORY_BUILDER_2D.max_z = 0.5
  
  POSE_GRAPH.constraint_builder.min_score = 0.65
  POSE_GRAPH.constraint_builder.global_localization_min_score = 0.65
  POSE_GRAPH.optimization_problem.huber_scale = 1e2
  POSE_GRAPH.optimize_every_n_nodes = 30
  
  return options
  

2.4 编译工作空间

$ cd ~/mav_ws
$ catkin build
$ source devel/setup.bash

catkin_make 和catkin build这两个命令的区别

2.5 启动

• 启动 roscore

  $ roscore
  

• 启动激光雷达节点

  # 本文使用 RPLiDAR A1 型号激光雷达，根据不同型号替换 rplidar_a1.launch
  $ roslaunch rplidar_ros rplidar_a1.launch
  

  <!-- rplidar_a1.launch -->
  <!-- 注意查看激光雷达连接机载电脑的端口号 /dev/ttyUSB0 -->
  <launch>
    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
    </node>
  </launch>
  

• 启动 cartographer

  $ roslaunch cartographer_ros cartographer.launch
  

• 启动 MAVROS 通信

  $ roslaunch mavros apm.launch
  

  基于Pixhawk和ROS搭建自主无人车（三）：ROS通信篇

3. 配置 APM 固件参数

• 打开 Mission Planner 地面站，在全部参数表中配置以下参数（记得写入参数并重启地面站）

  AHRS_EKF_TYPE = 3
  EK2_ENABLE = 0
  EK3_ENABLE = 1
  EK3_SRC1_POSXY = 6
  EK3_SRC1_POSZ = 1
  EK3_SRC1_VELXY = 6
  EK3_SRC1_VELZ = 6
  EK3_SRC1_YAW = 6
  GPS_TYPE = 0
  VISO_TYPE = 1
  ARMING_CHECK = 388598
  

• 在飞行数据界面地图上右键，然后选择 “设置家在此” >> “Set Home Here” >> “Set EKF Origin Here”，车辆应立即出现在地图上
  

4. 测试

• 要确认 ROS 端正常工作，请输入以下命令，并且应显示 cartographer 位置估计的实时更新

  $ rostopic echo /mavros/vision_pose/pose
  $ rostopic info /mavros/vision_pose/pose
  

• 打开 Mission Planner 地面站，在飞行数据界面按 Ctrl+F，然后点击 MAVLink Inspector，出现以下界面证明 VISION_POSITION_ESTIMATE 消息已成功发送到飞控
  

5. 报错解决

• PreArm：Fence requires position：将参数 FENCE_ENABLE 设置为 0
  

• FreArm：servo function 33 unassigned：将 33-36 对应的 Motor 都设置好（可以不使用，但必须设置）
  

6. 从 rviz 发布指令

• 通过远程桌面（如 Todesk）或 ssh 在机载电脑启动 rviz
  • Add --> LaserScan --> /scan
  • Add --> Map --> /map（本文 cartographer 建图节点 died 导致无法显示 map，暂未解决）
  • Add --> Pose --> /mavros/vision_pose/pose

  $ rosrun rviz rviz
  

• 修改 mavros 的 node.launch 文件

  ...
  <rosparam command="load" file="$(arg config_yaml)" />
  <!-- 在上行代码后添加下行代码 -->
  <remap from="/mavros/setpoint_position/local" to="/move_base_simple/goal" />
  ...
  

• Hold 模式下解锁（Arm）车辆，并切换到 Guided 模式，在 rviz 中设置 2D Nav Goal

  • 此时 Mission Planner 地面站应显示车辆正在驶向的目标位置（Mission Planner 地面站在目标位置放置一个绿色标记，并从车辆向目标绘制一条橙色线）

• 通信测试，显示下图所示内容表示通过 rviz 发布指令通信成功

  # 应将发布者显示为 “rviz”，订阅者显示为 “mavros”
  $ rostopic info /move_base_simple/goal
  
  # 在 rviz 中设置 “2D NavGoal” 后应立即显示 “position” 和 “orientation”
  $ rostopic echo /move_base_simple/goal
  
  # 显示 ROS 节点及其连接的图表
  $ rosrun rqt_graph rqt_graph