二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base
专栏系列文章如下：
一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base_goldqiu的博客-CSDN博客
本次实验是利用gmapping只使用二维点云进行建图，利用move_base进行规划。
后期需要写一个三维点云实时转换栅格地图包（包可以实现选择性转换输出三维栅格和2.5-D高程栅格、二维栅格地图），进行gmapping的功能替换就行，里程计就不用改，3D-SLAM的里程计直接可以拿来用，接口一样的。
1.先实现gmapping建图：
a.开启速腾聚创 rs_lidar_16雷达驱动（跟之前一样）
b.使用pointcloud_to_laserscan包实现三维转二维（编写pointcloudtoscan.launch）
# target_frame: rslidar # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: -0.4
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 0.2
range_max: 100
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
c.源码下载gmapping包，这里不要用apt去下载，因为要修改源码（gmapping源码不支持2048以上的点，需要修改下）
git clone https://github.com/ros-perception/openslam_gmapping.git
git clone https://github.com/ros-perception/slam_gmapping.git在文件目录openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h文件中
#define LASER_MAXBEAMS 2048 设置为20480（比较大就行）
d.调用gmapping包和laser_scan_matcher包，并用tf包的static_transform_publisher进行gmapping需要的tf信息提供，又因为gmapping必须要里程计信息，而laser_scan_matcher包实现的是二维雷达前端里程计odom输出。编写demo_gmapping.launch：
#### set up data playback from bag #############################
#### publish an example base_link -> laser transform ###########
#### start gmapping ############################################
这里要注意：
跑bag这个标志为true，实物跑这个标志为false
实车效果：
2.再实现move_base路径规划
move_base的框图如下：
拷贝gmapping建图功能下面的功能包，包括雷达驱动、gmapping功能包到src。
github下载scout2小车驱动包：https://github.com/agilexrobotics/scout_ros.git
https://github.com/agilexrobotics/ugv_sdk.git
安装小车驱动依赖：
sudo apt install -y libasio-dev
sudo apt install -y ros-$ROS_DISTRO-teleop-twist-keyboard设置CAN-To-USB模块
sudo modprobe gs_usb第一次用小车驱动包则执行：
rosrun scout_bringup setup_can2usb.bash不是第一次则执行：
rosrun scout_bringup bringup_can2usb.bash键盘测试下小车运动，没问题则进行下一步：
github下载move_base包（集成在navigation）：
https://github.com/ros-planning/navigation.git
将之前仿真的robot_sim包拷贝到src。
因为用laser_scan_matcher功能包生成的雷达odom没有数据（不知原因），所以用了小车自带的里程计。
更改demo_mapping.launch如下：
#### set up data playback from bag #############################
这里要注意：
需要调整base_link到rslidar的旋转变换（小车坐标系和雷达坐标系），由于这边是Z轴转了90度，所有要更改参数为 args="0.0 0.0 0.0 -1.57 0.0 0.0 /base_link /rslidar 40"
调好的现象是odom的箭头跟小车运动方向一致。
先实现局部路径规划（局部路径规划调用了gmapping和move_base）：
编写path_planning.launch
更改costmap_common_params.yaml
#机器人几何参数，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状
obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物
footprint: [[-0.350, -0.465], [-0.350, 0.465], [0.350, 0.465], [0.350, -0.465]]
#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0
map_type: costmap
observation_sources: scan
scan: {sensor_frame: rslidar, data_type: LaserScan, topic: scan, marking: true, clearing: true}主要要更改无人车的尺寸、膨胀半径、话题消息名称、类型、frame等。
更改global_costmap_params.yaml：
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: true主要更改frame
更改local_costmap_params.yaml：
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.05主要更改frame
编写local_navigation.launch
测试局部路径规划，在局部代价地图中，点击目标点，小车会运动过去，同时局部代价地图中进行障碍的感知，小车会绕过突然出现的障碍。
然后实现全局路径规划（需要读取录制好的地图，开启amcl、move_base）：
编写amcl.launch
编写map_save.launch
先运行建图，建好图后运行map_save.launch，进行地图保存。
编写global_navigation.launch
运行后效果如下：
全局路径规划下，小车起始位置为红色，终点为蓝色，经过10分钟左右能到达终点。
暴露出的问题点：
- 由于使用的是小车自带里程计，所以建出的图会漂，后续要迭代的功能是用多传感器融合SLAM下的三维点云地图转换成二维栅格地图，且多传感器融合后能给出高精度里程计，保证图不漂。
- 全局路径规划下小车运动缓慢且前进时不断左右转甚至转圈，运动不够平滑，需要根据小车运动学优化规划算法。（或者还可能是小车运动控制的问题）
- 目前使用的ros包比较多且杂，传感器底层驱动和小车底层驱动包无须更改，SLAM、感知和规划都是用的开源包，但后续需要将SLAM、感知和规划部分重新写框架代码并整合成不同的三个功能包，接口之间方便互相调用，能够迭代升级不同算法，不断迭代升级小车的智能化水平。