Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <nav_core_adapter/costmap_adapter.h>
00036 #include <nav_core2/exceptions.h>
00037 #include <pluginlib/class_list_macros.h>
00038 #include <string>
00039
00040 PLUGINLIB_EXPORT_CLASS(nav_core_adapter::CostmapAdapter, nav_core2::Costmap)
00041
00042 namespace nav_core_adapter
00043 {
00044
00045 nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROS* costmap_ros)
00046 {
00047 nav_grid::NavGridInfo info;
00048 costmap_2d::Costmap2D* costmap = costmap_ros->getCostmap();
00049 info.width = costmap->getSizeInCellsX();
00050 info.height = costmap->getSizeInCellsY();
00051 info.resolution = costmap->getResolution();
00052 info.frame_id = costmap_ros->getGlobalFrameID();
00053 info.origin_x = costmap->getOriginX();
00054 info.origin_y = costmap->getOriginY();
00055 return info;
00056 }
00057
00058 CostmapAdapter::~CostmapAdapter()
00059 {
00060 if (needs_destruction_)
00061 {
00062 delete costmap_ros_;
00063 }
00064 }
00065
00066 void CostmapAdapter::initialize(costmap_2d::Costmap2DROS* costmap_ros, bool needs_destruction)
00067 {
00068 costmap_ros_ = costmap_ros;
00069 needs_destruction_ = needs_destruction;
00070 info_ = infoFromCostmap(costmap_ros_);
00071 costmap_ = costmap_ros_->getCostmap();
00072 }
00073
00074 void CostmapAdapter::initialize(const ros::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
00075 {
00076 initialize(new costmap_2d::Costmap2DROS(name, *tf), true);
00077 }
00078
00079 nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
00080 {
00081 return costmap_->getMutex();
00082 }
00083
00084 void CostmapAdapter::reset()
00085 {
00086 costmap_->resetMap(0, 0, costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
00087 }
00088
00089 void CostmapAdapter::update()
00090 {
00091 info_ = infoFromCostmap(costmap_ros_);
00092 if (!costmap_ros_->isCurrent())
00093 throw nav_core2::CostmapDataLagException("Costmap2DROS is out of date somehow.");
00094 }
00095
00096 void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
00097 {
00098 costmap_->setCost(x, y, value);
00099 }
00100
00101 unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int y) const
00102 {
00103 unsigned int index = costmap_->getIndex(x, y);
00104 return costmap_->getCharMap()[index];
00105 }
00106
00107 void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info)
00108 {
00109 throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
00110 }
00111
00112 void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
00113 {
00114 costmap_->updateOrigin(new_info.origin_x, new_info.origin_y);
00115 }
00116
00117 }