costmap_adapter.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
36 #include <nav_core2/exceptions.h>
38 #include <string>
39 
41 
42 namespace nav_core_adapter
43 {
44 
46 {
48  costmap_2d::Costmap2D* costmap = costmap_ros->getCostmap();
49  info.width = costmap->getSizeInCellsX();
50  info.height = costmap->getSizeInCellsY();
51  info.resolution = costmap->getResolution();
52  info.frame_id = costmap_ros->getGlobalFrameID();
53  info.origin_x = costmap->getOriginX();
54  info.origin_y = costmap->getOriginY();
55  return info;
56 }
57 
58 CostmapAdapter::~CostmapAdapter()
59 {
60  if (needs_destruction_)
61  {
62  delete costmap_ros_;
63  }
64 }
65 
66 void CostmapAdapter::initialize(costmap_2d::Costmap2DROS* costmap_ros, bool needs_destruction)
67 {
68  costmap_ros_ = costmap_ros;
69  needs_destruction_ = needs_destruction;
70  info_ = infoFromCostmap(costmap_ros_);
71  costmap_ = costmap_ros_->getCostmap();
72 }
73 
74 void CostmapAdapter::initialize(const ros::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
75 {
76  initialize(new costmap_2d::Costmap2DROS(name, *tf), true);
77 }
78 
79 nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
80 {
81  return costmap_->getMutex();
82 }
83 
84 void CostmapAdapter::reset()
85 {
86  costmap_->resetMap(0, 0, costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
87 }
88 
89 void CostmapAdapter::update()
90 {
91  info_ = infoFromCostmap(costmap_ros_);
92  if (!costmap_ros_->isCurrent())
93  throw nav_core2::CostmapDataLagException("Costmap2DROS is out of date somehow.");
94 }
95 
96 void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
97 {
98  costmap_->setCost(x, y, value);
99 }
100 
101 unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int y) const
102 {
103  unsigned int index = costmap_->getIndex(x, y);
104  return costmap_->getCharMap()[index];
105 }
106 
107 void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info)
108 {
109  throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
110 }
111 
112 void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
113 {
114  costmap_->updateOrigin(new_info.origin_x, new_info.origin_y);
115 }
116 
117 } // namespace nav_core_adapter
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROS *costmap_ros)
ROSCONSOLE_DECL void initialize()
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
double getOriginX() const
boost::recursive_mutex mutex_t
double getOriginY() const
unsigned int getSizeInCellsY() const
unsigned int getSizeInCellsX() const
double getResolution() const
Costmap2D * getCostmap()
std::shared_ptr< tf::TransformListener > TFListenerPtr


nav_core_adapter
Author(s):
autogenerated on Wed Jun 26 2019 20:06:25