00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/depth_image_octomap_updater/lazy_free_space_updater.h>
00038 #include <ros/console.h>
00039
00040 namespace occupancy_map_monitor
00041 {
00042
00043 LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size) :
00044 tree_(tree),
00045 running_(true),
00046 max_batch_size_(max_batch_size),
00047 max_sensor_delta_(1e-3),
00048 process_occupied_cells_set_(NULL),
00049 process_model_cells_set_(NULL),
00050 update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)),
00051 process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this))
00052 {
00053 }
00054
00055 LazyFreeSpaceUpdater::~LazyFreeSpaceUpdater()
00056 {
00057 running_ = false;
00058 {
00059 boost::unique_lock<boost::mutex> _(update_cell_sets_lock_);
00060 update_condition_.notify_one();
00061 }
00062 {
00063 boost::unique_lock<boost::mutex> _(cell_process_lock_);
00064 process_condition_.notify_one();
00065 }
00066 update_thread_.join();
00067 process_thread_.join();
00068 }
00069
00070 void LazyFreeSpaceUpdater::pushLazyUpdate(octomap::KeySet *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)
00071 {
00072 ROS_DEBUG("Pushing %lu occupied cells and %lu model cells for lazy updating...", (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size());
00073 boost::mutex::scoped_lock _(update_cell_sets_lock_);
00074 occupied_cells_sets_.push_back(occupied_cells);
00075 model_cells_sets_.push_back(model_cells);
00076 sensor_origins_.push_back(sensor_origin);
00077 update_condition_.notify_one();
00078 }
00079
00080 void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)
00081 {
00082
00083
00084 if (cell_process_lock_.try_lock())
00085 {
00086 process_occupied_cells_set_ = occupied_cells;
00087 process_model_cells_set_ = model_cells;
00088 process_sensor_origin_ = sensor_origin;
00089 process_condition_.notify_one();
00090 cell_process_lock_.unlock();
00091 }
00092 else
00093 {
00094 ROS_WARN("Previous batch update did not complete. Ignoring set of cells to be freed.");
00095 delete occupied_cells;
00096 delete model_cells;
00097 }
00098 }
00099
00100 void LazyFreeSpaceUpdater::processThread()
00101 {
00102
00103 const float lg_0 = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
00104 const float lg_miss = tree_->getProbMissLog();
00105
00106 octomap::KeyRay key_ray1, key_ray2;
00107 OcTreeKeyCountMap free_cells1, free_cells2;
00108
00109 while (running_)
00110 {
00111 free_cells1.clear();
00112 free_cells2.clear();
00113
00114 boost::unique_lock<boost::mutex> ulock(cell_process_lock_);
00115 while (!process_occupied_cells_set_ && running_)
00116 process_condition_.wait(ulock);
00117
00118 if (!running_)
00119 break;
00120
00121 ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", (long unsigned int)process_occupied_cells_set_->size(), (long unsigned int)process_model_cells_set_->size());
00122
00123 ros::WallTime start = ros::WallTime::now();
00124 tree_->lockRead();
00125
00126 #pragma omp sections
00127 {
00128
00129 #pragma omp section
00130 {
00131
00132 for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
00133 if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
00134 for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end() ; jt != end ; ++jt)
00135 free_cells1[*jt] += it->second;
00136 }
00137
00138 #pragma omp section
00139 {
00140
00141 for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
00142 if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
00143 for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end() ; jt != end ; ++jt)
00144 free_cells2[*jt]++;
00145 }
00146 }
00147
00148 tree_->unlockRead();
00149
00150 for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
00151 {
00152 free_cells1.erase(it->first);
00153 free_cells2.erase(it->first);
00154 }
00155
00156 for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
00157 {
00158 free_cells1.erase(*it);
00159 free_cells2.erase(*it);
00160 }
00161 ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size()));
00162
00163 tree_->lockWrite();
00164
00165 try
00166 {
00167
00168 for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
00169 tree_->updateNode(*it, lg_0);
00170
00171
00172 for (OcTreeKeyCountMap::iterator it = free_cells1.begin(), end = free_cells1.end(); it != end; ++it)
00173 tree_->updateNode(it->first, it->second * lg_miss);
00174 for (OcTreeKeyCountMap::iterator it = free_cells2.begin(), end = free_cells2.end(); it != end; ++it)
00175 tree_->updateNode(it->first, it->second * lg_miss);
00176 }
00177 catch (...)
00178 {
00179 ROS_ERROR("Internal error while updating octree");
00180 }
00181 tree_->unlockWrite();
00182 tree_->triggerUpdateCallback();
00183
00184 ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
00185
00186 delete process_occupied_cells_set_;
00187 process_occupied_cells_set_ = NULL;
00188 delete process_model_cells_set_;
00189 process_model_cells_set_ = NULL;
00190 }
00191 }
00192
00193 void LazyFreeSpaceUpdater::lazyUpdateThread()
00194 {
00195 OcTreeKeyCountMap *occupied_cells_set = NULL;
00196 octomap::KeySet *model_cells_set = NULL;
00197 octomap::point3d sensor_origin;
00198 unsigned int batch_size = 0;
00199
00200 while (running_)
00201 {
00202 boost::unique_lock<boost::mutex> ulock(update_cell_sets_lock_);
00203 while (occupied_cells_sets_.empty() && running_)
00204 update_condition_.wait(ulock);
00205
00206 if (!running_)
00207 break;
00208
00209 if (batch_size == 0)
00210 {
00211 occupied_cells_set = new OcTreeKeyCountMap();
00212 octomap::KeySet *s = occupied_cells_sets_.front();
00213 occupied_cells_sets_.pop_front();
00214 for (octomap::KeySet::iterator it = s->begin(), end = s->end(); it != end; ++it)
00215 (*occupied_cells_set)[*it]++;
00216 delete s;
00217 model_cells_set = model_cells_sets_.front();
00218 model_cells_sets_.pop_front();
00219 sensor_origin = sensor_origins_.front();
00220 sensor_origins_.pop_front();
00221 batch_size++;
00222 }
00223
00224 while (!occupied_cells_sets_.empty())
00225 {
00226 if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_)
00227 {
00228 ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size);
00229 pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
00230 batch_size = 0;
00231 break;
00232 }
00233 sensor_origins_.pop_front();
00234
00235 octomap::KeySet *add_occ = occupied_cells_sets_.front();
00236 for (octomap::KeySet::iterator it = add_occ->begin(), end = add_occ->end(); it != end; ++it)
00237 (*occupied_cells_set)[*it]++;
00238 occupied_cells_sets_.pop_front();
00239 delete add_occ;
00240 octomap::KeySet *mod_occ = model_cells_sets_.front();
00241 model_cells_set->insert(mod_occ->begin(), mod_occ->end());
00242 model_cells_sets_.pop_front();
00243 delete mod_occ;
00244 batch_size++;
00245 }
00246
00247 if (batch_size >= max_batch_size_)
00248 {
00249 ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread", batch_size);
00250 pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
00251 occupied_cells_set = NULL;
00252 batch_size = 0;
00253 }
00254 }
00255 }
00256
00257 }