cartographer+pixhawk飞控+px4固件+ros kinetic+HOKUYO utm-30lx实现无GPS下的起飞

前面的博客讲了cartographer和mavros的安装，下面进行无人机室内定位的教程。简单点说就只要把cartographer的位置信息传入飞控后进行融合就可以了。
我用的计算机平台是tx2。cartographer为有IMU的2d版。

cartographer配置

cartographer的位置是在tf里面的，我们如果要用它的位置信息的话只需要获取base_link的位置信息即可，安装下面的包可以简单的解决这个问题，这个包的作用就是发布base_link在map下的位姿信息。

sudo apt install ros-kinetic-robot-pose-publisher

launch文件建立

在工作空间的install_isolated/share/cartographer_ros/launch这个目录下建立一个新的launch文件用来运行针对无人机室内定位的cartographer，用来启动雷达和cartographer，然后再把位姿信息发布出去。

gedit ~/cartographer_ws/install_isolated/share/cartographer_ros/launch/hokuyo_2d.launch

把下面的内容粘贴进去过后保存退出。

<launch>

  <node name="urg_node" pkg="urg_node" type="urg_node" respawn="true" output="screen">
    <param name="calibrate_time" value="true"/>
    <param name="serial_port" value="/dev/sensors/hokuyo_H1747925"/>
 </node>
 
  <param name="/use_sim_time" value="false" />

  <node name="cartographer_node"
    pkg="cartographer_ros"
    type="cartographer_node"
    args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename hokuyo_2d.lua"
    output="screen">
    <remap from="imu" to="mavros/imu/data_raw" />
  </node>

  <node name="cartographer_occupancy_grid_node"
    pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" />

  <node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher" respawn="true" output="screen" >
    <param name ="is_stamped" value = "true" />
    <param name ="publish_frequency" value = "20" />
    <remap from="/robot_pose" to="/mavros/vision_pose/pose"/> 
  </node>

  <node pkg="tf" type="static_transform_publisher" name="join_laser" args="0 0 0 0 0 0 base_link laser 10" />
  <node pkg="tf" type="static_transform_publisher" name="join_imu" args="0 0 -0.15 0 0 0 laser imu 10" />
</launch>

更改配置文件

配置文件在工作空间的下面这个目录

install_isolated/share/cartographer_ros/configuration_files/

添加一个新的配置文件

gedit ~/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files/hokuyo_2d.lua 

然后把下面的内容粘贴进去过后保存退出。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {

    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    map_frame = "map",
    tracking_frame = "base_link",
    published_frame = "base_link",
    odom_frame = "odom",
    provide_odom_frame = true,
    use_odometry = false,
    use_nav_sat = false,
    use_landmarks = false,
    publish_frame_projected_to_2d = false,
    num_laser_scans = 1,
    num_multi_echo_laser_scans = 0,
    num_subdivisions_per_laser_scan = 1,
    rangefinder_sampling_ratio = 1,
    odometry_sampling_ratio = 1,
    fixed_frame_pose_sampling_ratio = 1,
    imu_sampling_ratio = 1,
    landmarks_sampling_ratio = 1,
    num_point_clouds = 0,
    lookup_transform_timeout_sec = 0.2,
    submap_publish_period_sec = 0.3,
    pose_publish_period_sec = 5e-3,
    trajectory_publish_period_sec = 30e-3,
}

MAP_BUILDER.use_trajectory_builder_2d = true

return options

到此cartographer这边就准备完成了。

飞控设置

在飞控上面的设置就相对简单很多了。
更参数EKF2_AID_MASK把其它的参数都取消掉，然后勾选 vision position fusion选项，就代表飞控会融合雷达传入的位置信息了。接下来根据雷达相对飞控的位置设置参数EKF2_EV_POS_X、EKF2_EV_POS_Y、EKF2_EV_POS_Z。这个坐标系是px4定义的。
然后根据日志可以设置EKF2_EV_DELAY。到此就设置完成了。

测试

打开第一个终端先运行mavros，应为cartographer用了飞控的IMU所以要先打开mavros。

roslaunch mavros px4.launch

然后打开第二个终端运行cartographer

roslaunch cartographer_ros hokuyo_2d.launch

如果没有报错就标示ok，现在切换一下position模式，可以正常切换标示没有问题。
可以先静态的手拿着测一下数据后在实物试飞。