ROS-基于PX4的无人机SLAM建图(Cartographer)仿真
一、准备工作
1.1、安装Ubuntu和ROS系统
首先在电脑上安装好Ubuntu系统和ROS系统，我安装的是Ubuntu18.04和ROS Melodic，不同的Ubuntu版本对应不同的ROS版本
| ROS发布日期 | ROS版本 | 停止支持日期 | 对应Ubuntu版本 |
|---|---|---|---|
| 2018年5月23日 | ROS Melodic Morenia | 2023年5月 | Ubuntu 18.04 |
| 2016年5月23日 | ROS Kinetic Kame | 2021年4月 | Ubuntu 16.04 (Xenial) Ubuntu 15.10 (Wily) |
| 2015年5月23日 | ROS Jade Turtle | 2017年5月 | Ubuntu 15.04 (Wily) Ubuntu 14.04 LTS (Trusty) |
| 2014年7月22日 | ROS Indigo Igloo | 2019年4月 | Ubuntu 14.04 (Trusty) |
| 2013年9月4日 | ROS Hydro Medusa | 2015年5月 | Ubuntu 12.04 LTS (Precise) |
| 2012年12月31日 | ROS Groovy Galapagos | 2014年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-build1.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.launchcartographer_demo_rplidar.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.launch3、启动通信脚本
cd ~/XTDrone/communication/
python multirotor_communication.py iris 04、启动键盘控制脚本，控制无人机飞行，注意不能飞太高，导致激光雷达扫不到围墙
cd ~/XTDrone/control/keyboard/
python multirotor_keyboard_control.py iris 1 vel5、通过键盘控制无人机飞行，扫描地图环境，建立地图
可以先按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