配置使用的是costmap_converter::CostmapToLinesDBSRANSAC，类继承关系:
1 | class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons |
这个机制太复杂了，还没有搞清楚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
29
30
31
32
33
34void workerCallback(const ros::TimerEvent&)
{
updateCostmap2D();
compute();
}
void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
occupied_cells_.clear();
if (!costmap_->getMutex())
{
ROS_ERROR("Cannot update costmap since the mutex pointer is null");
return;
}
int idx = 0;
costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
// get indices of obstacle cells
for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
{
for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
{
int value = costmap_->getCost(i,j);
if(value >= costmap_2d::LETHAL_OBSTACLE)
{
double x, y;
costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
occupied_cells_.push_back( KeyPoint( x, y ) );
}
++idx;
}
}
}