Cleaned up locks and callbacks in perception so OcTree subclass is not needed

perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -105,7 +105,7 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue &params)
 bool DepthImageOctomapUpdater::initialize()
 {
   tf_ = monitor_->getTFClient();
-  free_space_updater_.reset(new LazyFreeSpaceUpdater(tree_));
+  free_space_updater_.reset(new LazyFreeSpaceUpdater(tree_, monitor_));
 
   // create our mesh filter
   mesh_filter_.reset(new mesh_filter::MeshFilter<mesh_filter::StereoCameraModel>(mesh_filter::MeshFilterBase::TransformCallback(),
@@ -422,10 +422,11 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   }
 
   // figure out occupied cells and model cells
-  tree_->lockRead();
+
 
   try
   {
+    ReadLock lock = monitor_->readingMap();
     const int h_bound = h - skip_vertical_pixels_;
     const int w_bound = w - skip_horizontal_pixels_;
 
@@ -491,19 +492,17 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   }
   catch (...)
   {
-    tree_->unlockRead();
     return;
   }
-  tree_->unlockRead();
 
   /* cells that overlap with the model are not occupied */
   for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
     occupied_cells.erase(*it);
 
   // mark occupied cells
-  tree_->lockWrite();
   try
   {
+    WriteLock lock = monitor_->writingMap();
     /* now mark all occupied cells */
     for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
       tree_->updateNode(*it, true);
@@ -512,8 +511,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   {
     ROS_ERROR("Internal error while updating octree");
   }
-  tree_->unlockWrite();
-  tree_->triggerUpdateCallback();
+  monitor_->triggerUpdateCallback();
 
   // at this point we still have not freed the space
   free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin);

perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h

@@ -38,6 +38,7 @@
 #define MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_
 
 #include <moveit/occupancy_map_monitor/occupancy_map.h>
+#include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
 #include <boost/thread.hpp>
 #include <deque>
 
@@ -48,7 +49,7 @@ class LazyFreeSpaceUpdater
 {
 public:
 
-  LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size = 10);
+  LazyFreeSpaceUpdater(const OccMapTreePtr &tree, OccupancyMapMonitor *monitor, unsigned int max_batch_size = 10);
   ~LazyFreeSpaceUpdater();
 
   void pushLazyUpdate(octomap::KeySet *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin);
@@ -67,6 +68,7 @@ class LazyFreeSpaceUpdater
   void processThread();
 
   OccMapTreePtr tree_;
+  OccupancyMapMonitor *monitor_;
   bool running_;
   std::size_t max_batch_size_;
   double max_sensor_delta_;

perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp

@@ -40,11 +40,12 @@
 namespace occupancy_map_monitor
 {
 
-LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size) :
+LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr &tree, OccupancyMapMonitor *monitor, unsigned int max_batch_size) :
   tree_(tree),
   running_(true),
   max_batch_size_(max_batch_size),
   max_sensor_delta_(1e-3), // 1mm
+  monitor_(monitor),
   process_occupied_cells_set_(NULL),
   process_model_cells_set_(NULL),
   update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)),
@@ -121,32 +122,32 @@ void LazyFreeSpaceUpdater::processThread()
     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());
 
     ros::WallTime start = ros::WallTime::now();
-    tree_->lockRead();
+    {
+      ReadLock lock = monitor_->readingMap();
 
 #pragma omp sections
-        {
+      {
 
 #pragma omp section
-      {
-        /* compute the free cells along each ray that ends at an occupied cell */
-        for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
-          if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
-            for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end() ; jt != end ; ++jt)
-              free_cells1[*jt] += it->second;
-      }
+        {
+          /* compute the free cells along each ray that ends at an occupied cell */
+          for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
+            if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
+              for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end() ; jt != end ; ++jt)
+                free_cells1[*jt] += it->second;
+        }
 
 #pragma omp section
-      {
-        /* compute the free cells along each ray that ends at a model cell */
-        for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
-          if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
-            for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end() ; jt != end ; ++jt)
-              free_cells2[*jt]++;
+        {
+          /* compute the free cells along each ray that ends at a model cell */
+          for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
+            if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
+              for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end() ; jt != end ; ++jt)
+                free_cells2[*jt]++;
+        }
       }
     }
 
-    tree_->unlockRead();
-
     for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
     {
       free_cells1.erase(it->first);
@@ -160,10 +161,9 @@ void LazyFreeSpaceUpdater::processThread()
     }
     ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size()));
 
-    tree_->lockWrite();
-
     try
     {
+      WriteLock lock = monitor_->writingMap();
       // set the logodds to the minimum for the cells that are part of the model
       for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
         tree_->updateNode(*it, lg_0);
@@ -178,8 +178,7 @@ void LazyFreeSpaceUpdater::processThread()
     {
       ROS_ERROR("Internal error while updating octree");
     }
-    tree_->unlockWrite();
-    tree_->triggerUpdateCallback();
+    monitor_->triggerUpdateCallback();
 
     ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
 

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h

@@ -39,84 +39,13 @@
 
 #include <octomap/octomap.h>
 #include <boost/shared_ptr.hpp>
-#include <boost/thread/shared_mutex.hpp>
-#include <boost/function.hpp>
 
 namespace occupancy_map_monitor
 {
 
 typedef octomap::OcTreeNode OccMapNode;
-
-class OccMapTree : public octomap::OcTree
-{
-public:
-
-  OccMapTree(double resolution) : octomap::OcTree(resolution)
-  {
-  }
-
-  OccMapTree(const std::string &filename) : octomap::OcTree(filename)
-  {
-  }
-
-  /** @brief lock the underlying octree. it will not be read or written by the
-   *  monitor until unlockTree() is called */
-  void lockRead()
-  {
-    tree_mutex_.lock_shared();
-  }
-
-  /** @brief unlock the underlying octree. */
-  void unlockRead()
-  {
-    tree_mutex_.unlock_shared();
-  }
-
-  /** @brief lock the underlying octree. it will not be read or written by the
-   *  monitor until unlockTree() is called */
-  void lockWrite()
-  {
-    tree_mutex_.lock();
-  }
-
-  /** @brief unlock the underlying octree. */
-  void unlockWrite()
-  {
-    tree_mutex_.unlock();
-  }
-
-  typedef boost::shared_lock<boost::shared_mutex> ReadLock;
-  typedef boost::unique_lock<boost::shared_mutex> WriteLock;
-
-  ReadLock reading()
-  {
-    return ReadLock(tree_mutex_);
-  }
-
-  WriteLock writing()
-  {
-    return WriteLock(tree_mutex_);
-  }
-
-  void triggerUpdateCallback(void)
-  {
-    if (update_callback_)
-      update_callback_();
-  }
-
-  /** @brief Set the callback to trigger when updates are received */
-  void setUpdateCallback(const boost::function<void()> &update_callback)
-  {
-    update_callback_ = update_callback;
-  }
-
-private:
-  boost::shared_mutex tree_mutex_;
-  boost::function<void()> update_callback_;
-};
-
-typedef boost::shared_ptr<OccMapTree> OccMapTreePtr;
-typedef boost::shared_ptr<const OccMapTree> OccMapTreeConstPtr;
+typedef boost::shared_ptr<octomap::OcTree> OccMapTreePtr;
+typedef boost::shared_ptr<const octomap::OcTree> OccMapTreeConstPtr;
 
 }
 

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h

@@ -49,10 +49,15 @@
 #include <moveit/occupancy_map_monitor/occupancy_map_updater.h>
 
 #include <boost/thread/mutex.hpp>
+#include <boost/thread/shared_mutex.hpp>
+#include <boost/function.hpp>
 
 namespace occupancy_map_monitor
 {
 
+typedef boost::shared_lock<boost::shared_mutex> ReadLock;
+typedef boost::unique_lock<boost::shared_mutex> WriteLock;
+
 class OccupancyMapMonitor
 {
 public:
@@ -107,10 +112,17 @@ class OccupancyMapMonitor
   /** \brief Forget about this shape handle and the shapes it corresponds to */
   void forgetShape(ShapeHandle handle);
 
+  /** @brief Signal that the occupancy map has been updated */
+  void triggerUpdateCallback()
+  {
+    if (update_callback_)
+      update_callback_();
+  }
+
   /** @brief Set the callback to trigger when updates to the maintained octomap are received */
   void setUpdateCallback(const boost::function<void()> &update_callback)
   {
-    tree_->setUpdateCallback(update_callback);
+    update_callback_ = update_callback;
   }
 
   void setTransformCacheCallback(const TransformCacheProvider &transform_cache_callback);
@@ -122,6 +134,42 @@ class OccupancyMapMonitor
     return active_;
   }
 
+  /** @brief lock the underlying octree. it will not be read or written by the
+   *  monitor until unlockTree() is called */
+  void lockMapRead()
+  {
+    map_mutex_.lock_shared();
+  }
+
+  /** @brief unlock the underlying octree. */
+  void unlockMapRead()
+  {
+    map_mutex_.unlock_shared();
+  }
+
+  /** @brief lock the underlying octree. it will not be read or written by the
+   *  monitor until unlockTree() is called */
+  void lockMapWrite()
+  {
+    map_mutex_.lock();
+  }
+
+  /** @brief unlock the underlying octree. */
+  void unlockMapWrite()
+  {
+    map_mutex_.unlock();
+  }
+
+  ReadLock readingMap()
+  {
+    return ReadLock(map_mutex_);
+  }
+
+  WriteLock writingMap()
+  {
+    return WriteLock(map_mutex_);
+  }
+
 private:
 
   void initialize();
@@ -138,6 +186,7 @@ class OccupancyMapMonitor
   std::string map_frame_;
   double map_resolution_;
   boost::mutex parameters_lock_;
+  boost::shared_mutex map_mutex_;
 
   OccMapTreePtr tree_;
   OccMapTreeConstPtr tree_const_;
@@ -146,6 +195,7 @@ class OccupancyMapMonitor
   std::vector<OccupancyMapUpdaterPtr> map_updaters_;
   std::vector<std::map<ShapeHandle, ShapeHandle> > mesh_handles_;
   TransformCacheProvider transform_cache_callback_;
+  boost::function<void()> update_callback_;
   bool debug_info_;
 
   std::size_t mesh_handle_count_;

perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp

@@ -84,7 +84,7 @@ void OccupancyMapMonitor::initialize()
   if (!tf_ && !map_frame_.empty())
     ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data.");
 
-  tree_.reset(new OccMapTree(map_resolution_));
+  tree_.reset(new octomap::OcTree(map_resolution_));
   tree_const_ = tree_;
 
   XmlRpc::XmlRpcValue sensor_list;
@@ -174,6 +174,7 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr &updater)
   {
     map_updaters_.push_back(updater);
     updater->publishDebugInformation(debug_info_);
+    updater->publishDebugInformation(debug_info_);
     if (map_updaters_.size() > 1)
     {
       mesh_handles_.resize(map_updaters_.size());
@@ -282,16 +283,15 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s
 bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response)
 {
   ROS_INFO("Writing map to %s", request.filename.c_str());
-  tree_->lockRead();
   try
   {
+    ReadLock lock = readingMap();
     response.success = tree_->writeBinary(request.filename);
   }
   catch (...)
   {
     response.success = false;
   }
-  tree_->unlockRead();
   return true;
 }
 
@@ -300,17 +300,16 @@ bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request
   ROS_INFO("Reading map from %s", request.filename.c_str());
 
   /* load the octree from disk */
-  tree_->lockWrite();
   try
   {
+    WriteLock lock = writingMap();
     response.success = tree_->readBinary(request.filename);
   }
   catch (...)
   {
     ROS_ERROR("Failed to load map from file");
     response.success = false;
   }
-  tree_->unlockWrite();
 
   return true;
 }