Enable Octomap diff

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

@@ -122,6 +122,12 @@ class OccupancyMapMonitor
     return active_;
   }
 
+  /** \brief Enable sending diffs of the occupancy map */
+  void enableOctomapDiff(bool enable) { octomap_diff_enable = enable; }
+
+  /** \brief Returns if sending diffs of the occupancy map is enabled */
+  bool isOctomapDiffEnabled() const { return octomap_diff_enable; }
+
 private:
 
   void initialize();
@@ -141,6 +147,7 @@ class OccupancyMapMonitor
 
   OccMapTreePtr tree_;
   OccMapTreeConstPtr tree_const_;
+  bool octomap_diff_enable;
 
   boost::scoped_ptr<pluginlib::ClassLoader<OccupancyMapUpdater> > updater_plugin_loader_;
   std::vector<OccupancyMapUpdaterPtr> map_updaters_;

perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp

@@ -87,6 +87,13 @@ void OccupancyMapMonitor::initialize()
   tree_.reset(new OccMapTree(map_resolution_));
   tree_const_ = tree_;
 
+  octomap_diff_enable = false;
+  if (nh_.getParam("octomap_diff", octomap_diff_enable))
+  {
+    if (octomap_diff_enable) ROS_INFO("Octomap diff enabled");
+    tree_->enableChangeDetection(octomap_diff_enable);
+  }
+
   XmlRpc::XmlRpcValue sensor_list;
   if (nh_.getParam("sensors", sensor_list))
   {

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -314,6 +314,11 @@ void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
     occupancy_map_monitor::OccMapTree::ReadLock lock;
     if (octomap_monitor_) lock = octomap_monitor_->getOcTreePtr()->reading();
     scene_->getPlanningSceneMsg(msg);
+    if (octomap_monitor_)
+    {
+      octomap_monitor_->getOcTreePtr()->resetChangeDetection();
+      octomap_monitor_->getOcTreePtr()->enableChangeDetection(octomap_monitor_->isOctomapDiffEnabled());
+    }
   }
   planning_scene_publisher_.publish(msg);
   ROS_DEBUG("Published the full planning scene: '%s'", msg.name.c_str());
@@ -339,6 +344,11 @@ void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
             occupancy_map_monitor::OccMapTree::ReadLock lock;
             if (octomap_monitor_) lock = octomap_monitor_->getOcTreePtr()->reading();
             scene_->getPlanningSceneDiffMsg(msg);
+            if (octomap_monitor_)
+            {
+              octomap_monitor_->getOcTreePtr()->resetChangeDetection();
+              octomap_monitor_->getOcTreePtr()->enableChangeDetection(octomap_monitor_->isOctomapDiffEnabled());
+            }
           }
           boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the transform cache to update while we are potentially changing attached bodies
           scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
@@ -357,6 +367,11 @@ void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
             occupancy_map_monitor::OccMapTree::ReadLock lock;
             if (octomap_monitor_) lock = octomap_monitor_->getOcTreePtr()->reading();
             scene_->getPlanningSceneMsg(msg);
+            if (octomap_monitor_)
+            {
+              octomap_monitor_->getOcTreePtr()->resetChangeDetection();
+              octomap_monitor_->getOcTreePtr()->enableChangeDetection(octomap_monitor_->isOctomapDiffEnabled());
+            }
           }
           publish_msg = true;
         }