沉默杀手HomeArchivesTagsCategories
服务 clear_costmaps
|路径规划代价地图|
Word count: 364|Reading time: 1 min

服务/move_base/clear_costmaps，类型std_srvs/Empty

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// 定义在 MoveBase构造函数
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

// 回调函数，其实就是两个代价地图重置所有层
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
{
    //clear the costmaps
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner( *(planner_costmap_ros_->getCostmap()->getMutex())  );
    planner_costmap_ros_->resetLayers();
    return true;
}

// 主要是 Costmap2D::resetMap 和 Layer::reset()
void Costmap2DROS::resetLayers()
{
	Costmap2D* top = layered_costmap_->getCostmap();
	top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());

	std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
	for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); 
				plugin != plugins->end();  ++plugin)
	{
      (*plugin)->reset();    //每一层覆盖Layer的虚函数reset()
	}
}

先看重置代价地图，这个简单，就是把地图的char值重置为默认值0 FREE_SPACE

1
2
3
4
5
6
7
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
	boost::unique_lock<mutex_t> lock(*(access_) );
	unsigned int len = xn - x0;
	for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
		memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

reset 所有层

三个层的reset()函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void InflationLayer::reset() { onInitialize(); }

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

void StaticLayer::reset()
{
  if (first_map_only_)
  {
    has_updated_data_ = true;
  }
  else
  {
    onInitialize();
  }
}

这里障碍层的把栅格设置为默认值，取决于参数track_unknown_space，但这个参数一般设置为true，所以栅格的默认值是 NO_INFORMATION

最好不要高频率call clear_costmaps服务。一般是周期性调用，防止代价地图上有太多的garbage