Added octomap services to rtabmap node

CMakeLists.txt

@@ -8,12 +8,13 @@ find_package(catkin REQUIRED COMPONENTS
              cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
              image_transport tf tf_conversions laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
-             genmsg stereo_msgs
+             genmsg stereo_msgs octomap_ros
 )
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 find_package(RTABMap 0.8 REQUIRED)
+find_package(octomap REQUIRED)
 
 #Qt stuff
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
@@ -81,7 +82,7 @@ catkin_package(
   CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
                  image_transport tf tf_conversions laser_geometry pcl_conversions 
                  pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
-                 stereo_msgs
+                 stereo_msgs octomap_ros
 )
 
 ###########
@@ -95,12 +96,14 @@ include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
   ${RTABMap_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
+  ${OCTOMAP_INCLUDE_DIRS}
 )
 
 # libraries
 SET(Libraries
    ${catkin_LIBRARIES}
    ${RTABMap_LIBRARIES}
+   ${OCTOMAP_LIBRARIES}
 )
 
 ## RVIZ plugin

package.xml

@@ -34,6 +34,8 @@
   <build_depend>class_loader</build_depend>
   <build_depend>rtabmap</build_depend>
   <build_depend>move_base_msgs</build_depend>
+  <build_depend>octomap_ros</build_depend>
+  <build_depend>octomap</build_depend>
   
   <run_depend>cv_bridge</run_depend>
   <run_depend>roscpp</run_depend>
@@ -58,6 +60,8 @@
   <run_depend>class_loader</run_depend>
   <run_depend>rtabmap</run_depend>
   <run_depend>move_base_msgs</run_depend>
+  <run_depend>octomap_ros</run_depend>
+  <run_depend>octomap</run_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
 

src/CoreWrapper.cpp

@@ -55,6 +55,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <laser_geometry/laser_geometry.h>
 #include <image_geometry/stereo_camera_model.h>
 
+#include <octomap/octomap.h>
+#include <octomap_ros/conversions.h>
+#include <octomap_msgs/Octomap.h>
+#include <octomap_msgs/conversions.h>
+
 //msgs
 #include "rtabmap_ros/Info.h"
 #include "rtabmap_ros/MapData.h"
@@ -78,7 +83,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		useActionForGoal_(false),
 		cloudDecimation_(4),
 		cloudMaxDepth_(4.0), // meters
-		cloudVoxelSize_(0.02), // meters
+		cloudVoxelSize_(0.05), // meters
+		cloudOutputVoxelized_(false),
 		projMaxGroundAngle_(45.0), // degrees
 		projMinClusterSize_(20),
 		projMaxHeight_(2.0), // meters
@@ -86,6 +92,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		gridSize_(0), // meters
 		mapFilterRadius_(0.5),
 		mapFilterAngle_(30.0), // degrees
+		mapCacheCleanup_(true),
 		mapToOdom_(tf::Transform::getIdentity()),
 		depthSync_(0),
 		depthScanSync_(0),
@@ -143,6 +150,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
 	pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
 	pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
+	pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
 
 	//projection map stuff
 	pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
@@ -156,6 +164,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	// common map stuff
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
 	pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
+	pnh.param("map_cache_cleanup", mapCacheCleanup_, mapCacheCleanup_);
 
 	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
 	ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
@@ -331,6 +340,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
+	octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
+	octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
 
@@ -986,6 +997,25 @@ void CoreWrapper::process(
 					gridMapPub_.getNumSubscribers() != 0);
 			this->publishMaps(filteredPoses, timeNow);
 
+			// clear memory if no one subscribed
+			if(mapCacheCleanup_)
+			{
+				if(cloudMapPub_.getNumSubscribers() == 0)
+				{
+					clouds_.clear();
+				}
+
+				if(projMapPub_.getNumSubscribers() == 0)
+				{
+					projMaps_.clear();
+				}
+
+				if(gridMapPub_.getNumSubscribers() == 0)
+				{
+					gridMaps_.clear();
+				}
+			}
+
 			// update goal if planning is enabled
 			if(!currentMetricGoal_.isNull())
 			{
@@ -1345,6 +1375,12 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::
 				xMin, yMin,
 				gridSize_);
 
+		// clear memory if no one subscribed
+		if(mapCacheCleanup_ && projMapPub_.getNumSubscribers() == 0)
+		{
+			projMaps_.clear();
+		}
+
 		if(!pixels.empty())
 		{
 			//init
@@ -1388,6 +1424,12 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::
 				xMin, yMin,
 				gridSize_);
 
+		// clear memory if no one subscribed
+		if(mapCacheCleanup_ && gridMapPub_.getNumSubscribers() == 0)
+		{
+			gridMaps_.clear();
+		}
+
 		if(!pixels.empty())
 		{
 			//init
@@ -1418,16 +1460,18 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::
 
 bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
 {
-	if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
+	ROS_INFO("rtabmap: Publishing map...");
-	{
-		ROS_INFO("rtabmap: Publishing map...");
 
+	if(mapDataPub_.getNumSubscribers() ||
+		mapGraphPub_.getNumSubscribers() ||
+		(!req.graphOnly && (cloudMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers() || gridMapPub_.getNumSubscribers())))
+	{
 		std::map<int, Signature> signatures;
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
 		std::map<int, int> mapIds;
 
-		if(mapDataPub_.getNumSubscribers() == 0 || req.graphOnly)
+		if(req.graphOnly)
 		{
 			rtabmap_.getGraph(
 					poses,
@@ -1453,46 +1497,92 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			return false;
 		}
 
-		rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
 		ros::Time now = ros::Time::now();
-		graphMsg->header.stamp = now;
+		if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
-		graphMsg->header.frame_id = mapFrameId_;
-
-		rtabmap_ros::mapGraphToROS(poses,
-			mapIds,
-			constraints,
-			Transform::getIdentity(),
-			*graphMsg);
-
-		if(mapDataPub_.getNumSubscribers())
 		{
-			//RGB-D SLAM data
+			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
-			rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
+			graphMsg->header.stamp = now;
-			msg->header = graphMsg->header;
+			graphMsg->header.frame_id = mapFrameId_;
-			msg->graph = *graphMsg;
 
-			// add data
+			rtabmap_ros::mapGraphToROS(poses,
-			msg->nodes.resize(signatures.size());
+				mapIds,
-			int i=0;
+				constraints,
-			for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
+				Transform::getIdentity(),
+				*graphMsg);
+
+			if(mapDataPub_.getNumSubscribers())
 			{
-				rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]);
+				//RGB-D SLAM data
+				rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
+				msg->header = graphMsg->header;
+				msg->graph = *graphMsg;
+
+				// add data
+				msg->nodes.resize(signatures.size());
+				int i=0;
+				for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
+				{
+					rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]);
+				}
+
+				mapDataPub_.publish(msg);
 			}
 
-			mapDataPub_.publish(msg);
+			if(mapGraphPub_.getNumSubscribers())
+			{
+				mapGraphPub_.publish(graphMsg);
+			}
 		}
 
-		if(mapGraphPub_.getNumSubscribers())
+		if(!req.graphOnly)
 		{
-			mapGraphPub_.publish(graphMsg);
+			std::map<int, Transform> filteredPoses;
-		}
+			if(signatures.size())
+			{
+				filteredPoses = this->updateMapCaches(poses,
+									cloudMapPub_.getNumSubscribers() != 0,
+									projMapPub_.getNumSubscribers() != 0,
+									gridMapPub_.getNumSubscribers() != 0,
+									signatures);
+			}
+			else if(mapFilterRadius_ > 0.0)
+			{
+				// filter nodes
+				double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
+				filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
+			}
+			else
+			{
+				filteredPoses = poses;
+			}
+			publishMaps(filteredPoses, now);
 
-		publishMaps(poses, now);
+			// clear memory if no one subscribed
+			if(mapCacheCleanup_)
+			{
+				if(cloudMapPub_.getNumSubscribers() == 0)
+				{
+					clouds_.clear();
+				}
+
+				if(projMapPub_.getNumSubscribers() == 0)
+				{
+					projMaps_.clear();
+				}
+
+				if(gridMapPub_.getNumSubscribers() == 0)
+				{
+					gridMaps_.clear();
+				}
+			}
+		}
 	}
 	else
 	{
-		ROS_INFO("rtabmap: not publishing the map because there are no subscribers to MapData...");
+		UWARN("No subscribers, don't need to publish!");
+		return false;
 	}
+
 	return true;
 }
 
@@ -1566,9 +1656,10 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
 		const std::map<int, rtabmap::Transform> & poses,
 		bool updateCloud,
 		bool updateProj,
-		bool updateGrid)
+		bool updateGrid,
+		const std::map<int, Signature> & signatures)
 {
-	if(!rtabmap_.getMemory())
+	if(!rtabmap_.getMemory() && signatures.size() == 0)
 	{
 		ROS_FATAL("Memory not initialized!?");
 		return std::map<int, rtabmap::Transform>();
@@ -1603,7 +1694,18 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
 					depthRequired ||
 					scanRequired)
 				{
-					data = rtabmap_.getMemory()->getSignatureDataConst(iter->first);
+					if(signatures.size())
+					{
+						std::map<int, Signature>::const_iterator findIter = signatures.find(iter->first);
+						if(findIter != signatures.end())
+						{
+							data = findIter->second;
+						}
+					}
+					else
+					{
+						data = rtabmap_.getMemory()->getSignatureDataConst(iter->first);
+					}
 				}
 
 				if(data.id() > 0)
@@ -1815,22 +1917,6 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
 		}
 	}
 
-	// clear memory if no one subscribed
-	if(cloudMapPub_.getNumSubscribers() == 0)
-	{
-		clouds_.clear();
-	}
-
-	if(projMapPub_.getNumSubscribers() == 0)
-	{
-		projMaps_.clear();
-	}
-
-	if(gridMapPub_.getNumSubscribers() == 0)
-	{
-		gridMaps_.clear();
-	}
-
 	return filteredPoses;
 }
 
@@ -1842,8 +1928,9 @@ void CoreWrapper::publishMaps(
 	if(cloudMapPub_.getNumSubscribers())
 	{
 		// generate the assembled cloud!
+		UTimer time;
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
+		int count = 0;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
@@ -1851,15 +1938,17 @@ void CoreWrapper::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZRGB>(jter->second, iter->second);
 				*assembledCloud+=*transformed;
+				++count;
 			}
 		}
 
 		if(assembledCloud->size())
 		{
-			if(cloudVoxelSize_ > 0)
+			if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
 			{
 				assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, cloudVoxelSize_);
 			}
+			ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
 
 			sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
 			pcl::toROSMsg(*assembledCloud, *cloudMsg);
@@ -1867,6 +1956,10 @@ void CoreWrapper::publishMaps(
 			cloudMsg->header.frame_id = mapFrameId_;
 			cloudMapPub_.publish(cloudMsg);
 		}
+		else if(poses.size())
+		{
+			ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
+		}
 	}
 
 	if(projMapPub_.getNumSubscribers())
@@ -1906,6 +1999,10 @@ void CoreWrapper::publishMaps(
 
 			projMapPub_.publish(map);
 		}
+		else if(poses.size())
+		{
+			ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
+		}
 	}
 
 	if(gridMapPub_.getNumSubscribers())
@@ -1945,6 +2042,10 @@ void CoreWrapper::publishMaps(
 
 			gridMapPub_.publish(map);
 		}
+		else if(poses.size())
+		{
+			ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
+		}
 	}
 }
 
@@ -2059,6 +2160,80 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
 	}
 }
 
+// returned OcTree must be deleted
+// RTAB-Map optimizes the graph at almost each iteration, an octomap cannot
+// be updated online. Only available on service. To have an "online" octomap published as a topic,
+// you may want to subscribe an octomap_server to /rtabmap/cloud topic.
+//
+octomap::OcTree * CoreWrapper::createOctomap()
+{
+	octomap::OcTree * octree = new octomap::OcTree(gridCellSize_);
+	UTimer time;
+	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
+	if(clouds_.size() == 0)
+	{
+		poses = this->updateMapCaches(poses, true, false, false);
+	}
+	for(std::map<int, Transform>::const_iterator posesIter = poses.begin(); posesIter!=poses.end(); ++posesIter)
+	{
+		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator cloudsIter = clouds_.find(posesIter->first);
+		if(cloudsIter != clouds_.end())
+		{
+			octomap::Pointcloud * scan = new octomap::Pointcloud();
+			octomap::pointcloudPCLToOctomap(*cloudsIter->second, *scan);
+			float x,y,z, r,p,w;
+			posesIter->second.getTranslationAndEulerAngles(x,y,z,r,p,w);
+			octomap::ScanNode node(scan, octomap::pose6d(x,y,z, r,p,w), posesIter->first);
+			octree->insertPointCloud(node, cloudMaxDepth_, true, true);
+			ROS_INFO("inserted %d pt=%d (%fs)", posesIter->first, (int)scan->size(), time.ticks());
+		}
+	}
+
+	octree->updateInnerOccupancy();
+	ROS_INFO("updated inner occupancy (%fs)", time.ticks());
+
+	// clear memory if no one subscribed
+	if(mapCacheCleanup_ && cloudMapPub_.getNumSubscribers() == 0)
+	{
+		clouds_.clear();
+	}
+	return octree;
+}
+
+bool CoreWrapper::octomapBinaryCallback(
+		octomap_msgs::GetOctomap::Request  &req,
+		octomap_msgs::GetOctomap::Response &res)
+{
+	ROS_INFO("Sending binary map data on service request");
+	res.map.header.frame_id = mapFrameId_;
+	res.map.header.stamp = ros::Time::now();
+
+	octomap::OcTree * octree = createOctomap();
+	bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
+	if(octree)
+	{
+		delete octree;
+	}
+	return success;
+}
+
+bool CoreWrapper::octomapFullCallback(
+		octomap_msgs::GetOctomap::Request  &req,
+		octomap_msgs::GetOctomap::Response &res)
+{
+	ROS_INFO("Sending full map data on service request");
+	res.map.header.frame_id = mapFrameId_;
+	res.map.header.stamp = ros::Time::now();
+
+	octomap::OcTree * octree = createOctomap();
+	bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
+	if(octree)
+	{
+		delete octree;
+	}
+	return success;
+}
+
 /**
  * exclusive callbacks:
  *     image

src/CoreWrapper.h

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/GetMap.h>
+#include <octomap_msgs/GetOctomap.h>
 
 #include <rtabmap/core/Statistics.h>
 #include <rtabmap/core/Parameters.h>
@@ -75,6 +76,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <actionlib_msgs/GoalStatusArray.h>
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 
+namespace octomap{
+class OcTree;
+}
+
 class CoreWrapper
 {
 public:
@@ -138,14 +143,21 @@ private:
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
+	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
+	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);
 
 	void publishLoop(double tfDelay);
 
+	std::map<int, rtabmap::Transform> updateMapCaches(const std::map<int, rtabmap::Transform> & poses,
+			bool updateCloud,
+			bool updateProj,
+			bool updateGrid,
+			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
+
 	void publishStats(const ros::Time & stamp);
-	std::map<int, rtabmap::Transform> updateMapCaches(const std::map<int, rtabmap::Transform> & poses, bool updateCloud, bool updateProj, bool updateGrid);
 	void publishMaps(const std::map<int, rtabmap::Transform> & poses, const ros::Time & stamp);
 	void publishCurrentGoal(const ros::Time & stamp);
 	void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
@@ -153,6 +165,8 @@ private:
 	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 	void publishLocalPath(const ros::Time & stamp);
 
+	octomap::OcTree * createOctomap();
+
 private:
 	rtabmap::Rtabmap rtabmap_;
 	bool paused_;
@@ -172,6 +186,7 @@ private:
 	int cloudDecimation_;
 	double cloudMaxDepth_;
 	double cloudVoxelSize_;
+	bool cloudOutputVoxelized_;
 	double projMaxGroundAngle_;
 	int projMinClusterSize_;
 	double projMaxHeight_;
@@ -179,6 +194,7 @@ private:
 	double gridSize_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
+	bool mapCacheCleanup_;
 
 	tf::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;
@@ -274,6 +290,8 @@ private:
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
+	ros::ServiceServer octomapBinarySrv_;
+	ros::ServiceServer octomapFullSrv_;
 
 	MoveBaseClient mbClient_;