ROS-基于PX4的无人机SLAM建图(Cartographer)仿真

一、准备工作

1.1、安装Ubuntu和ROS系统

首先在电脑上安装好Ubuntu系统和ROS系统，我安装的是Ubuntu18.04和ROS Melodic，不同的Ubuntu版本对应不同的ROS版本

ROS发布日期ROS版本停止支持日期对应Ubuntu版本2018年5月23日ROS Melodic Morenia2023年5月Ubuntu 18.042016年5月23日ROS Kinetic Kame2021年4月Ubuntu 16.04 (Xenial) Ubuntu 15.10 (Wily)2015年5月23日ROS Jade Turtle2017年5月Ubuntu 15.04 (Wily) Ubuntu 14.04 LTS (Trusty)2014年7月22日ROS Indigo Igloo2019年4月Ubuntu 14.04 (Trusty)2013年9月4日ROS Hydro Medusa2015年5月Ubuntu 12.04 LTS (Precise)2012年12月31日ROS Groovy Galapagos2014年7月Ubuntu 12.04 (Precise)

Ubuntu18.04安装ROS Melodic（详细，亲测安装完成，有清晰的截图步骤）

1.2、Cartographer功能包安装

1.2.1 安装工具：

sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

1.2.2 创建并初始胡工作空间：

使用如下命令创建并初始化工作空间（为了不和其他功能包冲突，最好为cartographer专门创建一个工作空间，这里我们新创建了一个工作空间carto_ws）

mkdir -p carto_ws/src
cd carto_ws
wstool init src   #ctrl+H 打开隐藏文件，会在src文件夹下生成.rosinstall文件

1.2.3 更新依赖

进入到carto_ws/src目录下，ctrl+H打开隐藏文件，打开.rosinstall文件，将如下代码复制粘贴进去

- git:
    local-name: cartographer
    uri: https://github.com/cartographer-project/cartographer.git
    version: master
- git:
    local-name: cartographer_ros
    uri: https://github.com/cartographer-project/cartographer_ros.git
    version: master
- git:
    local-name: ceres-solver
    uri: https://github.com/ceres-solver/ceres-solver.git
    version: master

再执行

wstool update -t src

注意：自动下载可能不成功，可以手动从上面的github地址中下载，解压放在carto_ws/src目录下。如果手动下载，可以不需要执行wstool update -t src命令

1.2.4 安装依赖并下载cartographer相关功能包

rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

${ROS_DISTRO}填自己的ROS版本名称，如melodic

1.2.5 编译并安装

catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

若编译安装没有报错，则cartographer功能包已安装成功。

3、安装功能包及源码下载

成功安装ROS和cartographer功能包后，参考仿真平台基础配置，安装Gazebo，MAVROS，PX4和XTDrone源码，本仿真将部分依赖XTDrone源码实现。

二、Cartographer建图配置

进入carto_ws/src/cartographer_ros/cartographer_ros/launch目录下，创建一个launch文件

cd ~/carto_ws/src/cartographer_ros/cartographer_ros/launch
touch cartographer_demo_rplidar.launch
gedit cartographer_demo_rplidar.launch

cartographer_demo_rplidar.launch文件的具体内容如下：

<launch>
  <!-- 在仿真时设为true，但当需要实际建图时，一定要改为false，否则会报错 -->
  <param name="/use_sim_time" value="true">

  <!-- 其中的***.lua文件需要改为自己的lua文件 -->
  <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename rplidar.lua" output="screen">
    <!-- remap后面的名称根据实际设置，可以在rostopic list中查找 -->
    <remap from="scan" to="iris_0/scan">
  </remap></node>

  <!-- 这是实现建图必不可少的部分 -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05">

  <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz">
</node></node></launch>

rplidar.lua文件在carto_ws/src/cartographer_ros/cartographer_ros/configuration目录下，具体内容及参数注释如下：

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
-- 地图坐标系，用来发布子地图的ROS坐标系ID，位姿的父坐标系，通常是"map"
  map_frame = "map",
-- 建图时跟踪的坐标系，如果使用了IMU应该是IMU的位置，尽管它可能转动了，"imu_link"或"base_link"
  tracking_frame = "iris_0/laser_2d",
-- 作为发布位置的子坐标系的ROS坐标系ID
-- 对于"odom"例程，如果系统的其他地方提供"odom"坐标系，那么"odom"在map_frame中的位置将会被发布，否则，设置为"base_link"即可
  published_frame = "base_link",
-- "provide_odom_frame"为真时使用，位于"published_frame"和"map_frame"之间，用来发布本地SLAM结果（非闭环），通常是"odom"
  odom_frame = "odom",
-- 如果使能，这个局部的、非闭环的、连续的"odom_frame"在"map_frame"中的位置将被发布
  provide_odom_frame = true,
-- 如果使能，发布的位姿信息将会被限制为2D位姿(无横滚、俯仰和z位置)。
  publish_frame_projected_to_2d = false,
--  use_pose_extrapolator = true,
-- 如果使能，将订阅nav_msgs/Odometry类型的topic "odom"，这种情况下，必须提供里程计，而且里程计的信息将被包含在SLAM中
  use_odometry = false,
-- 如果使能，将订阅sensor_msgs/NavSatFix类型的topic "fix"，这种情况下，必须提供导航数据，而且导航数据的信息将被包含在SLAM中
  use_nav_sat = false,
-- 如果使能，将订阅cartographer_ros_msgs/LandmarkList类型的topic "landmarks"
  use_landmarks = false,
-- 订阅的laser scan topics的个数
-- 对于单个激光，订阅sensor_msgs/LaserScan类型的"scan" topic(sensor_msgs/LaserScan这个topic将被用于SLAM的输入)
-- 对于多个激光，topics为"scan_1","scan_2"...(如果num_laser_scans大于1，那么多个被编号的scan topics，比如scan1、scan2、scan3...直到并包括num_laser_scans，将被用作SLAM的输入)
  num_laser_scans = 1,
-- 订阅多回波技术laser scan topics的个数
-- 对于单个激光，订阅sensor_msgs/MultiEchoLaserScan类型的"echoes"topic
-- 对于多个激光，topics为"echoes_1", "echoes_2"...

  num_multi_echo_laser_scans = 0,
-- 接收到的（多回波）laser scan中分离出来的点云的个数，分割scan能够保证在激光设备移动的过程中scans不弯曲变形。有相应的轨迹生成器选项可以将分离的scans累积到点云中，用来进行scan matching
  num_subdivisions_per_laser_scan = 1,
-- 订阅的点云topics的个数
-- 对于单个测距仪，订阅sensor_msgs/PointCloud2类型的"points2" topic
-- 对于多个测距仪，topics为"points2_1", "points2_2"...

  num_point_clouds = 0,
-- 使用tf2机型转换搜索的超时时间秒数
  lookup_transform_timeout_sec = 0.2,
-- 发布submap位置的间隔秒数
  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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.

TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.

TRAJECTORY_BUILDER_2D.use_imu_data = false
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 = 10.

TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

修改rplidar.lua文件的参数之后，要重新进行编译，因为lua文件，它调用了别的文件，参数修改后会有一个整体的影响：

cd carto_ws/
catkin_make_isolated --install --use-ninja

三、仿真实现

1、打开PX4中自带的室内地图环境，里面包含一个带有激光雷达的无人机（iris）：

roslaunch px4 indoor3.launch

2、运行cartographer算法，在运行之前注意要source一下进入工作空间，否则可能会报错：

source ~/carto_ws/install_isolated/setup.bash
roslaunch cartographer_ros cartographer_demo_rplidar.launch

3、启动通信脚本

cd ~/XTDrone/communication/
python multirotor_communication.py iris 0

4、启动键盘控制脚本，控制无人机飞行，注意不能飞太高，导致激光雷达扫不到围墙

cd ~/XTDrone/control/keyboard/
python multirotor_keyboard_control.py iris 1 vel

5、通过键盘控制无人机飞行，扫描地图环境，建立地图

可以先按b进入offboard模式，然后按t解锁无人机，可以从gazebo中看到无人机桨叶开始转动，再连续按i键（upward vel +）使upward vel达到0.3m/s以上，此时无人机开始起飞，飞到一定高度后按s或k使无人机悬停于某一高度，就可以移动无人机进行建图了。

6、完成建图后，保存地图

rosrun map_server map_saver -f ~/carto_ws/src/cartographer_ros-master/cartographer_ros/map/indoor3_carto

Original: https://blog.csdn.net/weixin_42600623/article/details/125377415
Author: 勤奋的搞机人
Title: ROS-基于PX4的无人机SLAM建图(Cartographer)仿真