0. 简介

最近群里有些老哥在问cartographer纯定位相关问题,网上已有的方法均已失效,这里作者研究了下cartographer相关的流程以及源码,给出了一种简单的解决策略。

1. 旧版cartographer_ros

launch文件的修改

在启动cartographer_occupancy_grid_node节点时,增加pure_localization参数。

<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05
                                                    -pure_localization 1" />

源文件的修改

cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc文件中

  • 增加纯定位参数
DEFINE_int32(pure_localization, 0, "Pure localization !");
  • 更改初始化函数,增加参数赋值
//Node::Node(const double resolution, const double publish_period_sec)
Node::Node(const int pure_localization,const double resolution, const double publish_period_sec) 
    : resolution_(resolution),
      pure_localization_(pure_localization),//kaikai.gao
  • 禁止地图话题发布:
void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
  if (submap_slices_.empty() || last_frame_id_.empty()) {
    return;
  }

  // 非常重要,逻辑不能出错,否则影响导航地图!
  if(pure_localization_ == 1) return;

  ::cartographer::common::MutexLocker locker(&mutex_);
  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
      painted_slices, resolution_, last_frame_id_, last_timestamp_);

  occupancy_grid_publisher_.publish(*msg_ptr);
}

在这里插入图片描述

2. 新版cartographer_ros

在仿真环境中使用最新版本的cartographer_ros的时候,注释以上代码,也不能实现地图不更新,大版本更新好像是在2020年11月左右,所以需要寻找其他的方法来解决这个问题。这里作者给出一个简单的解决方式。

launch项目文件配置

<launch>
  <param name="/use_sim_time" value="false" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/myself_robot.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_backpack_2d.lua
          -load_state_filename /home/spirit/maps/map.pbstream" 
      output="screen">
    <remap from="/points2" to="/rslidar_points" />
  </node>

  <node name="map_server" pkg="map_server" type="map_server" args="/home/spirit/maps/map.yaml" />

 <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

lua文件需要在建图的基础上添加回环参数和子图信息

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

include "backpack_3d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 100

return options

上面第一行引用的文件是 my_backpack_2d.lua,这个文件是backpack_2d.lua修改过来的,配置Cartographer在定位过程中的其它参数的,这里也建议新建一个my_backpack_2d.lua,用于区分建图时用的参数。
在这里插入图片描述

在打开rviz后,我们选择Submaps插件,然后可以看到在Submaps下存在两个图层,将Trajectory1勾选去掉即可。这样通过分层的形式能够有效避免实时扫描的结果对原图的干扰,从而实现有效的定位功能

3. 参考链接

https://blog.csdn.net/weixin_43259286/article/details/107226736
https://blog.csdn.net/wesigj/article/details/111334726