二维运动规划

学习该示例前，需要学习 二维激光SLAM（HectorSLAM） （Cartographer同理，本文会略作说明）
也就是要把HectorSLAM安装
运动规划，可以是在事先已经建好的地图上进行，也可以一边建图一边规划，这篇博客主要讲一边建图一边规划，为什么不讲在已经建好的图上做规划，那当然是因为我没有把图建好😭。天知道为什么我的图是下面这样的啊😭

正常应该是下面这个图的样子啊😭

在已经建好的地图上规划参考https://www.yuque.com/xtdrone/manual_cn/2d_motion_planning
这里作者说

我们使用的是ROS的global_planner模块，该模块可以使用Dijkstra和A两种算法，理论上A算法计算复杂低且能求得与Dijkstra算法一样的结果，但实际我们发现ROS的global_planner的A算法有bug，有些时候无法寻找到可行解，因此我们使用的是Dijkstra算法。下面的两张图展示了A算法失效（前两张图）但是Dijkstra算法成功（第三张图）的例子。注意这不是A*算法本身的问题，而是程序的bug。

怎么选A*还是Dijkstra算法应该是在下图所示的文件中修改的，但我总觉得应该还有一个更细节的文件

下面这两个博客感觉可以学习了解一下
但是我还没看，静不下心
ROS导航系列(四)：全局路径规划器的参数配置分析
局部路径规划器teb_local_planner详解6：关于动态障碍物与常见问题

相关配置

安装ROS的规划库

sudo apt install -y ros-melodic-move-base \
ros-melodic-costmap-2d \
ros-melodic-dwa-local-planner \
ros-melodic-global-planner

把HectoSLAM的launch文件中自带的RVIZ启动注释掉（Cartographer在xtdrone_2d.launch中注释）

roscd hector_slam_launch/launch/
sudo gedit hector_slam_xtdrone.launch

  <!--<node pkg="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_xtdrone.rviz"/>-->

边建图边规划

修改HectorSLAM的配置文件

roscd hector_mapping/launch/
sudo gedit hector_mapping_xtdrone.launch

把发布地图服务改为true

<param name="advertise_map_service" value="true"/>

注释掉56行的话题名映射

<!--remap from="/map" to="/map_new"/-->

修改地图的尺寸

<arg name="map_size" default="2048"/>

修改建图的原点，这里的0.5是指占整个地图大小的比例

 <param name="map_start_x" value="0.5"/>
 <param name="map_start_y" value="0.5" />

修改激光雷达的测距距离，由于地图较大，把测距距离调大

gedit  ~/PX4_Firmware/Tools/sitl_gazebo/models/hokuyo_lidar/model.sdf

把64行的最大距离改为20（这里我打开发现其实就是20）

<max>20</max>

启动PX4仿真

cd ~/PX4_Firmware
roslaunch px4 indoor4.launch

启动二维激光SLAM

roslaunch hector_slam_launch hector_slam_xtdrone.launch

建立和PX4仿真的通信，同时发布位置真值

cd ~/XTDrone/communication
python multirotor_communication.py iris 0

将激光水平定位和高度真值数据通过MAVROS发给PX4

cd ~/XTDrone/sensing/slam/laser_slam/script
python laser_transfer.py iris 0 hector #或cartographer

启动运动规划（注意 2d_motion_planning.launch、move_base.launch和indoor4.yaml中的路径变量，要改成自己电脑对应的路径名,如下图所示）
eg:

cd ~/XTDrone/motion_planning/2d/launch
roslaunch 2d_motion_planning.launch

弹出rviz界面

这里可能出现报错，rviz界面闪退的情况
解决办法：注释掉2d_motion_planning.launch中 <node pkg="tf" type="static_transform_publisher" name="odom_to_map" args="0.0 0.0 0 0.0 0.0 0.0 /odom /map 40" />

控制无人机起飞，如果您是用的takeoff模式起飞，起飞后需要切换到offboard模式

cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel

注意，此时应把multirotor_keyboard_control.py关闭，否则它发的指令会和运动规划指令发生冲突。然后下图所示，给无人机一个2D Nav Goal，然后程序会自动规划路径，并控制无人机按照规划的路径飞去。

由于全局优化较为费时，因此期望速度更新频率较低，需要把最大速度设置的较小（如0.3m/s，在dwa_local_planner_params.yaml中），否则实际轨迹和期望轨迹有较大差距，甚至发生碰撞。
😭我这里就失败了

二维运动规划(边建图边规划)