38 lines
1012 B
C++
38 lines
1012 B
C++
#include <costmap_2d/critical_layer.h>
|
|
#include <boost/dll/alias.hpp>
|
|
#include "shared.h"
|
|
|
|
PLUGINLIB_EXPORT_CLASS(costmap_2d::CriticalLayer, costmap_2d::Layer)
|
|
|
|
using costmap_2d::NO_INFORMATION;
|
|
using costmap_2d::LETHAL_OBSTACLE;
|
|
using costmap_2d::CRITICAL_SPACE;
|
|
|
|
namespace costmap_2d
|
|
{
|
|
|
|
CriticalLayer::CriticalLayer(){}
|
|
CriticalLayer::~CriticalLayer(){}
|
|
|
|
unsigned char CriticalLayer::interpretValue(unsigned char value)
|
|
{
|
|
// check if the static value is above the unknown or lethal thresholds
|
|
if(value >= *this->threshold_)
|
|
return CRITICAL_SPACE;
|
|
else
|
|
return NO_INFORMATION;
|
|
}
|
|
|
|
void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
|
{
|
|
if (!map_received_)
|
|
return;
|
|
|
|
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
|
|
if (!use_maximum_)
|
|
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
|
else
|
|
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
|
|
|
}
|
|
} |