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/lazy_free_space_updater/lazy_free_space_updater.h>
00038 #include <ros/console.h>
00039 
00040 namespace occupancy_map_monitor
00041 {
00042 LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr& tree, unsigned int max_batch_size)
00043   : tree_(tree)
00044   , running_(true)
00045   , max_batch_size_(max_batch_size)
00046   , max_sensor_delta_(1e-3)
00047   ,  // 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,
00071                                           const octomap::point3d& sensor_origin)
00072 {
00073   ROS_DEBUG("Pushing %lu occupied cells and %lu model cells for lazy updating...",
00074             (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size());
00075   boost::mutex::scoped_lock _(update_cell_sets_lock_);
00076   occupied_cells_sets_.push_back(occupied_cells);
00077   model_cells_sets_.push_back(model_cells);
00078   sensor_origins_.push_back(sensor_origin);
00079   update_condition_.notify_one();
00080 }
00081 
00082 void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, octomap::KeySet* model_cells,
00083                                               const octomap::point3d& sensor_origin)
00084 {
00085   // this is basically a queue of size 1. if this function is called repeatedly without any work being done by
00086   // processThread(),
00087   // data can be lost; this is intentional, to avoid spending too much time clearing the octomap
00088   if (cell_process_lock_.try_lock())
00089   {
00090     process_occupied_cells_set_ = occupied_cells;
00091     process_model_cells_set_ = model_cells;
00092     process_sensor_origin_ = sensor_origin;
00093     process_condition_.notify_one();
00094     cell_process_lock_.unlock();
00095   }
00096   else
00097   {
00098     ROS_WARN("Previous batch update did not complete. Ignoring set of cells to be freed.");
00099     delete occupied_cells;
00100     delete model_cells;
00101   }
00102 }
00103 
00104 void LazyFreeSpaceUpdater::processThread()
00105 {
00106   const float lg_0 = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
00107   const float lg_miss = tree_->getProbMissLog();
00108 
00109   octomap::KeyRay key_ray1, key_ray2;
00110   OcTreeKeyCountMap free_cells1, free_cells2;
00111 
00112   while (running_)
00113   {
00114     free_cells1.clear();
00115     free_cells2.clear();
00116 
00117     boost::unique_lock<boost::mutex> ulock(cell_process_lock_);
00118     while (!process_occupied_cells_set_ && running_)
00119       process_condition_.wait(ulock);
00120 
00121     if (!running_)
00122       break;
00123 
00124     ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells",
00125               (long unsigned int)process_occupied_cells_set_->size(),
00126               (long unsigned int)process_model_cells_set_->size());
00127 
00128     ros::WallTime start = ros::WallTime::now();
00129     tree_->lockRead();
00130 
00131 #pragma omp sections
00132     {
00133 #pragma omp section
00134       {
00135         /* compute the free cells along each ray that ends at an occupied cell */
00136         for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(),
00137                                          end = process_occupied_cells_set_->end();
00138              it != end; ++it)
00139           if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
00140             for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end(); jt != end; ++jt)
00141               free_cells1[*jt] += it->second;
00142       }
00143 
00144 #pragma omp section
00145       {
00146         /* compute the free cells along each ray that ends at a model cell */
00147         for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
00148              it != end; ++it)
00149           if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
00150             for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end(); jt != end; ++jt)
00151               free_cells2[*jt]++;
00152       }
00153     }
00154 
00155     tree_->unlockRead();
00156 
00157     for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(),
00158                                      end = process_occupied_cells_set_->end();
00159          it != end; ++it)
00160     {
00161       free_cells1.erase(it->first);
00162       free_cells2.erase(it->first);
00163     }
00164 
00165     for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
00166          it != end; ++it)
00167     {
00168       free_cells1.erase(*it);
00169       free_cells2.erase(*it);
00170     }
00171     ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size()));
00172 
00173     tree_->lockWrite();
00174 
00175     try
00176     {
00177       // set the logodds to the minimum for the cells that are part of the model
00178       for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end();
00179            it != end; ++it)
00180         tree_->updateNode(*it, lg_0);
00181 
00182       /* mark free cells only if not seen occupied in this cloud */
00183       for (OcTreeKeyCountMap::iterator it = free_cells1.begin(), end = free_cells1.end(); it != end; ++it)
00184         tree_->updateNode(it->first, it->second * lg_miss);
00185       for (OcTreeKeyCountMap::iterator it = free_cells2.begin(), end = free_cells2.end(); it != end; ++it)
00186         tree_->updateNode(it->first, it->second * lg_miss);
00187     }
00188     catch (...)
00189     {
00190       ROS_ERROR("Internal error while updating octree");
00191     }
00192     tree_->unlockWrite();
00193     tree_->triggerUpdateCallback();
00194 
00195     ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
00196 
00197     delete process_occupied_cells_set_;
00198     process_occupied_cells_set_ = NULL;
00199     delete process_model_cells_set_;
00200     process_model_cells_set_ = NULL;
00201   }
00202 }
00203 
00204 void LazyFreeSpaceUpdater::lazyUpdateThread()
00205 {
00206   OcTreeKeyCountMap* occupied_cells_set = NULL;
00207   octomap::KeySet* model_cells_set = NULL;
00208   octomap::point3d sensor_origin;
00209   unsigned int batch_size = 0;
00210 
00211   while (running_)
00212   {
00213     boost::unique_lock<boost::mutex> ulock(update_cell_sets_lock_);
00214     while (occupied_cells_sets_.empty() && running_)
00215       update_condition_.wait(ulock);
00216 
00217     if (!running_)
00218       break;
00219 
00220     if (batch_size == 0)
00221     {
00222       occupied_cells_set = new OcTreeKeyCountMap();
00223       octomap::KeySet* s = occupied_cells_sets_.front();
00224       occupied_cells_sets_.pop_front();
00225       for (octomap::KeySet::iterator it = s->begin(), end = s->end(); it != end; ++it)
00226         (*occupied_cells_set)[*it]++;
00227       delete s;
00228       model_cells_set = model_cells_sets_.front();
00229       model_cells_sets_.pop_front();
00230       sensor_origin = sensor_origins_.front();
00231       sensor_origins_.pop_front();
00232       batch_size++;
00233     }
00234 
00235     while (!occupied_cells_sets_.empty())
00236     {
00237       if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_)
00238       {
00239         ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size);
00240         pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
00241         batch_size = 0;
00242         break;
00243       }
00244       sensor_origins_.pop_front();
00245 
00246       octomap::KeySet* add_occ = occupied_cells_sets_.front();
00247       for (octomap::KeySet::iterator it = add_occ->begin(), end = add_occ->end(); it != end; ++it)
00248         (*occupied_cells_set)[*it]++;
00249       occupied_cells_sets_.pop_front();
00250       delete add_occ;
00251       octomap::KeySet* mod_occ = model_cells_sets_.front();
00252       model_cells_set->insert(mod_occ->begin(), mod_occ->end());
00253       model_cells_sets_.pop_front();
00254       delete mod_occ;
00255       batch_size++;
00256     }
00257 
00258     if (batch_size >= max_batch_size_)
00259     {
00260       ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread", batch_size);
00261       pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin);
00262       occupied_cells_set = NULL;
00263       batch_size = 0;
00264     }
00265   }
00266 }
00267 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10