Turtlebot+Rplidar A2使用 hector 建图与导航，并获取机器人实时位姿

总结一学期ROS下使用激光雷达进行机器人导航定位的学习收获。这是本人的第一篇博客，如有错误地方，请指出。由于之前有些图片是手机拍摄的，并未截图，带来不便请谅解。
参考博客：
1．Turtlebot + Rplidar A2使用Gmapping建图与导航：
https://www.jianshu.com/p/4277243c786d
2. Ros—RPLIDAR A2激光雷达安装(hector_mapping算法建图同cartographer_ros建图对比)
https://blog.csdn.net/qq_40503771/article/details/87906730?utm_medium=distribute.pc_relevant.none-task-blog-title-1&spm=1001.2101.3001.4242
3. Rplidar A1/A2使用及Hector_SLAM建图
https://blog.csdn.net/NouriXiiX/article/details/102690064
4. Turtlebot + Rplidar A2使用Gmapping建图与导航（参考定位和导航部分）
https://blog.csdn.net/dbdxnuliba/article/details/89322205
5. 写python脚本订阅/amcl_pose坐标：
https://blog.csdn.net/groot_lee/article/details/79273097

环境

1、Rplidar A2 激光雷达；
2、工控机；
3、Ubuntu 16.04;
4、Turtlebot。

1．在电脑上安装Turtlebot 底盘驱动

1.1 安装 Turtlebot 底盘驱动

$ sudo apt-get update
$ sudo apt-get install ros-kinetic-turtlebot

1.2 安装 Turbot

新建名为 catkin_hector 的工作空间。

$ cd ~/catkin_hector/src
$ git clone https://github.com/ncnynl/turbot
$ cd ..
$ catkin_make

1.3 安装 turtlebot_apps

$ cd ~/catkin_hector/src
$ git clone https://github.com/ncnynl/turtlebot_apps
$ cd ..
$ catkin_make

1.4 测试底盘

安装完 Turtlebot 底盘驱动后，测试电脑是否可以驱动底盘。

1.4.1 检查端口：

检查是否有 kobuki 端口，打开一个终端：

$ ls /dev/kobuki

1.4.2 底盘启动测试：

启动底盘

$ roslaunch turbot_bringup minimal.launch

执行命令后，会听到一串开机音乐
新终端，执行命令

$ rosrun turbot_tools test_move

成功执行命令后，小车向前缓慢移动，按 Ctrl + C 即可中止。

2. 安装雷达驱动程序

2.1 安装雷达驱动

$ cd ~/catkin_hector/src
$ git clone https://github.com/robopeak/rplidar_ros.git
$ cd ..
$ catkin_make

2.2 设置 USB 口的权限

$ sudo gedit /etc/udev/rules.d/70-ttyusb.rules

在打开的界面中，输入

KERNEL=="ttyUSB[0-9]*", MODE="0666"

运行如下程序，查看权限，有两个 rw 就可以了

ls -l /dev |grep ttyUSB*

2.3 雷达启动测试

$ roslaunch rplidar_ros rplidar.launch

运行成功后如下，雷达开始旋转。
如果报错：
Error,cannot bind to the specicified serial port /dev/ttyUSB0。
是因为端口权限不够，给端口加权限：

$ sudo chmod +x /dev/ttyUSB0
$ sudo chmod 777 /dev/ttyUSB0

2.4 测试扫描界面

$ roslaunch rplidar_ros view_rplidar.launch

情况正常的情况下可以看见激光数据。

成功出现 rviz 中激光画面之后， 按 Ctrl+c 结束操作。

3.使用 hector_slam算法建图测试

3.1 安装 hector_slam 包

$ cd ~/catkin_hector/src
$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git
$ cd …
$ catkin_make

3.2 进入 launch 文件夹下,在文件夹下添加hector_mapping_demo.launch 文件：

$ cd src/rplidar_ros/launch/
$ touch hector_mapping_demo.launch
$ gedit hector_mapping_demo.launch

hector_mapping_demo.launch：

<launch>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"
output="screen">
<!-- Frame names -->
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.05"/>
<param name="map_size" value="2048"/><param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<param name="map_multi_res_levels" value="2" />
<param name="map_pub_period" value="2" />
<param name="laser_min_dist" value="0.4" />
<param name="laser_max_dist" value="5.5" />
<param name="output_timing" value="false" />
<param name="pub_map_scanmatch_transform" value="true" />
<!--<param name="tf_map_scanmatch_transform_frame_name"
value="scanmatcher_frame" />-->
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.7" />
<param name="map_update_distance_thresh" value="0.2"/>
<param name="map_update_angle_thresh" value="0.06" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
<param name="scan_topic" value="scan"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster"
args="0 0 0 0 0 0 /base_link /laser 100"/>
<node pkg="rviz" type="rviz" name="rviz"args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>
</launch>

3.3 建图

重新启动激光雷达：

$ roslaunch rplidar_ros rplidar.launch

再打开一个新的terminal，输入命令以测试Hector建图：

$ roslaunch rplidar_ros hector_mapping_demo.launch

成功观察到所建的图。
启动键盘控制机器人移动：

$ roslaunch turbot_teleop keyboard.launch

3.4 保存地图

安装map-server包
$ sudo apt-get install ros-kinetic-map-server
保存地图
$ rosrun map_server map_saver -f ~/my_map

4. 使用AMCL和move_base实现定位和导航

4.1 导航定位

先启动底盘:

$ roslaunch turbot_bringup minimal.launch

启动AMCL，指定地图：

$ roslaunch turbot_slam laser_amcl_demo.launch map_file:=/home/turtlebot/my_map.yaml

(navigation中laser中driver中rplidar_laser.launch中我修改了/dev/rplidar为/dev/ttyUSB0)
给端口权限

$ sudo chmod +x /dev/ttyUSB0
$ sudo chmod 777 /dev/ttyUSB0

再打开rviz：

$ roslaunch turbot_rviz nav.launch

4.2 获取机器人位置信息

查看话题

$ rostopic list

使用如下指令查看到位置

$ rostopic echo /amcl_pose

4.3 订阅机器人位姿，python实现

amcl_pose的数据类型：

新建工作空间，新建learning_topic包，新建scripts文件夹，新建pose_subscriber.py，订阅/amcl_pose:

#! /usr/bin/env python
#coding=utf-8
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
def PoseCallBack(msg):
	data=""
	#订阅到的坐标信息
	x = msg.pose.pose.position.x
	y = msg.pose.pose.position.y
	#订阅到的四元数的信息，用来表示朝向
	orien_z = msg.pose.pose.orientation.z
	orien_w = msg.pose.pose.orientation.w
	
	data = str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)
	rospy.loginfo(data)

def PoseSub():
	rospy.init_node('pose_sub',anonymous=False)
	#监控话题，并在回调函数中处理
	rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,PoseCallBack)
	rospy.spin()
if __name__=='__main__':
	try:
		PoseSub()
	except:
		rospy.loginfo("出错,退出中...")

编译及运行：

$ cd catkin_learning
$ catkin_make 
$ rosrun learning_topic pose_subscriber