lazy_free_space_updater.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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 OWNER 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 /* Author: Ioan Sucan */
36 
38 #include <ros/console.h>
39 
40 namespace occupancy_map_monitor
41 {
42 LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr& tree, unsigned int max_batch_size)
43  : tree_(tree)
44  , running_(true)
45  , max_batch_size_(max_batch_size)
46  , max_sensor_delta_(1e-3)
47  , // 1mm
48  process_occupied_cells_set_(NULL)
49  , process_model_cells_set_(NULL)
50  , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this))
51  , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this))
52 {
53 }
54 
56 {
57  running_ = false;
58  {
59  boost::unique_lock<boost::mutex> _(update_cell_sets_lock_);
60  update_condition_.notify_one();
61  }
62  {
63  boost::unique_lock<boost::mutex> _(cell_process_lock_);
64  process_condition_.notify_one();
65  }
66  update_thread_.join();
67  process_thread_.join();
68 }
69 
71  const octomap::point3d& sensor_origin)
72 {
73  ROS_DEBUG("Pushing %lu occupied cells and %lu model cells for lazy updating...",
74  (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size());
75  boost::mutex::scoped_lock _(update_cell_sets_lock_);
76  occupied_cells_sets_.push_back(occupied_cells);
77  model_cells_sets_.push_back(model_cells);
78  sensor_origins_.push_back(sensor_origin);
79  update_condition_.notify_one();
80 }
81 
83  const octomap::point3d& sensor_origin)
84 {
85  // this is basically a queue of size 1. if this function is called repeatedly without any work being done by
86  // processThread(),
87  // data can be lost; this is intentional, to avoid spending too much time clearing the octomap
88  if (cell_process_lock_.try_lock())
89  {
90  process_occupied_cells_set_ = occupied_cells;
91  process_model_cells_set_ = model_cells;
92  process_sensor_origin_ = sensor_origin;
93  process_condition_.notify_one();
94  cell_process_lock_.unlock();
95  }
96  else
97  {
98  ROS_WARN("Previous batch update did not complete. Ignoring set of cells to be freed.");
99  delete occupied_cells;
100  delete model_cells;
101  }
102 }
103 
105 {
106  const float lg_0 = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
107  const float lg_miss = tree_->getProbMissLog();
108 
109  octomap::KeyRay key_ray1, key_ray2;
110  OcTreeKeyCountMap free_cells1, free_cells2;
111 
112  while (running_)
113  {
114  free_cells1.clear();
115  free_cells2.clear();
116 
117  boost::unique_lock<boost::mutex> ulock(cell_process_lock_);
119  process_condition_.wait(ulock);
120 
121  if (!running_)
122  break;
123 
124  ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells",
125  (long unsigned int)process_occupied_cells_set_->size(),
126  (long unsigned int)process_model_cells_set_->size());
127 
129  tree_->lockRead();
130 
131 #pragma omp sections
132  {
133 #pragma omp section
134  {
135  /* compute the free cells along each ray that ends at an occupied cell */
136  for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(),
137  end = process_occupied_cells_set_->end();
138  it != end; ++it)
139  if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
140  for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end(); jt != end; ++jt)
141  free_cells1[*jt] += it->second;
142  }
143 
144 #pragma omp section
145  {
146  /* compute the free cells along each ray that ends at a model cell */
147  for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
148  it != end; ++it)
149  if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
150  for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end(); jt != end; ++jt)
151  free_cells2[*jt]++;
152  }
153  }
154 
155  tree_->unlockRead();
156 
157  for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(),
158  end = process_occupied_cells_set_->end();
159  it != end; ++it)
160  {
161  free_cells1.erase(it->first);
162  free_cells2.erase(it->first);
163  }
164 
165  for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
166  it != end; ++it)
167  {
168  free_cells1.erase(*it);
169  free_cells2.erase(*it);
170  }
171  ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size()));
172 
173  tree_->lockWrite();
174 
175  try
176  {
177  // set the logodds to the minimum for the cells that are part of the model
178  for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
179  it != end; ++it)
180  tree_->updateNode(*it, lg_0);
181 
182  /* mark free cells only if not seen occupied in this cloud */
183  for (OcTreeKeyCountMap::iterator it = free_cells1.begin(), end = free_cells1.end(); it != end; ++it)
184  tree_->updateNode(it->first, it->second * lg_miss);
185  for (OcTreeKeyCountMap::iterator it = free_cells2.begin(), end = free_cells2.end(); it != end; ++it)
186  tree_->updateNode(it->first, it->second * lg_miss);
187  }
188  catch (...)
189  {
190  ROS_ERROR("Internal error while updating octree");
191  }
192  tree_->unlockWrite();
193  tree_->triggerUpdateCallback();
194 
195  ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
196 
201  }
202 }
203 
205 {
206  OcTreeKeyCountMap* occupied_cells_set = NULL;
207  octomap::KeySet* model_cells_set = NULL;
208  octomap::point3d sensor_origin;
209  unsigned int batch_size = 0;
210 
211  while (running_)
212  {
213  boost::unique_lock<boost::mutex> ulock(update_cell_sets_lock_);
214  while (occupied_cells_sets_.empty() && running_)
215  update_condition_.wait(ulock);
216 
217  if (!running_)
218  break;
219 
220  if (batch_size == 0)
221  {
222  occupied_cells_set = new OcTreeKeyCountMap();
224  occupied_cells_sets_.pop_front();
225  for (octomap::KeySet::iterator it = s->begin(), end = s->end(); it != end; ++it)
226  (*occupied_cells_set)[*it]++;
227  delete s;
228  model_cells_set = model_cells_sets_.front();
229  model_cells_sets_.pop_front();
230  sensor_origin = sensor_origins_.front();
231  sensor_origins_.pop_front();
232  batch_size++;
233  }
234 
235  while (!occupied_cells_sets_.empty())
236  {
237  if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_)
238  {
239  ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size);
240  pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
241  batch_size = 0;
242  break;
243  }
244  sensor_origins_.pop_front();
245 
246  octomap::KeySet* add_occ = occupied_cells_sets_.front();
247  for (octomap::KeySet::iterator it = add_occ->begin(), end = add_occ->end(); it != end; ++it)
248  (*occupied_cells_set)[*it]++;
249  occupied_cells_sets_.pop_front();
250  delete add_occ;
251  octomap::KeySet* mod_occ = model_cells_sets_.front();
252  model_cells_set->insert(mod_occ->begin(), mod_occ->end());
253  model_cells_sets_.pop_front();
254  delete mod_occ;
255  batch_size++;
256  }
257 
258  if (batch_size >= max_batch_size_)
259  {
260  ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread", batch_size);
261  pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
262  occupied_cells_set = NULL;
263  batch_size = 0;
264  }
265  }
266 }
267 }
iterator begin()
std::tr1::unordered_map< octomap::OcTreeKey, unsigned int, octomap::OcTreeKey::KeyHash > OcTreeKeyCountMap
std::shared_ptr< OccMapTree > OccMapTreePtr
XmlRpcServer s
#define ROS_WARN(...)
std::deque< octomap::KeySet * > occupied_cells_sets_
LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size=10)
std::vector< OcTreeKey >::iterator iterator
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
void pushLazyUpdate(octomap::KeySet *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)
std::deque< octomap::KeySet * > model_cells_sets_
iterator end()
static WallTime now()
#define ROS_ERROR(...)
void pushBatchToProcess(OcTreeKeyCountMap *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)
#define ROS_DEBUG(...)


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23