35 lines
945 B
C++
35 lines
945 B
C++
#include <costmap_2d/preferred_layer.h>
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
using costmap_2d::NO_INFORMATION;
|
|
using costmap_2d::FREE_SPACE;
|
|
using costmap_2d::LETHAL_OBSTACLE;
|
|
using costmap_2d::PREFERRED_SPACE;
|
|
|
|
namespace costmap_2d
|
|
{
|
|
|
|
PreferredLayer::PreferredLayer(){}
|
|
PreferredLayer::~PreferredLayer(){}
|
|
|
|
unsigned char PreferredLayer::interpretValue(unsigned char value)
|
|
{
|
|
printf("TEST PLUGIN !!!\n");
|
|
// check if the static value is above the unknown or lethal thresholds
|
|
if(value == 0) return NO_INFORMATION;
|
|
else if (value >= *this->threshold_)
|
|
return PREFERRED_SPACE;
|
|
|
|
double scale = (double) value / *this->threshold_;
|
|
return scale * LETHAL_OBSTACLE;
|
|
}
|
|
|
|
// Export factory function
|
|
static std::shared_ptr<Layer> create_preferred_plugin() {
|
|
return std::make_shared<PreferredLayer>();
|
|
}
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(create_preferred_plugin, create_preferred_layer)
|
|
|
|
} |