Ａ_LOAM建图同时构建栅格地图(去除动态物体构建可行驶区域)（Lego_Loam也适用）

Ａ_LOAM构建三维点云一般只会用于定位，而小车的导航还是需要二维的栅格地图
本文是在使用Ａ_LOAM（loam系列都适用）构建三维地图的同时，构建概率栅格地图，并去除动态物体，最终输出二维的可行驶区域，用于后续的导航．

建三维点云以及栅格图后定位导航以及实时更新的二维的可行驶区域：

用构建三维点云地图定位，再实时进行可行驶区域

octomap_server的安装：

sudo apt-get install ros-kinetic-octomap-ros #安装octomap
sudo apt-get install ros-kinetic-octomap-msgs
sudo apt-get install ros-kinetic-octomap-server
 
sudo apt-get install ros-kinetic-octomap-rviz-plugins

octomap-server源码安装　下载到/src下，catkin_make
安装上这个插件以后你可以启动 rviz ,这时候点开Add选项，会多一个octomap_rviz_plugins模组

写一个launch文件去启动 octomap_server ,创建 test.launch 文件

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
 
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.1" />
 
    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="/camera_init" />
 
    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="50.0" />
    <param name="latch" value="true" />
 
    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1.6" />
    <param name="pointcloud_min_z" value="0.8" />
 
    <param name="ground_filter_angle" value="3.14" />
 
    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="cloud_in" to="laser_cloud_map" />
 
  </node>
 
</launch>

注意：frame_id改成了与A_LOAM一致：/camera_init　　点云数据来自A_LOAM：/laser_cloud_map
启动：

roslaunch test test.launch

会发布（octomap_*）这五个节点信息：

最后在rviz 中添加一个“OccupancyMap” 模块. 设置 topic 为"/octomap_full",即可以得到如下结果:

保存二维栅格地图：（节点是/projected_map）（这个地图格式可以使用到gampping）

rosrun map_server map_saver  map:=/projected_map -f ~/map

正如图中所示，有乱七八糟的黑点，是因为，误将地面当障碍物了，解决办法:（调整获得的点云与地面之间的高度）建议取小车高度左右的值：

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1.6" />
    <param name="pointcloud_min_z" value="0.8" />

保存octotree　（.bt格式　便于定位导航时导入栅格地图）
cd map/ rosrun octomap_server octomap_saver -f tree.bt　　　或者（.ot）

使用：
一是在定位或者导航的launch　文件中添加导入.bt文件

  <node pkg="octomap_server" type="octomap_server_loc" name="octomap_server" output="screen" args="/home/．．．/map/tree.bt">
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.1" />
    <!-- name of the fixed frame, needs to be "map" for SLAM  camera_init (aloam)   -->
    <param name="frame_id" type="string" value="map" />
    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="50" />
    <param name="latch" value="true" />
    <!-- data source to integrate (PointCloud2), we modified octomap to support 3 point cloud inputs-->
    <param name="filter_ground" value="False" />
    <param name="filter_speckles" type="bool" value="true" />
    <param name="ground_filter/distance" type="double" value="0.2" />
    <param name="ground_filter/plane_distance" type="double" value="0.2" />
    <!--param name="occupancy_min_z" type="double" value="1.0" / -->
    <!--param name="occupancy_max_z" type="double" value="2.5" / -->
    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="100" />
    <param name="pointcloud_min_z" value="-100" />

     <!-- max/min reload 0.12 0.97 -->
    <param name="sensor_model_min" value="0.12" />
    <param name="sensor_model_max" value="0.97" />
    <!-- miss/hit reload 0.4  0.7-->
    <param name="sensor_model_miss" value="0.4" />
    <param name="sensor_model_hit" value="0.7" />

二是在定位或者导航过程中octomap-server的octomap_server.cpp中添加.bt文件：（这样有个好处就是，栅格地图是实时导入更新的，不会在导航时进行累积）

OcTree* octree = new OcTree("/home/．．．/map/tree.bt");
m_octree = octree