From 6550b72e7ef2783f68df3e7b0ce216487582fced Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Wed, 31 Aug 2016 03:07:35 +0900
Subject: [PATCH] Update inner occupancy periodically

---
 octomap_server/include/octomap_server/OctomapServer.h |  1 +
 octomap_server/src/OctomapServer.cpp                  | 11 +++++++++++
 octomap_server/src/octomap_server_node.cpp            |  3 ++-
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h
index 0dcc4eb7..9c9fe969 100644
--- a/octomap_server/include/octomap_server/OctomapServer.h
+++ b/octomap_server/include/octomap_server/OctomapServer.h
@@ -124,6 +124,7 @@ class OctomapServer {
   void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
   void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
   virtual void publishAll(const ros::Time& rostime = ros::Time::now());
+  void updateInnerOccupancy(const ros::TimerEvent& event);
 
   /**
   * @brief update occupancy map with a scan labeled as ground and nonground.
diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp
index d845474e..b931c098 100644
--- a/octomap_server/src/OctomapServer.cpp
+++ b/octomap_server/src/OctomapServer.cpp
@@ -185,6 +185,12 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_)
   dynamic_reconfigure::Server<OctomapServerConfig>::CallbackType f;
   f = boost::bind(&OctomapServer::reconfigureCallback, this, _1, _2);
   m_reconfigureServer.setCallback(f);
+
+  private_nh.createTimer(
+      ros::Duration(1),
+      &OctomapServer::updateInnerOccupancy,
+      this,
+      /*oneshot=*/false);
 }
 
 OctomapServer::~OctomapServer(){
@@ -477,6 +483,11 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl
 }
 
 
+void OctomapServer::updateInnerOccupancy(const ros::TimerEvent& event)
+{
+  m_octree->updateInnerOccupancy();
+}
+
 
 void OctomapServer::publishAll(const ros::Time& rostime){
   ros::WallTime startTime = ros::WallTime::now();
diff --git a/octomap_server/src/octomap_server_node.cpp b/octomap_server/src/octomap_server_node.cpp
index ffebc966..de8ba4a5 100644
--- a/octomap_server/src/octomap_server_node.cpp
+++ b/octomap_server/src/octomap_server_node.cpp
@@ -77,7 +77,8 @@ int main(int argc, char** argv){
   }
 
   try{
-    ros::spin();
+    ros::MultiThreadedSpinner spinner(4);
+    spinner.spin();
   }catch(std::runtime_error& e){
     ROS_ERROR("octomap_server exception: %s", e.what());
     return -1;