XTDrone——无人机基于2D激光Lidar进行二维运动规划（细节提醒以及相关报错解决）

参考XTDrone文档：

二维激光SLAM（HectorSLAM） · 语雀

二维运动规划 · 语雀

相关ROS依赖库：

本地基于的是20.04的ubuntu系统：

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

把HectoSLAM的launch文件中自带的RVIZ启动注释掉：

roscd hector_slam_launch/launch/
sudo gedit hector_slam_xtdrone.launch

官网上只需要注释掉rviz显示部分，但是后面尝试只注释掉rviz发现，运行hector_slam_xtdrone.launch会报坐标系TF转化的错，所以添加TF树转换：

报错展示：

INFO] 1755748763.633934538: HectorSM p_laser_z_min_value_: -1.000000 INFO 1755748763.633955212: HectorSM p_laser_z_max_value_: 1.000000 INFO 1755748764.311158091, 595.524000000: lookupTransform base_stabilized to iris_0/laser_2d timed out. Could not transform laser scan into base_frame.

hector_slam_xtdrone.launch文件修改如下：

<?xml version="1.0"?>

<launch>

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

  <!-- <node pkg="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_xtdrone.rviz"/> -->
    
  <node pkg="tf" type="static_transform_publisher" name="laser_to_base" 
      args="0 0 0.1 0 0 0 base_stabilized iris_0/laser_2d 100" />
      
  <include file="$(find hector_mapping)/launch/hector_mapping_xtdrone.launch"/>

  <include file="$(find hector_imu_attitude_to_tf)/launch/hector_imu_xtdrone.launch"/>

</launch>

采用之前HectorSLAM建好的图进行规划：

相关细节见：二维激光SLAM（HectorSLAM） · 语雀

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

roscd hector_mapping/launch/
sudo gedit hector_mapping_xtdrone.launch

把发布地图服务改为false 并取消注释56行的话题名映射，将map映射为map_new，从而不被订阅使用：

修改launch文件如下：

<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_stabilized"/>
  <arg name="odom_frame" default="base_stabilized"/>
  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="/iris_0/scan"/>
  <arg name="map_size" default="512"/>
  
  <node pkg="tf" type="static_transform_publisher" name="odom_to_map_broadcaster" args="0 0 0 0 0 0 map odom 100" />

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    
    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />
    
    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
    
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.2"/>
    <param name="map_start_y" value="0.2" />
    <param name="map_multi_res_levels" value="2" />
    
    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />
    
    <!-- Advertising config --> 
    <param name="advertise_map_service" value="false"/>
    
    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>
    
    <!-- Debug parameters -->
    <!--
      <param name="output_timing" value="false"/>
      <param name="pub_drawings" value="true"/>
      <param name="pub_debug_output" value="true"/>
    -->
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />

    <remap from="/slam_out_pose" to="/iris_0/pose"/>
    <remap from="/map" to="/map_new"/>
  </node>
    
</launch>
  
  

启动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

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

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

启动运动规划：

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

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

然后控制无人机起飞，并悬停在一定的高度：

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

然后关掉控制无人机起飞的键盘控制话题，在2d_motion_planning.launch话题的rviz界面进行2D Nav goal****航点规划，观察无人机运行状况。

路径规划结果如下：