costmap_adapter.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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 }  // namespace nav_core_adapter


nav_core_adapter
Author(s):
autogenerated on Wed Jun 26 2019 20:09:49