基于Cartographer的建图与导航

2023-05-16

一、 RoboSense 16线雷达驱动安装 2

二、 Cartographer的安装 2

三、配置文件结构说明 4

四、配置文件详解 4

2D lua文件的配置 5

2D launch文件的配置 6

2D pure location lua文件的配置 7

2D pure location launch文件的配置 7

3D lua文件的配置 8

3D launch文件的配置 9

3D pure location lua文件的配置 10

3D pure location launch文件的配置 10

Assets writer 3D lua文件的配置 11

Assets writer 3D launch文件的配置 12

五、建图命令 12

六、纯定位命令 13

七、导航文件配置 13

launch文件的配置 13

move_base_params.yaml文件参数 15

costmap_common_params.yaml文件参数 16

local_costmap_params.yaml 文件参数 17

global_costmap_params.yaml文件参数 18

dwa_local_planner_params.yaml文件参数 18

navfn_global_planner_params.yaml文件参数 19

八、导航命令 20

九、发送目标点的导航路线规划 20

十、导航探索SLAM 20

十一、其余功能包说明 21

1. imu_pub 21

2. odom_pub（未用到） 21

3. cmd_vel_to_stm 21

4. pointcloud_to_laserscan 21

参数设置请参照wiki.ros.org/pointcloud_to_laserscan 21

十二、相关博客推荐 21

Ros版本：ROS-Kinect

工作目录：~/carto_ws

RoboSense 16线雷达驱动安装

安装依赖项

$>> sudo apt-get install libpcap-dev

下载源码到工作目录的src/文件夹下

$>> cd ~/carto_ws/src

$>> git clone https://github.com/RoboSense-LiDAR/ros_rslidar

修改文件权限

$>> cd ~/carto_ws/src/ros_rslidar/rslidar_drvier

$>> chmod 777 cfg/*

$>> cd ~/carto_ws/src/ros_rslidar/rslidar_pointcloud

$>> chmod 777 cfg/*

源码编译

$>> cd ~/carto_ws

$>> catkin_make

设置电脑的静态ip地址

$>> sudo gedit /etc/networsk/interfaces

设置eth0（有些是eth1，或者eno0等，根据自己的网口修改）,修改或增加内容如下：

# interfaces eth0 for rslidar connect

auto eth0

iface eth0 inet static

address 192.168.1.102

netmask 255.255.255.0

gateway 192.168.1.1

连接雷达电源和网线，重启电脑。

$>> cd carto_ws

$>> source devel/setup.bash

$>> roslaunch rslidar_pointcloud rs_lidar_16.launch

可以在RVIZ中看到点云,注意要设置全局坐标系为rslidar。

Cartographer的安装

cartographer的安装主要是ceres、prtobuf和cartographer的安装，cartographer_ros是ROS的功能包，调用cartographer算法。

安装依赖：

$>> sudo apt-get update

$>> sudo apt-get install -y python-wstool python-rosdep ninja-build

安装Ceres solver

$>> cd ~/carto_ws/document

$>> git clone GitHub - ceres-solver/ceres-solver: A large scale non-linear optimization library

$>> cd ceres-solver

$>> mkdir build

$>> cd build

$>> cmake ..

$>> make -j

$>> sudo make install

ceres：可能存在报错，

先查看gcc版本：ubuntu16.04安装ceres时make出错_阿巴乾的博客-CSDN博客_ceres库make失败 https://blog.csdn.net/weixin_44235615/article/details/122651628

如果版本正确，编译ceres时，需要更改eigen3 到 3.3 版本以上。3.4测试有效。

安装Prtobuf 3.0

$>> cd ~/carto_ws/document

$>> git clone https://github.com/google/protobuf.git

$>> cd protobuf

$>> git checkout v3.6.1

$>> mkdir build

$>> cd build

$>> cmake \

-DCMAKE_POSITION_INDEPENDENT_CODE=ON \

-DCMAKE_BUILD_TYPE=Release \

-Dprotobuf_BUILD_TESTS=OFF \

../cmake

$>> make -j 2

$>> sudo make install

安装Cartographer

$>> cd ~/carto_ws/document

$>> git clone https://github.com/BlueWhaleRobot/cartographer.git

$>> cd cartographer

$>> mkdir build

$>> cd build

$>> cmake ..

$>> make -j

$>> sudo make install

安装cartographer_ros

$>> cd ~/carto_ws/src #请修改路径到自己的ROS catkin工作空间

$>> git clone https://github.com/BlueWhaleRobot/cartographer_ros.git

$>> cd ..

$>> catkin_make_isolated --install --use-ninja

测试

下载文件：https://www.bwbot.org/s/vQ2D9Z

然后运行。根据个人平台计算能力不同，完整运行时间一般为半个小时到1个小时之间

$>> source devel_ioslated/setup.bash

$>> roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Desktop/cartographer_paper_deutsches_museum.bag

##修改到自己保存的路径

提取建立的地图，结束测试

上面步骤会生成一个pbstream文件，用cartographer_ assets_writer可以转换成ROS使用的栅格地图。

$>> roslaunch cartographer_ros assets_writer_ros_map.launch bag_filenames:=${HOME}/Desktop/cartographer_paper_deutsche

配置文件结构说明

工作目录为carto_ws，src同级目录除了编译产生的，其余都是安装依赖留下的文件夹。

Cartographer配置文件主要存在src/cartographer_ros/cartographer_ros下的urdf文件夹的myself_robot.urdf、configuration文件夹下的myself_rslidar_2(3)d.lua，myself_backpack_2(3)d_localization.lua以及rslidar_2(3)d.rviz、demo_2d.rviz配置文件、launch文件夹下的myself_rslidar_2(3)d.launch、myself_backpack_2(3)d_localization.launch。这里定位由于和后面导航rviz显示冲突，注释了rviz，纯定位时需要取消注释，显示调用rviz。

在3D建图还需要后面进行离线处理，需要assets_writer_myself_3d.launch，其中相关的配置文件为 assets_writer_myself_3d.lua。

利用bag包进行离线3D SLAM建图可以使用offline_myself_rslidar_3d.launch

配置文件详解

URDF文件的配置（这里2D和3D都是使用的同一个urdf）：

</material>

</material>

</geometry>

</visual>

</link>

</geometry>

</visual>

</link>

</geometry>

</visual>

</link>

</joint>

</joint>

</robot>

2D lua文件的配置

include "map_builder.lua"

include "trajectory_builder.lua"

options = {

map_builder = MAP_BUILDER,

trajectory_builder = TRAJECTORY_BUILDER,

map_frame = "map", #这个是地图坐标系名称

tracking_frame = "imu",#设置为IMU的坐标系

published_frame = "base_link",#设置为机器人基底坐标系

odom_frame = "odom",#里程计坐标系名称

provide_odom_frame = true,#发布base_link->odom的tf变化，否则发布base_link->map

publish_frame_projected_to_2d = false, #如果启用，发布的姿态将被限制为纯2D姿态(无滚动、俯仰或z-偏移)。

use_odometry = false,#是否使用编码器提供odom

use_nav_sat = false,#是否使用gps

use_landmarks = false,#是否使用路标

num_laser_scans = 0,#使用2d 雷达的数量

num_multi_echo_laser_scans = 0,#使用 echo_laser的数量

num_subdivisions_per_laser_scan = 1,

num_point_clouds = 1, #使用3D雷达的数量

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.,

}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

MAP_BUILDER.use_trajectory_builder_3d = true

MAP_BUILDER.num_background_threads = 7

POSE_GRAPH.optimization_problem.huber_scale = 5e2

POSE_GRAPH.optimize_every_n_nodes = 320

POSE_GRAPH.constraint_builder.sampling_ratio = 0.03

POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10

POSE_GRAPH.constraint_builder.min_score = 0.62

POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

其他参数，参考链接：

Lua configuration reference documentation — Cartographer ROS documentation

2D launch文件的配置

<param name="robot_description"

textfile="$(find cartographer_ros)/urdf/myself_robot.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"

type="robot_state_publisher" />

<node name="cartographer_node" pkg="cartographer_ros"

type="cartographer_node" args="

-configuration_directory

$(find cartographer_ros)/configuration_files

-configuration_basename myself_rslidar_2d.lua"

output="screen">

</node>

<node name="rviz" pkg="rviz" type="rviz" required="true"

args="-d $(find cartographer_ros)/configuration_files/rslidar_2d.rviz" />

</launch>

2D pure location lua文件的配置

include "myself_rslidar_2d.lua"

TRAJECTORY_BUILDER.pure_localization = true

POSE_GRAPH.optimize_every_n_nodes = 20

--fast location

MAP_BUILDER.num_background_threads=8

POSE_GRAPH.constraint_builder.sampling_ratio=0.5*POSE_GRAPH.constraint_builder.sampling_ratio

POSE_GRAPH.global_sampling_ratio=0.1*POSE_GRAPH.global_sampling_ratio

POSE_GRAPH.max_num_final_iterations=1

return options

2D pure location launch文件的配置

<param name="robot_description"

textfile="$(find cartographer_ros)/urdf/myself_robot.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"

type="robot_state_publisher" />

<node name="cartographer_node" pkg="cartographer_ros"

type="cartographer_node" args="

-configuration_directory $(find cartographer_ros)/configuration_files

-configuration_basename myself_backpack_2d_localization.lua

-load_state_filename /home/dcz/bag/map.pbstream"

output="screen">

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

</launch>

3D lua文件的配置

include "map_builder.lua"

include "trajectory_builder.lua"

options = {

map_builder = MAP_BUILDER,

trajectory_builder = TRAJECTORY_BUILDER,

map_frame = "map",

tracking_frame = "imu",

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 = 0,

num_multi_echo_laser_scans = 0,

num_subdivisions_per_laser_scan = 1,

num_point_clouds = 1,

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.,

}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

--change 当三维点云效果不好时使用离线建图方法，这里为离线建图需要调整的参数

TRAJECTORY_BUILDER_3D.min_range = 0.2

TRAJECTORY_BUILDER_3D.max_range = 30.

--need try

--TRAJECTORY_BUILDER_2D.min_z = 0.1

--TRAJECTORY_BUILDER_2D.max_z = 1.0

MAP_BUILDER.use_trajectory_builder_3d = true

MAP_BUILDER.num_background_threads = 7

POSE_GRAPH.optimization_problem.huber_scale = 5e2

POSE_GRAPH.optimize_every_n_nodes = 320

POSE_GRAPH.constraint_builder.sampling_ratio = 0.03

POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10

POSE_GRAPH.constraint_builder.min_score = 0.62

POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

3D launch文件的配置

<param name="robot_description"

textfile="$(find cartographer_ros)/urdf/myself_robot.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"

type="robot_state_publisher" />

<node name="cartographer_node" pkg="cartographer_ros"

type="cartographer_node" args="

-configuration_directory

$(find cartographer_ros)/configuration_files

-configuration_basename myself_rslidar_3d.lua"

output="screen">

</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_3d.rviz" />

3D pure location lua文件的配置

include "myself_rslidar_3d.lua"

TRAJECTORY_BUILDER.pure_localization = true

POSE_GRAPH.optimize_every_n_nodes = 100

return options

3D pure location launch文件的配置

<param name="robot_description"

textfile="$(find cartographer_ros)/urdf/myself_robot.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"

type="robot_state_publisher" />

<node name="cartographer_node" pkg="cartographer_ros"

type="cartographer_node" args="

-configuration_directory $(find cartographer_ros)/configuration_files

-configuration_basename myself_backpack_3d_localization.lua

-load_state_filename $(arg load_state_filename)"

output="screen">

</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_3d.rviz" />

</launch>

Assets writer 3D lua文件的配置

VOXEL_SIZE = 5e-2

include "transform.lua"

options = {

tracking_frame = "imu", #imu坐标系

pipeline = {

{

action = "min_max_range_filter",

min_range = 1.,

max_range = 20.,

{

action = "dump_num_points",

{

action = "fixed_ratio_sampler",

sampling_ratio = 0.01,

{

action = "write_ply",

filename = "points.ply"

-- Gray X-Rays. These only use geometry to color pixels.

-- Now we recolor our points by frame and write another batch of X-Rays. It

-- is visible in them what was seen by the horizontal and the vertical

-- laser.

{

action = "color_points",

frame_id = "rslidar",#雷达坐标系

color = { 255., 0., 0. },

{

action = "write_xray_image",

voxel_size = VOXEL_SIZE,

filename = "xray_xy_all_color",

transform = XY_TRANSFORM,

}

return options

Assets writer 3D launch文件的配置

<node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"

type="cartographer_assets_writer" args="

-configuration_directory $(find cartographer_ros)/configuration_files

-configuration_basename assets_writer_myself_3d.lua

-urdf_filename $(find cartographer_ros)/urdf/myself_robot.urdf

-bag_filenames $(arg bag_filenames)

-pose_graph_filename $(arg pose_graph_filename)"

output="screen">

</node>

</launch>

建图命令

#SLAM

启动雷达点云

$>> roslaunch rslidar_pointcloud cloud_nodelet.launch

启动Cartographer建图

$>> roslaunch cartographer_ros myself_rslidar_2d.launch

$>> roslaunch cartographer_ros myself_rslidar_3d.launch

Tips：纯定位需要修改源码，所以如果建图前请确保，src/cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node.main.cc中main函数上面第五行是否注释了

occupancy_grid_publisher_.publish(*msg);

纯定位注释掉，建图时打开，然后执行catkin_make，再执行建图或者定位。

3d建图需要先发布imu ，并且需要rosbag保存点云和imu消息：

$>> rosrun imu_pub imu_pub_node

$>> rosbag record /imu /rslidar_points

结束建图

$>> rosservice call /finish_trajectory 0

保存地图，在~/.ros目录下

$>> rosservice call /write_state “filenames: ’map.pbstream’”

拷贝出地图转化地图到ros使用的栅格地图yaml文件，使用绝对路径

rosrun cartographer_ros cartographer_pbstream_to_ros_map -pbstream_filename=<pbstream_file_path> -map_filestem=<map_file_path> -resolution=0.05

3d点云地图转化与查看

地图格式Bag转ply

$>> roslaunch cartographer_ros assert_writer_myself_3d.launch bag_filenames:=~/bag_path/2020.....bag.bag pose_graph_filename:=~/pbstream_path/map.pbstream

地图格式Ply转pcd

$>> pcl_ply2pcd your.bag_points.ply your_pcd_filename.pcd

查看点云

$>> pcl_viewer your_pcd_filename.pcd

如果效果不好，使用offline_myself_rslidar_3d.launch重新建图，记得更改launch加载的bag文件路径，如果不满意，修改myself_rslidar_3d.lua文件的TRAJECTORY_BUILD_3D.max_range的大小，建议区间在30-80，之至点云图满意收敛。

纯定位命令

命令：

$>> roslaunch cartographe_ros myself_backpack_2(3)d_localization.launch

根据实际情况加载load_state_filename，如果未显示rviz，请查看launch文件是否注释了rviz的显示。

导航文件配置

安装导航功能包

$>> sudo apt-get install ros-kinect-navigation

导航包主要是launch文件加cost_map、planner参数配置文件。

launch文件的配置

</node>

</launch>

move_base_params.yaml文件参数

# Move base node parameters. For full documentation of the parameters in this file, please see

# http://www.ros.org/wiki/move_base

#启用costmap

shutdown_costmaps: false

#设置控制频率

controller_frequency: 5.0

controller_patience: 3.0

planner_frequency: 1.0

planner_patience: 5.0

oscillation_timeout: 10.0

oscillation_distance: 0.2

#选择使用dwa local planner和navfn global planner

# local planner - default is trajectory rollout

base_local_planner: "dwa_local_planner/DWAPlannerROS"

base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

#这里定义恢复行为，当机器人震荡时，会按顺序进行恢复行为，清除障碍物，这里注释了使用默认的恢复行为。

#We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted.

## recovery behaviors; we avoid spinning, but we need a fall-back replanning

#recovery_behavior_enabled: true

#recovery_behaviors:

#- name: 'super_conservative_reset1'