Added get_node_data service

CMakeLists.txt

@@ -15,7 +15,7 @@ endif (POLICY CMP0042)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS 
+find_package(catkin REQUIRED COMPONENTS
              cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
              image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure message_filters class_loader rosgraph_msgs
@@ -119,6 +119,7 @@ add_message_files(
    SetLabel.srv
    GetPlan.srv
    AddLink.srv
+   GetNodeData.srv
  )
 
 ## Generate added messages and services with any dependencies listed here

include/rtabmap_ros/CoreWrapper.h

@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/OdometryInfo.h>
 
+#include "rtabmap_ros/GetNodeData.h"
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/ListLabels.h"
 #include "rtabmap_ros/PublishMap.h"
@@ -193,6 +194,7 @@ private:
 	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool getNodeDataCallback(rtabmap_ros::GetNodeData::Request& req, rtabmap_ros::GetNodeData::Response& res);
 	bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
 	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getProbMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
@@ -300,6 +302,7 @@ private:
 	ros::ServiceServer setLogInfoSrv_;
 	ros::ServiceServer setLogWarnSrv_;
 	ros::ServiceServer setLogErrorSrv_;
+	ros::ServiceServer getNodeDataSrv_;
 	ros::ServiceServer getMapDataSrv_;
 	ros::ServiceServer getProjMapSrv_;
 	ros::ServiceServer getMapSrv_;

src/CoreWrapper.cpp

@@ -596,6 +596,7 @@ void CoreWrapper::onInit()
 	backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
 	setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
+	getNodeDataSrv_ = nh.advertiseService("get_node_data", &CoreWrapper::getNodeDataCallback, this);
 	getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
 	getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 	getProbMapSrv_ = nh.advertiseService("get_prob_map", &CoreWrapper::getProbMapCallback, this);
@@ -2668,6 +2669,32 @@ bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
 	return true;
 }
 
+bool CoreWrapper::getNodeDataCallback(rtabmap_ros::GetNodeData::Request& req, rtabmap_ros::GetNodeData::Response& res)
+{
+	NODELET_INFO("rtabmap: Getting node data (%d node(s), images=%s scan=%s grid=%s user_data=%s)...",
+			(int)req.ids.size(),
+			req.images?"true":"false",
+			req.scan?"true":"false",
+			req.grid?"true":"false",
+			req.user_data?"true":"false");
+
+	for(size_t i=0; i<req.ids.size(); ++i)
+	{
+
+		int id = req.ids[i];
+		Signature s = rtabmap_.getSignatureCopy(id, req.images, req.scan, req.user_data, req.grid);
+
+		if(s.id()>0)
+		{
+			NodeData msg;
+			rtabmap_ros::nodeDataToROS(s, msg);
+			res.data.push_back(msg);
+		}
+	}
+
+	return !res.data.empty();
+}
+
 bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
 {
 	NODELET_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",

src/MapsManager.cpp

@@ -349,7 +349,7 @@ void MapsManager::set2DMap(
 			if(!uContains(gridMaps_, iter->first))
 			{
 				rtabmap::SensorData data;
-				data = memory->getSignatureDataConst(iter->first, false, false, false, true);
+				data = memory->getNodeData(iter->first, false, false, false, true);
 				if(data.gridCellSize() == 0.0f)
 				{
 					ROS_WARN("Local occupancy grid doesn't exist for node %d", iter->first);
@@ -558,7 +558,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 					}
 					else if(memory)
 					{
-						data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
+						data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
 					}
 
 					UDEBUG("Adding grid map %d to cache...", iter->first);
@@ -587,7 +587,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						{
 							// if we are here, it is because we loaded a database with old nodes not having occupancy grid set
 							// try reload again
-							data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
+							data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
 						}
 						data.uncompressData(
 								occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,

srv/GetNodeData.srv

@@ -0,0 +1,9 @@
+#request
+int32[] ids
+bool images
+bool scan
+bool grid
+bool user_data
+---
+#response
+NodeData[] data