Turtlebot2 导航过程中切换地图

在实验中过程探索机器人如何在导航过程中切换地图，有两种方法，第一种是直接在终端中运行 rosrun map_server map_server target_map.yaml就可以切换到想要的地图，第二种方法是修改map_server的主程序，即在主程序中加入一个关联容器map，注意此map非彼map，此map为C++程序中存储key-value键值的容器，存储每一个地图的id和与之对应的地图信息。

1、修改map_server主程序

主要修改如下：
（1）定义一个map容器

std::map< std::string, MapServer > msv;

（2）将地图加入容器
根据输入的地图参数(yaml文件)中提取map_id作为key，创建MapServer的对象作为value，并且加入到map容器中。默认加载的是第一个参数对应的地图信息。

try{
      for(int i=1; i < argc; i++) {
          MapServer ms = MapServer(argv[i],0.0);
          std::string id = ms.map_id;
          msv.insert(make_pair(id, ms));
          if(i==1) {
    	       current_map = id;
    	       msv.find(id)->second.SwitchPublisher(true);
        }
    }
  ros::spin();
  }
  catch(std::runtime_error& e){
      ROS_ERROR("map_server exception: %s", e.what());
      return -1;
  }

（3）订阅/map_reload话题，用于接收用户发布过来的的消息，回调函数切换相应的地图。

sub = n.subscribe("/map_reload", 1, MapSelect);

回调函数

//切换地图
void MapSelect(const std_msgs::StringConstPtr& msg) {
    if(msv.find(msg->data) != msv.end()) {
        current_map = msg->data;
        msv.find(current_map)->second.SwitchPublisher(true);
        ROS_INFO("map[%s] is available.", msg->data.c_str());
    }
}

修改的内容不止以上几个地方，还有许多细节，源代码请见链接: main.cpp，如果想直接使用，可以下载代码包链接: multi_map_server，直接编译即可。

2、建立两个地图

先建立两个地图，为了演示效果，我在两个实验室中建立了两张地图

（1）map1

在这里插入图片描述
map1.pgm
在这里插入图片描述

为了multi_map_server能够读取每张地图的id，要在创建好地图的yaml文件中加入该地图的id，比如在map1.yaml文件中的第一行加入map_id: map1。
map1.yaml

map_id: map1
image: /home/robot/map1/map1.pgm
resolution: 0.050000
origin: [-13.800000, -13.800000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

（2）map2

在这里插入图片描述
map2.pgm
在这里插入图片描述

在map2.yaml文件的第一行加入map_id: map2
map2.yaml:

map_id: map2
image: /home/robot/map2/map2.pgm
resolution: 0.050000
origin: [-13.800000, -13.800000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3、修改launch文件

在机器人导航的时候，要运行导航的launch文件，比如我的Turtlebot2的导航launch文件为laser_amcl_demo.launch，里面包含了启动雷达节点，map_server节点，amcl节点，代价地图以及move_base节点等。我们只需要把map_server节点改为自己的multi_map_server节点就可以了，前提是提前编译好multi_map_server功能包。
修改之前：

<!-- Map server -->
<!-- arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)" -->
<arg name="map_file" default="/home/robot/map/map.yaml" -->
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

修改之后

<!-- Map server -->
<!-- arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)" -->
<arg name="laboratory_map1" default="/home/robot/map1/map1.yaml" />
<arg name="laboratory_map2" default="/home/robot/map2/map2.yaml" />
<node name="multi_map_server" pkg="multi_map_server" type="multi_map_server" args="$(arg laboratory_map1) $(arg laboratory_map2)" />

之后启动launch文件，默认加载的地图是map1
在这里插入图片描述

然后使用命令rosropic list命令可以看到有/map_reload话题，然后输入rostopic pub /map_reload std_msgs/String "data: 'map2'"，就可以看到成功切换到map2了。
在这里插入图片描述

4、总结

这种方法比较简便的实现了地图的切换，可以使机器人在导航的过程到达某个固定的位置之后切换地图接着导航，这样就把一个非常大的地图分割成几个小的地图，减小建图的累积误差。当然，机器人切换地图的时候需要确定机器人在另一个地图中的位姿，这个是比较难的，我的方法是利用相机识别二维码来确定机器人的位姿，记录到下一篇文章吧。这篇文章只是我在实验过程中，探索机器人在导航过程中如何切换地图的记录，欢迎各位大佬指点！！！