XTDrone——二维运动规划

语雀

学习该示例前，需要学习 二维激光SLAM
运动规划，可以是在事先已经建好的地图上进行，也可以一边建图一边规划，本示例将用两个例子来说明。

一、配置

安装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"/>-->

二、在已经建好的地图上规划

1、修改配置文件

修改HectorSLAM的配置文件，使新发布的地图不会取代已经建好的地图

roscd hector_mapping/launch/
sudo gedit hector_mapping_xtdrone.launch

把发布地图服务改为false（Cartographer在xtdrone_2d.launch中注释cartographer_occupancy_grid_node节点）

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

取消注释56行的话题名映射，将map映射为map_new，从而不被订阅使用

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

（注意 2d_motion_planning.launch、move_base.launch和indoor3.yaml中的路径变量，要改成自己电脑对应的路径名）

注释掉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" /-->

把2d_motion_planning.launch里面的Map sever取消注释，并改成要用的地图路径。

2、启动仿真

启动PX4仿真

cd ~/PX4_Firmware
roslaunch px4 indoor3.launch

启动二维激光SLAM

roslaunch hector_slam_launch hector_slam_xtdrone.launch

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

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

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

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

启动运动规划

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

弹出rviz界面，显示建好的地图、代价地图（costmap）和激光扫描的障碍（绿线显示）

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

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

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

三、边建图边规划

本部分采用indoor4.world，这个场景较大且复杂，如果事先建图需要较大的工作量，这里选择边建图边规划。注意，由于cartographer默认程序建的地图的尺寸是逐渐变大的，运动规划的目标点很可能在未定义的区域，因此无法直接使用，需要修改其源代码从而预先分配较大的地图尺寸。如果有朋友做了此工作，欢迎提PR，这里我们以HectorSLAM为例进行讲解。

1、修改配置文件

修改HectorSLAM的配置文件

roscd hector_mapping/launch/
sudo gedit hector_mapping_xtdrone.launch

把发布地图服务改为true（Cartographer在xtdrone_2d.launch中注释cartographer_occupancy_grid_node节点）

<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

<max>20</max>

（注意 2d_motion_planning.launch、move_base.launch和indoor3.yaml中的路径变量，要改成自己电脑对应的路径名）

2、启动仿真

启动PX4仿真

cd ~/PX4_Firmware
roslaunch px4 indoor3.launch

启动二维激光SLAM

roslaunch hector_slam_launch hector_slam_xtdrone.launch

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

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

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

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

启动运动规划

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

弹出rviz界面

控制无人机起飞，如果您是用的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中），否则实际轨迹和期望轨迹有较大差距，甚至发生碰撞。

到此，二维运动规划仿真完成！祝贺您又完成了一个仿真！