42 static const std::string
LOGNAME =
"lazy_free_space_updater";
47 , max_batch_size_(max_batch_size)
48 , max_sensor_delta_(1e-3)
49 , process_occupied_cells_set_(nullptr)
50 , process_model_cells_set_(nullptr)
51 , update_thread_([this] { lazyUpdateThread(); })
52 , process_thread_([
this] { processThread(); })
56 LazyFreeSpaceUpdater::~LazyFreeSpaceUpdater()
60 boost::unique_lock<boost::mutex> _(update_cell_sets_lock_);
61 update_condition_.notify_one();
64 boost::unique_lock<boost::mutex> _(cell_process_lock_);
65 process_condition_.notify_one();
67 update_thread_.join();
68 process_thread_.join();
75 (
long unsigned int)occupied_cells->size(), (
long unsigned int)model_cells->size());
76 boost::mutex::scoped_lock _(update_cell_sets_lock_);
77 occupied_cells_sets_.push_back(occupied_cells);
78 model_cells_sets_.push_back(model_cells);
79 sensor_origins_.push_back(sensor_origin);
80 update_condition_.notify_one();
83 void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap* occupied_cells,
octomap::KeySet* model_cells,
89 if (cell_process_lock_.try_lock())
91 process_occupied_cells_set_ = occupied_cells;
92 process_model_cells_set_ = model_cells;
93 process_sensor_origin_ = sensor_origin;
94 process_condition_.notify_one();
95 cell_process_lock_.unlock();
99 ROS_WARN_NAMED(
LOGNAME,
"Previous batch update did not complete. Ignoring set of cells to be freed.");
100 delete occupied_cells;
105 void LazyFreeSpaceUpdater::processThread()
107 const float lg_0 = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
108 const float lg_miss = tree_->getProbMissLog();
111 OcTreeKeyCountMap free_cells1, free_cells2;
118 boost::unique_lock<boost::mutex> ulock(cell_process_lock_);
119 while (!process_occupied_cells_set_ && running_)
120 process_condition_.wait(ulock);
126 "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells",
127 (
long unsigned int)process_occupied_cells_set_->size(),
128 (
long unsigned int)process_model_cells_set_->size());
138 for (std::pair<const octomap::OcTreeKey, unsigned int>& it : *process_occupied_cells_set_)
139 if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it.first), key_ray1))
141 free_cells1[jt] += it.second;
148 if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it), key_ray2))
156 for (std::pair<const octomap::OcTreeKey, unsigned int>& it : *process_occupied_cells_set_)
158 free_cells1.erase(it.first);
159 free_cells2.erase(it.first);
164 free_cells1.erase(it);
165 free_cells2.erase(it);
168 (
long unsigned int)(free_cells1.size() + free_cells2.size()));
176 tree_->updateNode(it, lg_0);
179 for (std::pair<const octomap::OcTreeKey, unsigned int>& it : free_cells1)
180 tree_->updateNode(it.first, it.second * lg_miss);
181 for (std::pair<const octomap::OcTreeKey, unsigned int>& it : free_cells2)
182 tree_->updateNode(it.first, it.second * lg_miss);
188 tree_->unlockWrite();
189 tree_->triggerUpdateCallback();
193 delete process_occupied_cells_set_;
194 process_occupied_cells_set_ =
nullptr;
195 delete process_model_cells_set_;
196 process_model_cells_set_ =
nullptr;
200 void LazyFreeSpaceUpdater::lazyUpdateThread()
202 OcTreeKeyCountMap* occupied_cells_set =
nullptr;
205 unsigned int batch_size = 0;
209 boost::unique_lock<boost::mutex> ulock(update_cell_sets_lock_);
210 while (occupied_cells_sets_.empty() && running_)
211 update_condition_.wait(ulock);
218 occupied_cells_set =
new OcTreeKeyCountMap();
220 occupied_cells_sets_.pop_front();
222 (*occupied_cells_set)[it]++;
224 model_cells_set = model_cells_sets_.front();
225 model_cells_sets_.pop_front();
226 sensor_origin = sensor_origins_.front();
227 sensor_origins_.pop_front();
231 while (!occupied_cells_sets_.empty())
233 if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_)
235 ROS_DEBUG_NAMED(
LOGNAME,
"Pushing %u sets of occupied/model cells to free cells update thread (origin changed)",
237 pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
241 sensor_origins_.pop_front();
245 (*occupied_cells_set)[it]++;
246 occupied_cells_sets_.pop_front();
249 model_cells_set->insert(mod_occ->begin(), mod_occ->end());
250 model_cells_sets_.pop_front();
255 if (batch_size >= max_batch_size_)
257 ROS_DEBUG_NAMED(
LOGNAME,
"Pushing %u sets of occupied/model cells to free cells update thread", batch_size);
258 pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
259 occupied_cells_set =
nullptr;