ROS package updated to RTAB-Map 0.6.2

updated service name: publish_current_map_data and publish_full_map_data
fixed not published recalled images
Grid map assembler: remove obstacle if a new laser says empty
Modified MapData and InfoEx msg (MapCorrection field is not anymore in MapData)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1067 f169173b-cf89-36c8-b27e-44dbe73f0c83

rtabmap/launch/config/rgbd_gui.ini

@@ -1,6 +1,6 @@
 [Gui]
 General\imagesKept=true
-General\loggerLevel=2
+General\loggerLevel=1
 General\loggerEventLevel=3
 General\loggerPauseLevel=3
 General\loggerType=1
@@ -13,14 +13,14 @@ General\beep=false
 General\keypointsOpacity=16
 General\voxelSize=0
 General\decimation=16
-MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\x11\0\0\x2u\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0(\0\0\x2u\0\0\x1\xcc\0\xff\xff\xff\0\0\0\x1\0\0\x3\xa0\0\0\x2u\xfc\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2u\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\xf7\0\0\0\x9c\xfc\x1\0\0\0\x4\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\xf7\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1:\0\0\x2u\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
-MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0*\0\0\0Y\0\0\x6\x30\0\0\x3\xd1\0\0\0\x32\0\0\0u\0\0\x6(\0\0\x3\xc9\0\0\0\0\0\0)
+MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1$\0\0\x2\x18\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0(\0\0\x2\x18\0\0\x1\xcc\0\xff\xff\xff\0\0\0\x1\0\0\x3\xd8\0\0\x2\x18\xfc\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x18\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\xd0\0\0\0\x9c\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\xd0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\x5u\0\0\0\xea\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\xc8\0\0\x2\x18\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
+MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0*\0\0\x1\x5\0\0\x6\t\0\0\x4 \0\0\0\x32\0\0\x1!\0\0\x6\x1\0\0\x4\x18\0\0\0\0\0\0)
 General\showClouds0=true
 General\voxelSize0=0
 General\decimation0=4
 General\maxDepth0=4
 General\showScans0=true
-General\opacity0=0.6
+General\opacity0=1
 General\ptSize0=1
 General\opacityScan0=1
 General\ptSizeScan0=1

rtabmap/msg/InfoEx.msg

@@ -9,10 +9,12 @@ int32 refId
 int32 loopClosureId
 int32 localLoopClosureId
 
-# The map data (rgb, depth, depthConstant, depth2D, localTransform, poses, map ids, mapCorrection)
+# The map data (rgb, depth, depthConstant, depth2D, localTransform, poses, map ids)
 rtabmap/MapData data
 
+geometry_msgs/Transform mapCorrection
 geometry_msgs/Transform loopClosureTransform
+
 geometry_msgs/Pose currentPose
 
 ####

rtabmap/msg/MapData.msg

@@ -35,5 +35,3 @@ geometry_msgs/Transform[] localTransforms
 int32[] poseIDs
 geometry_msgs/Pose[] poses
 
-geometry_msgs/Transform mapCorrection
-

rtabmap/src/CoreWrapper.cpp

@@ -172,7 +172,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
 	resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
 	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
-	publishMapDataSrv_ = nh.advertiseService("publish_map_data", &CoreWrapper::publishMapDataCallback, this);
+	publishFullMapDataSrv_ = nh.advertiseService("publish_full_map_data", &CoreWrapper::publishFullMapDataCallback, this);
+	publishCurrentMapDataSrv_ = nh.advertiseService("publish_current_map_data", &CoreWrapper::publishCurrentMapDataCallback, this);
 
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -603,16 +604,27 @@ bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Emp
 	return true;
 }
 
-bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+bool CoreWrapper::publishFullMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
-	ROS_INFO("rtabmap: Publishing map data...");
+	publishMapData(true);
+	return true;
+}
+
+bool CoreWrapper::publishCurrentMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	publishMapData(false);
+	return true;
+}
+
+void CoreWrapper::publishMapData(bool full)
+{
+	ROS_INFO("rtabmap: Publishing map data (full=%s)...", full?"true":"false");
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
 	std::map<int, float> depthConstants;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
-	Transform mapCorrection;
 
 	if(mapData_.getNumSubscribers())
 	{
@@ -625,7 +637,8 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em
 				depthConstants,
 				localTransforms,
 				poses,
-				mapCorrection);
+				true,
+				full);
 
 		int i=0;
 
@@ -689,12 +702,8 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em
 			++i;
 		}
 
-		transformToGeometryMsg(mapCorrection, msg->mapCorrection);
-
 		mapData_.publish(msg);
 	}
-
-	return true;
 }
 
 void CoreWrapper::publishStats(const Statistics & stats)
@@ -754,7 +763,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				++i;
 			}
 
-			transformToGeometryMsg(stats.mapCorrection(), msg->data.mapCorrection);
+			transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
 			transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
 			transformToPoseMsg(stats.currentPose(), msg->currentPose);
 
@@ -823,6 +832,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				{
 					msg->data.imageIDs[index] = i->first;
 					msg->data.images[index].bytes = i->second;
+					++index;
 				}
 
 				msg->data.depthIDs.resize(stats.getDepths().size());
@@ -834,6 +844,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				{
 					msg->data.depthIDs[index] = i->first;
 					msg->data.depths[index].bytes = i->second;
+					++index;
 				}
 
 				msg->data.depth2DIDs.resize(stats.getDepth2ds().size());
@@ -845,6 +856,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				{
 					msg->data.depth2DIDs[index] = i->first;
 					msg->data.depth2Ds[index].bytes = i->second;
+					++index;
 				}
 
 				msg->data.localTransformIDs.resize(stats.getLocalTransforms().size());
@@ -856,6 +868,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				{
 					msg->data.localTransformIDs[index] = i->first;
 					transformToGeometryMsg(i->second, msg->data.localTransforms[index]);
+					++index;
 				}
 
 				msg->data.depthConstantIDs = uKeys(stats.getDepthConstants());

rtabmap/src/CoreWrapper.h

@@ -74,7 +74,10 @@ private:
 	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishFullMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishCurrentMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	void publishMapData(bool full);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);
@@ -135,7 +138,8 @@ private:
 	ros::ServiceServer pauseSrv_;
 	ros::ServiceServer resumeSrv_;
 	ros::ServiceServer triggerNewMapSrv_;
-	ros::ServiceServer publishMapDataSrv_;
+	ros::ServiceServer publishFullMapDataSrv_;
+	ros::ServiceServer publishCurrentMapDataSrv_;
 
 	boost::thread* transformThread_;
 

rtabmap/src/GridMapAssemblerNode.cpp

@@ -227,7 +227,7 @@ public:
 			UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f", upperbound, grid.cols, x+1, slope, b).c_str());
 			for(int y = lowerbound; y<=(int)upperbound; ++y)
 			{
-				if(grid.at<char>(y, x) == -1)
+				//if(grid.at<char>(y, x) == -1)
 				{
 					grid.at<char>(y, x) = 0; // free space
 				}

rtabmap/src/GuiWrapper.cpp

@@ -173,7 +173,7 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	}
 
 	//RGB-D SLAM data
-	stat.setMapCorrection(transformFromGeometryMsg(msg->data.mapCorrection));
+	stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection));
 	stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform));
 	stat.setCurrentPose(transformFromPoseMsg(msg->currentPose));
 
@@ -237,7 +237,6 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
 	std::map<int, float> depthConstants;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
-	Transform mapCorrection;
 
 	if(msg->imageIDs.size() != msg->images.size())
 	{
@@ -295,15 +294,12 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
 		poses.insert(std::make_pair(msg->poseIDs[i], t));
 	}
 
-	mapCorrection = transformFromGeometryMsg(msg->mapCorrection);
-
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
 			depthConstants,
 			localTransforms,
-			poses,
-			mapCorrection));
+			poses));
 }
 
 void GuiWrapper::handleEvent(UEvent * anEvent)
@@ -393,9 +389,25 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 		{
 			if(mapDataTopic_.getNumPublishers())
 			{
-				if(!ros::service::call("publish_map_data", srv))
+				if(!ros::service::call("publish_current_map_data", srv))
 				{
-					ROS_WARN("Can't call \"publish_map_data\" service");
+					ROS_WARN("Can't call \"publish_current_map_data\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapFull)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_full_map_data", srv))
+				{
+					ROS_WARN("Can't call \"publish_full_map_data\" service");
 					this->post(new RtabmapEvent3DMap(1)); // service error
 				}
 			}