map_assembler: added octomap services, call get_map_data on initialization to have all data needed, fixed GridGlobal parameters not used.

src/MapAssemblerNode.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
 #include "rtabmap_ros/MapsManager.h"
+#include "rtabmap_ros/GetMap.h"
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -44,6 +45,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <nav_msgs/OccupancyGrid.h>
 #include <std_srvs/Empty.h>
 
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+#include <octomap_msgs/GetOctomap.h>
+#include <octomap_msgs/conversions.h>
+#include <rtabmap/core/OctoMap.h>
+#endif
+#endif
+
 using namespace rtabmap;
 
 class MapAssembler
@@ -63,6 +72,7 @@ public:
 		//parameters
 		rtabmap::ParametersMap parameters;
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
 		if(!configPath.empty())
 		{
@@ -112,12 +122,6 @@ public:
 				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
 				iter->second = uNumber2Str(vInt);
 			}
-
-			if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
-			{
-				ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
-				iter->second = uNumber2Str(8);
-			}
 		}
 
 		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
@@ -162,15 +166,75 @@ public:
 			}
 		}
 
+		// set private parameters
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			pnh.setParam(iter->first, iter->second);
+		}
+
 		ROS_INFO("%s: regenerate_local_grids          = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
 		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
 		mapsManager_.backwardCompatibilityParameters(pnh, parameters);
 		mapsManager_.setParameters(parameters);
 
+		std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
+		std::string rtabmapNs;
+		for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
+		{
+			if(!rtabmapNs.empty())
+			{
+				rtabmapNs += "/";
+			}
+			rtabmapNs += *iter;
+		}
+		ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
+		if(rtabmapNs.empty())
+		{
+			rtabmapNs = "get_map_data";
+		}
+		else
+		{
+			rtabmapNs += "/get_map_data";
+		}
+
+		rtabmap_ros::GetMap getMapSrv;
+		getMapSrv.request.global = false;
+		getMapSrv.request.optimized = true;
+		getMapSrv.request.graphOnly = false;
+		if(ros::service::waitForService(rtabmapNs, 5000))
+		{
+			if(!ros::service::call(rtabmapNs, getMapSrv))
+			{
+				ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
+			}
+			else
+			{
+				ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
+				processMapData(getMapSrv.response.data);
+				ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
+						" will be assembled on next subscriber connection.", rtabmapNs.c_str());
+			}
+		}
+		else
+		{
+			ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
+					"may not be a problem if rtabmap is started afterwards. If rtabmap "
+					"is started after in localization mode, call /rtabmap/publish_maps "
+					"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
+		}
+
+
 		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
 
-		// private service
+		// private services
 		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+		octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
+		octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
+#endif
+#endif
 	}
 
 	~MapAssembler()
@@ -178,25 +242,29 @@ public:
 	}
 
 	void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
+	{
+		processMapData(*msg);
+	}
+	void processMapData(const rtabmap_ros::MapData & msg)
 	{
 		UTimer timer;
 
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
 		Transform mapOdom;
-		rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
-		for(unsigned int i=0; i<msg->nodes.size(); ++i)
+		rtabmap_ros::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
+		for(unsigned int i=0; i<msg.nodes.size(); ++i)
 		{
-			if(msg->nodes[i].image.size() ||
-			   msg->nodes[i].depth.size() ||
-			   msg->nodes[i].laserScan.size())
+			if(msg.nodes[i].image.size() ||
+			   msg.nodes[i].depth.size() ||
+			   msg.nodes[i].laserScan.size())
 			{
-				Signature data = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
+				Signature data = rtabmap_ros::nodeDataFromROS(msg.nodes[i]);
 				if(localGridsRegenerated_)
 				{
 					data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
 				}
-				uInsert(nodes_, std::make_pair(msg->nodes[i].id, data));
+				uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
 			}
 		}
 
@@ -211,16 +279,23 @@ public:
 		}
 
 		// Update maps
-		poses = mapsManager_.updateMapCaches(
-				poses,
-				0,
-				false,
-				false,
-				nodes_);
+		if(!nodes_.empty())
+		{
+			poses = mapsManager_.updateMapCaches(
+					poses,
+					0,
+					false,
+					false,
+					nodes_);
+		}
+		double updateTime = timer.ticks();
 
-		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
+		mapFrameId_ = msg.header.frame_id;
+		optimizedPoses_ = poses;
 
-		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
+		mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
+
+		ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
 	}
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -230,20 +305,64 @@ public:
 		return true;
 	}
 
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	bool 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();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+
+	bool 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();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+#endif
+#endif
+
 private:
 	MapsManager mapsManager_;
 	std::map<int, Signature> nodes_;
+	std::map<int, Transform> optimizedPoses_;
+	std::string mapFrameId_;
 
 	ros::Subscriber mapDataTopic_;
 
 	ros::ServiceServer resetService_;
-
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	ros::ServiceServer octomapBinarySrv_;
+	ros::ServiceServer octomapFullSrv_;
+#endif
+#endif
 	bool localGridsRegenerated_;
 };
 
 
 int main(int argc, char** argv)
 {
+	ULogger::setLevel(ULogger::kError);
+	ULogger::setType(ULogger::kTypeConsole);
+
 	ros::init(argc, argv, "map_assembler");
 
 	// process "--params" argument
@@ -253,6 +372,7 @@ int main(int argc, char** argv)
 		{
 			rtabmap::ParametersMap parameters;
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
 			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 			{