在移动机器人导航模式下,cartographer与move_base联合使用,其中move_base利用map_sever加载之前已经建好的地图,并且地图上存在后期的认为标注,比如电子围栏等。
在纯定位模式下,cartographer如果持续发布地图,就会影响map_sever加载的地图效果,最终影响路径规划用的地图。
本文描述内容主要解决以上问题。
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);
}