lazy_free_space_updater.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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), // 1mm
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   // this is basically a queue of size 1. if this function is called repeatedly without any work being done by processThread(),
00083   // data can be lost; this is intentional, to avoid spending too much time clearing the octomap
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         /* compute the free cells along each ray that ends at an occupied cell */
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         /* compute the free cells along each ray that ends at a model cell */
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       // set the logodds to the minimum for the cells that are part of the model
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       /* mark free cells only if not seen occupied in this cloud */
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 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Aug 26 2015 12:43:21