map_based_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_MAP_BASED_QUEUE_H
36 #define COSTMAP_QUEUE_MAP_BASED_QUEUE_H
37 
38 #include <algorithm>
39 #include <map>
40 #include <stdexcept>
41 #include <utility>
42 #include <vector>
43 
44 namespace costmap_queue
45 {
60 template <class item_t>
62 {
63 public:
67  explicit MapBasedQueue(bool reset_bins = true) : reset_bins_(reset_bins), item_count_(0)
68  {
69  reset();
70  }
71 
75  virtual void reset()
76  {
77  if (reset_bins_ || item_count_ > 0)
78  {
79  item_bins_.clear();
80  item_count_ = 0;
81  }
83  }
84 
90  void enqueue(const double priority, item_t item)
91  {
92  // We keep track of the last priority we inserted. If this items priority matches the previous insertion
93  // we can avoid searching through all the bins.
94  if (last_insert_iter_ == item_bins_.end() || last_insert_iter_->first != priority)
95  {
96  last_insert_iter_ = item_bins_.find(priority);
97 
98  // If not found, create a new bin
99  if (last_insert_iter_ == item_bins_.end())
100  {
101  auto map_item = std::make_pair(priority, std::move(std::vector<item_t>()));
102 
103  // Inserts an item if it doesn't exist. Returns an iterator to the item whether it existed or was inserted.
104  std::pair<ItemMapIterator, bool> insert_result = item_bins_.insert(std::move(map_item));
105  last_insert_iter_ = insert_result.first;
106  }
107  }
108 
109  // Add the item to the vector for this map key
110  last_insert_iter_->second.push_back(item);
111  item_count_++;
112 
113  // Use short circuiting to check if we want to update the iterator
114  if (iter_ == item_bins_.end() || priority < iter_->first)
115  {
117  }
118  }
119 
126  bool isEmpty()
127  {
128  return item_count_ == 0;
129  }
130 
135  item_t& front()
136  {
137  if (iter_ == item_bins_.end())
138  {
139  throw std::out_of_range("front() called on empty costmap_queue::MapBasedQueue!");
140  }
141 
142  return iter_->second.back();
143  }
144 
148  void pop()
149  {
150  if (iter_ != item_bins_.end() && !iter_->second.empty())
151  {
152  iter_->second.pop_back();
153  item_count_--;
154  }
155 
156  auto not_empty = [](const typename ItemMap::value_type& key_val) { return !key_val.second.empty(); };
157  iter_ = std::find_if(iter_, item_bins_.end(), not_empty);
158  }
159 
160 protected:
161  using ItemMap = std::map<double, std::vector<item_t>>;
162  using ItemMapIterator = typename ItemMap::iterator;
163 
165 
167  unsigned int item_count_;
170 };
171 } // namespace costmap_queue
172 
173 #endif // COSTMAP_QUEUE_MAP_BASED_QUEUE_H
item_t & front()
Return the item at the front of the queue.
void enqueue(const double priority, item_t item)
Add a new item to the queue with a set priority.
typename ItemMap::iterator ItemMapIterator
void pop()
Remove (and destroy) the item at the front of the queue.
Templatized interface for a priority queue.
virtual void reset()
Clear the queue.
bool isEmpty()
Check to see if there is anything in the queue.
MapBasedQueue(bool reset_bins=true)
Default Constructor.
std::map< double, std::vector< CellData >> ItemMap


costmap_queue
Author(s):
autogenerated on Sun Jan 10 2021 04:08:29