修改cartographer源代码使其发布ros标准格式的占据栅格地图

	在2d激光slam开源方案中，cartographer因为性能优越已经成为首选方案，目前来说，低成本的雷达和相对低配的嵌入式开发板已经能够运行整个一套的slam、location和planning。

1、cartographer存在的不方便的地方

	cartographer的occupancy grid格式和gmapping、hector等不一样，这在我的后续其他功能开发的时候遇到了问题，因此需要对cartographer发布occupancy grid的源代码进行修改，使其能够发布ros标准格式的地图。
	注：（1）cartographer发布的这种格式应该有其考虑的地方，这肯定不是cartographer的bug或者说cartographer的缺点，毕竟谷歌的工程师，不知道比我高到哪里去了;
	（2）cartographer建图的launch文件中，有一个occupancy grid node，我能力很差，不是很明白这个节点是否必须，因为我记得之前测试的时候我好像注释过这里，对结果好像没什么影响？期望大佬们能指点一下这里。

2、参考的资料

https://blog.csdn.net/xiekaikaibing/article/details/80090298?utm_medium=distribute.pc_relevant.none-task-blog-title-1&spm=1001.2101.3001.4242
https://blog.csdn.net/xiekaikaibing/article/details/80366908
https://blog.csdn.net/xiaoma_bk/article/details/82963731
https://blog.csdn.net/zhao_ke_xue/article/details/87875984?utm_medium=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.channel_param&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.channel_param
上面四个是讲解cartographer生成ros标准格式的博客，我基本上是按照他们的思路来的，只是在代码里面的判断标准和他们略有一点点小差距。
https://zhuanlan.zhihu.com/p/21738718
https://blog.csdn.net/zhao_ke_xue/article/details/109040676
上面两个是讲解占据栅格地图的数学原理的，非常棒，还有测试代码。
推荐大家去看一下上面六个。

3、测试地图数据方法

3.1gmapping等的栅格地图数据格式测试方法

可以用turtlebot3、husky、中科院仿真等在gazebo中进行仿真，也可以用你自己的机器人，都可以。
启动gmapping、hector、karto等经典的2dslam建图，然后新开一个终端，输入：

rostopic echo /map

然后大家会发现，终端打印出的/map的数据，里面只有-1,0,100.

3.2cartographer地图数据格式测试方法

和3.1一样，运行cartographer官方2d数据集或者跑你自己的机器人，启动cartographer建图之后，同样新终端

rostopic echo /map

，会发现，/map的数据是

-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 50, 49, 47, 47, 47, 46, 46, 45, 47, 47, 49, 51, 53, 48, 48, 48, 48, 45, 45, 45, 46, 49, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 50, 49, 50, -1, -1, -1, -1, -1, -1, -1, -1, 50, 49, 48, 46, 46, 44, 43, 43, 43, 43, 43, 41, 38, 39, 37, 37, 36, 34, 31, 31, 27, 20, 18, 20, 22, 22, 22, 22, 20, 19, 16, 18, 24, 26, 29, 29, 28, 25, 20, 16, 11, 7, 8, 11, 13, 13, 9, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 73, 43, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 49, 81, 89, 93, 73, 51, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 51, 89, 99, 31, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 19, 92, 99, 83, 50, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 50, 48, 48, 48, 49, 47, 45, 45, 46, 45, 45, 46, 46, 48, 48, 48, 49, 54, 52, 45, 41, 35, 32, 26, 22, 17, 11, 9, 10, 11, 13, 11, 4, 1, 1, 37, 97, 89, 5, 0, 0, 0, 0, 10, 13, 0, 0, 0, 0, 0, 0, 0, 7, 10, 20, 40, 73, 60, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 37, 88, 91, 77, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, , 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 13, 43, 38, 0, 0, 63, 90, 97, 91, 83, 78, 80, 89, 96, 93, 93, 96, 99, 99, 85, 53, 46, 51, 52, 49, 49, 51, 52, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 49, 61, 72, 47, 8, 0, 0, 0, 0, 0, 0, 5, 8, 10, 11, 10, 11, 13, 13, 14, 16, 18, 21, 23, 25, 26, 26, 28, 33, 36, 38, 41, 43, 45, 47, 49, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 50, 48, 47, 47, 47, 46, 47, 48, 48, 49, -1, -1, -1, -1, -1, -1, -1,, 43, 43, 42, 40, 40, 41, 40, 40, 42, 45, 46, 47, 47, 47, 45, 45, 47, 45, 44, 44, 45, 46, 46, 46, 47, 48, 48, 48, 49, 49, 50, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 50, 49, 49, 49, 49, 48, 48, 49, 49, 49, 49, 49, 49, 49, 49, 49, 49, 50, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,

我截取了一部分，基本上没有100。

3.3占据栅格地图的原理

此处借鉴https://github.com/HaoQChen/map_server的总结，ros标准格式中free、occupied、unknown三种状态分别是0,100.-1;cartographer对应的则是0-N、M-100、-1&N-M

4、cartographer源码修改

我的版本是cartographer1.0.0,我去看过，0.3.0的也一样。在src/cartographer_ros/cartographer_ros/下找到occupancy_grid_node_main.cc，大概161-165行左右，有一个函数：

std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
      painted_slices, resolution_, last_frame_id_, last_timestamp_);
  occupancy_grid_publisher_.publish(*msg_ptr);

其中，CreateOccupancyGridMsg（）是关于栅格地图的数据发布，同样在这个路径下，找到msg_conversion.cc，打开，找到338行左右：

     const int value =
          observed == 0
              ? -1
              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
      CHECK_LE(-1, value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }

  return occupancy_grid;
}

把其更换为我的：

 int value_temp = ::cartographer::common::RoundToInt((1. - color / 255.) * 100. );
      if (value_temp > 50)
      {
        value_temp = 100;
      }
      //else if (value_temp < 10)
      //{
        //value_temp = 0;
      //}
      else
      {
        value_temp = 0;
        //value_temp += 0;
      }
      const int value = observed ==0 ? -1 : value_temp;
      CHECK_LE(-1,value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }
  return occupancy_grid; 
} 

编译即可。

我的测试结果

不影响整个算法的建图结果（起码直观上），/map的数据格式只有-1.0,100，并且我把修改后代码建立的地图用到navigation上，没有影响，导航正常进行。

cartographer保存地图的脚本文件

新建SavingMap.sh,打开，输入以下内容：

#!/bin/bash
rosservice call /finish_trajectory 0
sleep 5
rosservice call /write_state "{filename: '${HOME}/Downloads/map23.pbstream'}"
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/Downloads/map123 -pbstream_filename=${HOME}/Downloads/map123.pbstream -resolution=0.05

记得修改map123的名字。当你感觉cartographer建图ok的时候，在终端中输入：

sh SavingMap.sh

就可以在你的${HOME}/Downloads/路径下生成pgm格式的地图。