47 lines
1.3 KiB
C++
47 lines
1.3 KiB
C++
#include <costmap_2d/critical_layer.h>
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
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)
|
|
{
|
|
// printf("TEST PLUGIN CRITICAL\n");
|
|
// 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)
|
|
{
|
|
// printf("TEST PLUGIN CRITICAL\n");
|
|
if (!map_received_)
|
|
return;
|
|
|
|
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
|
|
if (!use_maximum_)
|
|
StaticLayer::CostmapLayer::updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
|
else
|
|
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
|
|
|
}
|
|
|
|
// Export factory function
|
|
static PluginStaticLayerPtr create_critical_plugin() {
|
|
return std::make_shared<CriticalLayer>();
|
|
}
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(create_critical_plugin, create_plugin_static_layer)
|
|
|
|
|
|
} |