Ａ_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