rosbag记录与回放数据以及通过GMapping算法创建地图与导航

1、引言

通过前面几章的介绍，明白了ROS的工作原理，并且我们也能遥控机器人了，那机器人如何在实际环境中自主导航呢？要做到这点，机器人必须知道它自己在哪以及需要到哪儿去。
意味着，机器人需要有一个周围环境的地图并知道自己在地图中的位置，所以这节我们将会熟悉如何利用机器人的传感器数据来构建地图的。

2、rosbag工具

2.1、rosbag记录

这里我们先来熟悉一个记录和回放ROS话题的工具：rosbag
依然沿用上篇文章的launch文件，启动机器人：roslaunch mywanderbot test.launch
开始记录话题：rosbag record -a
会记录所有在线话题发送的消息，如果传感器很多，记录的数据会特别多，一般建议记录指定的话题，比如记录scan和tf：
rosbag record scan tf
这样会把记录的scan和tf话题上发送的所有消息，都保存在一个.bag文件中，默认是按照"年-月-日-时-分-秒"的名称，形如，2023-10-13-08-28-57.bag
当然也可以指定名称，或者加前缀来表示。
rosbag record -O test.bag scan tf
生成的文件：test.bag

rosbag record -o test.bag scan tf
生成的是test前缀加时间的名称：test_2023-10-13-08-36-10.bag
这里的记录话题，本质就是一个订阅，我们可以查看话题信息：rostopic info scan
可以看到有一个记录的订阅者。

2.2、查看rosbag

查看这个bag文件的信息：rosbag info test.bag

path:        test.bag
version:     2.0
duration:    1.2s
start:       Jan 01 1970 08:05:48.80 (348.80)
end:         Jan 01 1970 08:05:50.03 (350.03)
size:        53.9 KB
messages:    83
compression: none [1/1 chunks]
types:       sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
             tf2_msgs/TFMessage    [94810edda583a504dfda3829e70d7eec]
topics:      scan    7 msgs    : sensor_msgs/LaserScan
             tf     76 msgs    : tf2_msgs/TFMessage    (2 connections)

可以看到里面有记录的时长，大小，类型与话题等，这个查看bag文件信息，对于了解是否记录了你想要的信息是很有帮助的。 

2.3、rosbag回放

记录好了之后，可以对其进行回放：rosbag play --clock test.bag
其中--clock标志表示需要rosbag发布时间消息，这对创建地图来说是很重要的。
注意事项：--clock标志会使rosbag发布时间消息，而且时间是从这个包记录的时刻开始的，如果别的节点在发布时间消息，比如Gazebo仿真器，将会导致很多问题。如果有两个源发布时间，由于时间不一致，就会在时间跳来跳去，这将会使建图算法非常迷惑。
所以当你带--clock参数运行rosbag命令时，请确保系统中没有其他东西在发布时间，简单的处理就是关闭正在运行的仿真器。

3、创建地图

地图对于机器人而言，重要性不言而喻，这里用到gmapping包中的slam_gmapping节点来创建地图，这个节点使用的是GMapping算法，原理是使用一个Rao-Blackwellized粒子滤波器来保持对机器人自身位置的追踪，这个算法基于机器人的传感器数据和已经建立的部分地图。
虽然可以在机器人运行过程中使用实时的传感器数据来生成地图，但在此我们使用另一种方法，我们将会驱动着机器人到处移动，并使用上面的rosbag来记录传感器数据，然后进行数据的回放，使用slam_gmapping节点来获得一个地图。
使用rosbag的好处就是不用驾驶机器人再跑一次，很节省时间，尤其是你需要大量尝试不同参数的时候。

3.1、launch不存在

我们来启动机器人和键盘控制：

roslaunch turtlebot_stage turtlebot_in_stage.launch
roslaunch turtlebot_teleop keyboard_teleop.launch

如果出现下面这样的错误：

RLException: [turtlebot_in_stage.launch] is neither a launch file in package [turtlebot_stage] nor is [turtlebot_stage] a launch file name
The traceback for the exception was written to the log file

RLException: [keyboard_teleop.launch] is neither a launch file in package [turtlebot_teleop] nor is [turtlebot_teleop] a launch file name
The traceback for the exception was written to the log file

这样的错误一般是没有安装包，原因是版本问题，我们需要进行安装
sudo apt-get install ros-melodic-turtlebot-teleop

同样的这里需要注意Ubuntu的版本，以前的版本应该是 ros-melodic-turtlebot-teleop，如果出现下面这样的错误：

 E: Unable to locate package ros-melodic-turtlebot-teleop

我们使用双击tab键来提示，本人出现的是这样的版本
sudo apt-get install ros-melodic-turtlebot3-teleop
可以看出turtlebot升级成了turtlebot3

安装好了之后，我们就启动，正确命令就是如下(名称也是通过查找以及双击tab键来确定)：
roslaunch turtlebot3_gazebo turtlebot3_stage_1.launch
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 

启动了之后，如图：

键盘w,x用来线速度的加速与减速；a,d用来角速度的加速与减速；s是停止。

3.2、记录数据

熟练操作机器人之后，我们就来记录scan和tf发送的消息，这里使用-O来指定bag文件名称。
rosbag record -O data.bag /scan /tf

在驾驶机器遍历周边环境的过程，使其拥有足够多的数据。

3.3、GMapping算法

在启动建图程序之前我们先终止仿真软件，这个是很关键的，因为仿真器会发布LaserScan消息，这样就会跟rosbag重放的消息冲突，关闭之后，我们启动roscore。
开新终端，我们将要求ROS使用记录下来的包文件中的时间戳，并启动slam_gmapping节点：
rosparam set use_sim_time true
rosrun gmapping slam_gmapping
如果出现下面这样的错误：

[rospack] Error: package 'gmapping' not found

安装gmapping，同样的根据系统版本来选择：sudo apt install ros-melodic-gmapping 

安装好了之后就启动：rosrun gmapping slam_gmapping
这里需要注意的是，如果话题不是scan，需要做一个重映射，类似：scan:=mylaser_scan，好了，这个时候建图程序就已经开启了，在等待数据的输入了，接下来就是回放数据。
回放rosbag记录命令：rosbag play --clock data.bag

这个时候的地图还没有存在硬盘中，为了让slam_gmapping存盘，我们使用map_server包中的map_saver节点来保存。
rosrun map_server map_saver

[rospack] Error: package 'map_server' not found

同样看版本来安装：sudo apt-get install ros-melodic-map-server  

当然我们在保存地图的过程，如果出现下面这样的错误：

[ WARN] [1697176900.416482590, 1697172684.125851250]: MessageFilter [target=odom ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_filter] rosconsole logger to DEBUG for more information.

我们可以查看tf树来熟悉它们之间的关系：rosrun rqt_tf_tree rqt_tf_tree，如下图：

查看它们的节点是否完整，一般是因为没有记录需要的话题造成的，所以我们可以简单的将所有的消息都记录:rosbag record -O data_all.bag -a
这样就不会有什么问题了。 

3.4、整个流程

对于前面的一些错误处理与说明，这里做一个流程小结，开启两个终端，一个是启动机器人，一个就是记录全部数据：

roslaunch mywanderbot test.launch
rosbag record -O data_all.bag -a 

记录好了之后，在接下来操作之前，我们需要先将仿真器停掉，然后分别打开新终端输入如下命令：

roscore

rosparam set use_sim_time true
rosrun gmapping slam_gmapping

rosbag play --clock data_all.bag

rosrun map_server map_saver

开启GMapping算法，等待记录数据的回放进行计算处理，最后就是将地图保存到硬盘即可。
这样就生成了两个文件，一个是map.pgm（地图），另一个是map.yaml（地图元数据）
我们可以查看下在线的话题：rostopic list，如下图：

图中我们可以看到，这里多出一个/tf_static的话题，我们可以查看这个话题的信息：
rostopic info tf_static

Type: tf2_msgs/TFMessage

Publishers: None

Subscribers: 
 * /slam_gmapping (http://YAB:37763/)

其订阅者是slam_gmapping，所以说这里的建图程序是需要这个tf_static话题消息的。 

查看其类型：rosmsg show tf2_msgs/TFMessage

geometry_msgs/TransformStamped[] transforms
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

上面记录全部数据，文件将会很大，也没有必要，所以这里我们只需要多记录一个话题：tf_static
rosbag record -O data_three.bag /scan /tf /tf_static
我们来看下生成的地图，查看地图命令：eog map.pgm 或者：rosrun rviz rviz(在rviz里添加map，选择map话题查看也可以)，如下图： 

查看地图元数据：cat map.yaml

image: map.pgm
resolution: 0.050000
origin: [-100.000000, -100.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3.5、改进地图质量

在Turtlebot的传感器，我们使用的其实是微软的Kinect深度相机获取的数据，而不是激光测距仪，这个假的激光测距仪比真的激光传感器量程小，视角小。slam_gmapping使用激光数据来估计机器人的运动，远距离，宽视角的数据将会使这个估计更准确。
我们可以更改slam_gmapping节点的参数来改进地图的质量：

rosparam set /slam_gmapping/angularUpdate 0.1
rosparam set /slam_gmapping/linearUpdate 0.1
rosparam set /slam_gmapping/lskip 10
rosparam set /slam_gmapping/xmax 10
rosparam set /slam_gmapping/xmin -10
rosparam set /slam_gmapping/ymax 10
rosparam set /slam_gmapping/ymin 10

这些命令改变了每次旋转angularUpdate和移动linearUpdate多远才进行一次新的扫描，在处理每个LaserScan消息的时候跳过多少条光线(lskip)以及地图延伸的长度(xmin,xmax,ymin,ymax)
这些参数的改动仅仅影响slam_gmapping，所以你可以使用上述采集的数据，而不需要重新采集数据，这也是用rosbag来记录然后回放来创建地图的一个好处。

参数的一些用法：

rosparam list：列出当前所有参数
rosparam get xxx：显示xxx参数值
rosparam set xxx v：设置xxx参数值v
rosparam dump file_name：保存参数到文件
rosparam load file_name：从文件读取参数
rosparam delete xxx：删除xxx参数

4、全新安装 

对于一些朋友，如果没有安装上述包的情况，这里使用开源的turtlebot3来全新安装：

4.1、建立工作区间 

mkdir -p ~/mybot3_ws/src
cd ~/mybot3_ws/src
catkin_init_workspace

4.2、编译

cd ..
catkin_make
echo "source ~/mybot3_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

4.3、下载包

cd ~/mybot3_ws/src
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git

4.4、再次编译

cd ..
catkin_make

4.5、缺失包

这里出错，摘要如下：

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "turtlebot3_msgs"
  with any of the following names:

    turtlebot3_msgsConfig.cmake
    turtlebot3_msgs-config.cmake
Invoking "cmake" failed

缺少turtlebot3_msgs包，我们继续下载和编译。

cd ~/mybot3_ws/src
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git

如果不是科学上网，出现错误：

gnutls_handshake() failed: The TLS connection was non-properly terminated.
取消代理：

git config --global --unset https.https://github.com.proxy 
git config --global --unset http.https://github.com.proxy

如果还不行可以删除配置文件：rm -rf ~/.gitconfig

更多关于包安装可以查阅：ROS新建工作区(workspace)与包(package)编译的实践(C++示例)

4.6、创建地图

再次编译没有问题之后，我们就开始创建地图与导航操作(温馨提示：多双击tab键来自动键入)：
开启gazebo仿真机器人
roslaunch turtlebot3_gazebo turtlebot3_world.launch
开启地图
roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping
开启键盘控制机器人，行驶中就创建地图
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
如下图：

最后可以保存为一张地图:rosrun map_server map_saver -f ~/map

如下图：

5、导航

5.1、加载地图

地图创建好了之后，我们来看下机器人在地图中的导航情况：

roslaunch turtlebot3_gazebo turtlebot3_world.launch
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

出现如下错误，摘要如下：

ERROR: cannot launch node of type [amcl/amcl]: amcl
ERROR: cannot launch node of type [move_base/move_base]: move_base

AMCL(Adaptive Monte Carlo Localization)自适应蒙特卡罗定位方法，常用于机器人定位和导航，安装命令(根据ROS版本来决定)
sudo apt-get install ros-melodic-navigation

5.2、版本的命令

lsb_release -a

No LSB modules are available.
Distributor ID:	Ubuntu
Description:	Ubuntu 18.04.6 LTS
Release:	18.04
Codename:	bionic

ROS版本：

printenv ROS_DISTRO
melodic

仿真机器人的版本：

gazebo -version
Gazebo multi-robot simulator, version 9.0.0

5.3、完整版的tf树

rosrun rqt_tf_tree rqt_tf_tree
如下图： 

5.4、手动导航

点击rviz菜单栏的“2D Nav Goal”，我们可以在地图中选择需要机器人行驶到那的目的地，然后机器人就会通过规划路径行驶了，如下图：

这个导航包的原理如下：

1、导航的目的地，就是点击的那个“2D Nav Goal”，会被发送到导航软件中，我们进行一个ROS行为调用(action)传入一个MoveBaseGoal类型的参数表示这个目的地。这个类型的结构，我们来查看下：rosmsg info MoveBaseGoal

[move_base_msgs/MoveBaseGoal]:
geometry_msgs/PoseStamped target_pose
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

可以看到这个参数就是某个坐标系(通常是map坐标系)中的位姿(位置position和方向orientation)。

2、全局规划器(Global Planner)使用路径规划算法和地图，规划一条从当前位置到目标位置的最短路径

3、这条路径传入到本地规划器(Local Planner)中，本地规划器会试图驱动机器人沿着这条路径走。本地规划器使用传感器的信息绕开挡在机器人前面却不在地图中的障碍(比如行走的人)，如果本地规划器被卡住了无法规划路径，它会请求全局规划器划一条新的路径，并尝试重新跟随这条路径。

4、当机器人接近目的位姿的时候，这个行为调用就完成了。

5.5、代码导航

上述的手动导航没有问题了，我们最后来看下自己写一个节点来导航，同样的启动机器人与加载导航地图

roslaunch turtlebot3_gazebo turtlebot3_world.launch
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

我们来到turtlebot3_navigation这个导航包下面，初始化一个节点

cd ~/mybot3_ws/src/turtlebot3/turtlebot3_navigation
gedit patrol.py

#!/usr/bin/env python
import rospy
import actionlib

from move_base_msgs.msg import MoveBaseAction,MoveBaseGoal

waypoints=[[(-2.5,-2.2,0),(0,0,0,1)],[(6.5,4.43,0),(0,0,-1.2,1.5)]]

def goal_pose(pose):
    goal_pose=MoveBaseGoal()
    goal_pose.target_pose.header.frame_id='map'
    goal_pose.target_pose.pose.position.x=pose[0][0]
    goal_pose.target_pose.pose.position.y=pose[0][1]
    goal_pose.target_pose.pose.position.z=pose[0][2]

    goal_pose.target_pose.pose.orientation.x=pose[1][0]
    goal_pose.target_pose.pose.orientation.y=pose[1][1]
    goal_pose.target_pose.pose.orientation.z=pose[1][2]
    goal_pose.target_pose.pose.orientation.w=pose[1][3]
    return goal_pose

if __name__=='__main__':
    rospy.init_node('patrol')
    client=actionlib.SimpleActionClient('move_base',MoveBaseAction)
    client.wait_for_server()
    while True:
        for pose in waypoints:
            goal=goal_pose(pose)
            client.send_goal(goal)
            client.wait_for_result()

代码比较简单，首先定义导航点waypoints用来规划路径使用，然后建一个函数goal_base将这些导航点转换成MoveBaseGoal，最后就是写一个动作客户端，等待服务端就绪，在导航点之间循环，每个点都作为目的地发送给导航软件。
加个可执行权限：chmod u+x patrol.py
我们执行节点：rosrun turtlebot3_navigation patrol.py
就可以看到机器人在地图中行驶了。
关于行为调用(动作)的更多用法，有兴趣的可以查阅：ROS通信机制之动作(Action)服务的实践