costmap_queue.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, 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 
35 #ifndef COSTMAP_QUEUE_COSTMAP_QUEUE_H
36 #define COSTMAP_QUEUE_COSTMAP_QUEUE_H
37 
38 #include <nav_core2/costmap.h>
41 #include <algorithm>
42 #include <limits>
43 #include <vector>
44 
45 namespace costmap_queue
46 {
51 class CellData
52 {
53 public:
62  CellData(const double d, const unsigned int x, const unsigned int y, const unsigned int sx, const unsigned int sy) :
63  distance_(d), x_(x), y_(y), src_x_(sx), src_y_(sy)
64  {
65  }
66 
71  distance_(std::numeric_limits<double>::max()), x_(0), y_(0), src_x_(0), src_y_(0)
72  {
73  }
74 
75  double distance_;
76  unsigned int x_, y_;
77  unsigned int src_x_, src_y_;
78 };
79 
99 class CostmapQueue : public MapBasedQueue<CellData>
100 {
101 public:
107  explicit CostmapQueue(nav_core2::Costmap& costmap, bool manhattan = false);
108 
112  void reset() override;
113 
119  void enqueueCell(unsigned int x, unsigned int y);
120 
127  CellData getNextCell();
128 
132  virtual int getMaxDistance() const { return std::max(costmap_.getWidth(), costmap_.getHeight()); }
133 
139  virtual bool validCellToQueue(const CellData& cell) { return true; }
140 
144  using Ptr = std::shared_ptr<CostmapQueue>;
145 protected:
149  void enqueueCell(unsigned int cur_x, unsigned int cur_y, unsigned int src_x, unsigned int src_y);
150 
154  void computeCache();
155 
157 
158  // This really should be VectorNavGrid<bool>, but since
159  // vector<bool> is wacky, it would result in compile errors.
162 protected:
177  inline double distanceLookup(const unsigned int cur_x, const unsigned int cur_y,
178  const unsigned int src_x, const unsigned int src_y)
179  {
180  unsigned int dx = std::abs(static_cast<int>(cur_x) - static_cast<int>(src_x));
181  unsigned int dy = std::abs(static_cast<int>(cur_y) - static_cast<int>(src_y));
182  return cached_distances_[dx][dy];
183  }
184  std::vector<std::vector<double> > cached_distances_;
186 };
187 } // namespace costmap_queue
188 
189 #endif // COSTMAP_QUEUE_COSTMAP_QUEUE_H
double distanceLookup(const unsigned int cur_x, const unsigned int cur_y, const unsigned int src_x, const unsigned int src_y)
Lookup pre-computed distances.
CellData()
Default Constructor - Should be used sparingly.
Definition: costmap_queue.h:70
virtual bool validCellToQueue(const CellData &cell)
Check to see if we should add this cell to the queue. Always true unless overridden.
nav_core2::BasicCostmap costmap
Definition: utest.cpp:43
std::vector< std::vector< double > > cached_distances_
A tool for finding the cells closest to some set of originating cells.
Definition: costmap_queue.h:99
virtual int getMaxDistance() const
Get the maximum x or y distance we&#39;ll need to calculate the distance between.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Templatized interface for a priority queue.
TFSIMD_FORCE_INLINE const tfScalar & x() const
nav_grid::VectorNavGrid< unsigned char > seen_
Storage for cell information used during queue expansion.
Definition: costmap_queue.h:51
nav_core2::Costmap & costmap_
CellData(const double d, const unsigned int x, const unsigned int y, const unsigned int sx, const unsigned int sy)
Real Constructor.
Definition: costmap_queue.h:62
std::shared_ptr< CostmapQueue > Ptr
convenience definition for a pointer


costmap_queue
Author(s):
autogenerated on Wed Jun 26 2019 20:06:08