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