Added planner node

Added publishing of laserscan data with data_player
Updated for latest changes of the library (some methods moved from util3d)
rtabmapviz not saving GUI config on ctrl-c

CMakeLists.txt

@@ -159,6 +159,9 @@ target_link_libraries(map_assembler rtabmap_ros ${Libraries})
 add_executable(grid_map_assembler src/GridMapAssemblerNode.cpp)
 target_link_libraries(grid_map_assembler rtabmap_ros ${Libraries})
 
+add_executable(planner src/PlannerNode.cpp)
+target_link_libraries(planner rtabmap_ros ${Libraries})
+
 add_executable(camera src/CameraNode.cpp)
 add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(camera ${Libraries})

launch/azimut3/config/azimut3.rviz

@@ -272,7 +272,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap/MapCloud
+      class: rtabmap_ros/MapCloud
       Cloud decimation: 4
       Cloud max depth (m): 3
       Cloud voxel size (m): 0.02
@@ -298,7 +298,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rtabmap/Info
+    - class: rtabmap_ros/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
@@ -330,7 +330,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap/OrbitOriented
+      class: rtabmap_ros/OrbitOriented
       Distance: 7.83194
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06

launch/azimut3/config/azimut3_nav.rviz

@@ -265,7 +265,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap/MapCloud
+      Class: rtabmap_ros/MapCloud
       Cloud decimation: 8
       Cloud max depth (m): 4
       Cloud voxel size (m): 0
@@ -291,7 +291,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rtabmap/Info
+    - Class: rtabmap_ros/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
@@ -327,7 +327,7 @@ Visualization Manager:
       Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
       Value: true
     - Alpha: 1
-      Class: rtabmap/MapGraph
+      class: rtabmap_ros/MapGraph
       Color: 0; 0; 255
       Enabled: true
       Name: MapGraph
@@ -381,7 +381,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap/OrbitOriented
+      class: rtabmap_ros/OrbitOriented
       Distance: 25.5693
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06

launch/azimut3/config/azimut3_stereo_nav.rviz

@@ -288,7 +288,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap/MapCloud
+      class: rtabmap_ros/MapCloud
       Cloud decimation: 8
       Cloud max depth (m): 4
       Cloud voxel size (m): 0
@@ -314,7 +314,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rtabmap/Info
+    - class: rtabmap_ros/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
@@ -350,7 +350,7 @@ Visualization Manager:
       Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
       Value: true
     - Alpha: 1
-      Class: rtabmap/MapGraph
+      class: rtabmap_ros/MapGraph
       Color: 0; 0; 255
       Enabled: true
       Name: MapGraph
@@ -404,7 +404,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap/OrbitOriented
+      class: rtabmap_ros/OrbitOriented
       Distance: 6.3581
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06

launch/demo/demo_robot_localization_rviz.launch

@@ -0,0 +1,69 @@
+
+<launch>
+
+   <!-- ROBOT LOCALIZATION VERSION: use this with ROS bag demo_mapping.bag -->
+   <!-- A database "~/.ros/rtabmap.db" must be already created from -->
+   <!-- the "demo_robot_mapping.launch" demo -->
+   <!-- Once RTAB-Map GUI stated, you can do "Edit->Download Map" to get all the map in the GUI -->
+   
+  <param name="use_sim_time" type="bool" value="True"/>
+
+<group ns="rtabmap">
+  <!-- SLAM (robot side) -->
+  <!-- args: "delete_db_on_start" and "udebug" -->
+  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
+	<param name="frame_id" type="string" value="base_footprint"/>
+	
+	<param name="subscribe_depth" type="bool" value="true"/>
+	<param name="subscribe_laserScan" type="bool" value="true"/>
+	
+	<remap from="odom" to="/az3/base_controller/odom"/>
+	<remap from="scan" to="/jn0/base_scan"/>
+
+	<remap from="rgb/image" to="/data_throttled_image"/>
+  	<remap from="depth/image" to="/data_throttled_image_depth"/>
+  	<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
+  	
+  	<param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="depth/image_transport" type="string" value="compressedDepth"/>
+	
+	<param name="queue_size" type="int" value="10"/>
+	
+	<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+    <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
+    <param name="Rtabmap/DetectionRate" type="string" value="1"/>     <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
+    <param name="Mem/STMSize" type="string" value="1"/>               <!-- 1 location in short-term memory -->
+    <param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- false = Localization mode-->
+    <param name="Mem/InitWMWithAllNodes" type="string" value="true"/> <!-- Load the full global map in RAM -->
+    <param name="RGBD/PoseScanMatching" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
+	<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
+	<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
+	<param name="LccIcp/Type" type="string" value="2"/>                      <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
+	<param name="LccIcp2/MaxFitness" type="string" value="10"/>
+	<param name="LccBow/MaxDepth" type="string" value="0.0"/>                <!-- 3D visual words maximum depth 0=infinity -->
+	<param name="LccBow/InlierDistance" type="string" value="0.1"/>          <!-- 3D visual words correspondence distance -->
+  </node>
+  
+  <!-- Grid map assembler for rviz -->
+  <node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
+	  <param name="unknown_space_filled" type="bool" value="false"/>
+  </node>
+  
+</group>
+
+  <!-- Visualisation (client side) -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
+   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+    <remap from="rgb/image"       to="data_throttled_image"/>
+    <remap from="depth/image"     to="data_throttled_image_depth"/>
+    <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
+    <remap from="cloud"           to="voxel_cloud" />
+    
+    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="depth/image_transport" type="string" value="compressedDepth"/>
+    
+    <param name="queue_size" type="int" value="10"/>
+    <param name="voxel_size" type="double" value="0.01"/>
+  </node>
+
+</launch>

src/DbPlayerNode.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <nav_msgs/Odometry.h>
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
@@ -38,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_srvs/Empty.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap/utilite/ULogger.h>
-
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/DBReader.h>
 
 bool paused = false;
@@ -133,6 +134,7 @@ int main(int argc, char** argv)
 	ros::Publisher leftCamInfoPub;
 	ros::Publisher rightCamInfoPub;
 	ros::Publisher odometryPub;
+	ros::Publisher scanPub;
 	tf::TransformBroadcaster tfBroadcaster;
 
 	rtabmap::SensorData data = reader.getNextData();
@@ -216,6 +218,11 @@ int main(int argc, char** argv)
 		camInfoB.height = data.depthOrRightImage().rows;
 		camInfoB.width = data.depthOrRightImage().cols;
 
+		if(!data.laserScan().empty())
+		{
+			if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
+		}
+
 		// publish transforms first
 		if(publishTf)
 		{
@@ -339,6 +346,16 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}
 
+		if(scanPub.getNumSubscribers() && !data.laserScan().empty())
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan());
+			sensor_msgs::PointCloud2 msg;
+			pcl::toROSMsg(*cloud, msg);
+			msg.header.frame_id = frameId;
+			msg.header.stamp = time;
+			scanPub.publish(msg);
+		}
+
 		ros::spinOnce();
 
 		while(ros::ok() && paused)

src/GridMapAssemblerNode.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <nav_msgs/OccupancyGrid.h>
@@ -80,7 +82,7 @@ public:
 		{
 			if(!uContains(scans_, msg->nodes[i].id) && msg->nodes[i].laserScan.size())
 			{
-				cv::Mat laserScan = util3d::uncompressData(msg->nodes[i].laserScan);
+				cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
 				scans_.insert(std::make_pair(msg->nodes[i].id, util3d::laserScanToPointCloud(laserScan)));
 			}
 		}
@@ -93,7 +95,7 @@ public:
 
 		if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
 		{
-			poses = util3d::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
+			poses = rtabmap::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
 		}
 
 		if(gridMap_.getNumSubscribers())

src/GuiNode.cpp

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <signal.h>
 
 void my_handler(int s){
-	QApplication::closeAllWindows();
+	QApplication::exit();
 }
 
 int main(int argc, char** argv)

src/MapAssemblerNode.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <pcl_ros/transforms.h>
@@ -119,8 +121,8 @@ public:
 					float cy = msg->nodes[i].cy;
 
 					//uncompress data
-					util3d::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
-					util3d::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
+					rtabmap::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
+					rtabmap::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
 					ctImage.start();
 					ctDepth.start();
 					ctImage.join();
@@ -183,7 +185,7 @@ public:
 
 			if(!uContains(scans_, id) && msg->nodes[i].laserScan.size())
 			{
-				cv::Mat laserScan = util3d::uncompressData(msg->nodes[i].laserScan);
+				cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
 				if(!laserScan.empty())
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
@@ -207,7 +209,7 @@ public:
 		}
 		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
 		{
-			poses = util3d::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
+			poses = rtabmap::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
 		}
 
 		if(assembledMapClouds_.getNumSubscribers())

src/MapOptimizerNode.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/core/util3d.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -211,12 +212,12 @@ public:
 			{
 				if(optimizeFromLastNode_)
 				{
-					std::map<int, int> depthGraph = util3d::generateDepthGraph(constraints, poses.rbegin()->first);
-					util3d::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+					std::map<int, int> depthGraph = rtabmap::generateDepthGraph(constraints, poses.rbegin()->first);
+					rtabmap::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
 				}
 				else
 				{
-					util3d::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+					rtabmap::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
 				}
 
 				mapToOdomMutex_.lock();

src/MsgConversion.cpp

@@ -41,7 +41,7 @@ void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTrans
 {
 	if(!transform.isNull())
 	{
-		tf::transformEigenToTF(rtabmap::util3d::transformToEigen3d(transform), tfTransform);
+		tf::transformEigenToTF(transform.toEigen3d(), tfTransform);
 	}
 	else
 	{
@@ -53,7 +53,7 @@ rtabmap::Transform transformFromTF(const tf::Transform & transform)
 {
 	Eigen::Affine3d eigenTf;
 	tf::transformTFToEigen(transform, eigenTf);
-	return rtabmap::util3d::transformFromEigen3d(eigenTf);
+	return rtabmap::Transform::fromEigen3d(eigenTf);
 }
 
 void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::Transform & msg)

src/PlannerNode.cpp

@@ -0,0 +1,318 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+
+#include <std_msgs/Int32.h>
+#include <std_msgs/Empty.h>
+#include <geometry_msgs/PointStamped.h>
+#include <nav_msgs/Path.h>
+
+#include <tf/transform_listener.h>
+
+#include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap_ros/GetMap.h>
+
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
+
+#include <rtabmap/core/Graph.h>
+
+#include <queue>
+
+class Planner
+{
+
+public:
+	Planner() :
+		frameId_("base_link"),
+		mapFrameId_("map"),
+		plan2d_(true),
+		goalError_(1.0),
+		graphChangeError_(0.2),
+		currentGoalIndex_(0)
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("plan_2d", plan2d_, plan2d_);
+		pnh.param("goal_error", goalError_, goalError_);
+		pnh.param("graph_change_error", graphChangeError_, graphChangeError_);
+
+
+		ros::NodeHandle nh;
+		goalTopic_ = nh.subscribe("goal", 1, &Planner::goalCallback, this);
+		mapDataTopic_ = nh.subscribe("mapData", 1, &Planner::mapDataCallback, this);
+
+		pubNextGoal_ = nh.advertise<geometry_msgs::PoseStamped>("next_goal", 1);
+		pubGoalReached_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
+		pubPath_ = nh.advertise<nav_msgs::Path>("path", 1);
+	}
+
+	~Planner()
+	{
+	}
+
+	void goalCallback(const std_msgs::Int32ConstPtr & msg)
+	{
+		int id = msg->data;
+		ROS_INFO("planner: set goal %d", id);
+		rtabmap_ros::GetMap getMapSrv;
+		getMapSrv.request.global = true;
+		getMapSrv.request.optimized = true;
+		getMapSrv.request.graphOnly = true;
+		if(!ros::service::call("get_map", getMapSrv))
+		{
+			ROS_WARN("Can't call \"get_map\" service");
+			return;
+		}
+
+		std::map<int, rtabmap::Transform> poses;
+		std::map<int, int> mapIds;
+		std::multimap<int, rtabmap::Link> constraints;
+		rtabmap::Transform mapToOdom;
+		rtabmap_ros::mapGraphFromROS(getMapSrv.response.data.graph, poses, mapIds, constraints, mapToOdom);
+		std::multimap<int, int> links;
+		for(std::multimap<int, rtabmap::Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
+		{
+			links.insert(std::make_pair(iter->first, iter->second.to()));
+			links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
+		}
+
+		if(!uContains(poses, id))
+		{
+			ROS_WARN("Goal %d not found in the global graph!", id);
+			return;
+		}
+
+		// find the nearest node of the current location
+		rtabmap::Transform currentPose;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(mapFrameId_, frameId_, ros::Time(0), tmp);
+			currentPose = rtabmap_ros::transformFromTF(tmp);
+			if(plan2d_)
+			{
+				currentPose.z() = 0;
+			}
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		int nearestPose = -1;
+		float nearestDistance = -1.0f;
+		for(std::map<int, rtabmap::Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(plan2d_)
+			{
+				iter->second.z() = 0;
+			}
+			float d = currentPose.getDistance(iter->second);
+			if(nearestPose == -1 || d < nearestDistance)
+			{
+				nearestPose = iter->first;
+				nearestDistance = d;
+			}
+		}
+
+		if(nearestPose <= 0)
+		{
+			ROS_WARN("Nearest pose not found!?!?");
+			return;
+		}
+
+		ROS_INFO("Computing path from location %d to %d", nearestPose, id);
+		path_ = rtabmap::computePath(poses, links, nearestPose, id);
+		currentGoalIndex_ = 0;
+		currentGoal_.setNull();
+		if(path_.size()<2)
+		{
+			path_.clear();
+			ROS_WARN("Cannot compute a path (or goal is already reached)!");
+		}
+		else
+		{
+			ROS_INFO("Path generated! Size=%d", (int)path_.size());
+			if(pubPath_.getNumSubscribers())
+			{
+				nav_msgs::Path path;
+				path.header.frame_id = mapFrameId_;
+				path.header.stamp = ros::Time::now();
+				path.poses.resize(path_.size());
+				for(unsigned int i=0; i<path_.size(); ++i)
+				{
+					path.poses[i].header = path.header;
+					rtabmap_ros::transformToPoseMsg(poses[path_[i]], path.poses[i].pose);
+				}
+				pubPath_.publish(path);
+			}
+		}
+	}
+
+	void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
+	{
+		if(path_.size())
+		{
+			// Get current pose
+			rtabmap::Transform currentPose;
+			try
+			{
+				tf::StampedTransform tmp;
+				tfListener_.lookupTransform(mapFrameId_, frameId_, ros::Time(0), tmp);
+				currentPose = rtabmap_ros::transformFromTF(tmp);
+				if(plan2d_)
+				{
+					currentPose.z() = 0;
+				}
+			}
+			catch(tf::TransformException & ex)
+			{
+				ROS_WARN("%s",ex.what());
+				return;
+			}
+
+			// Convert graph data
+			std::map<int, rtabmap::Transform> poses;
+			std::map<int, int> mapIds;
+			std::multimap<int, rtabmap::Link> constraints;
+			rtabmap::Transform mapToOdom;
+			rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, constraints, mapToOdom);
+
+			// Post a new goal if:
+			// - no current goal is set,
+			// - the current goal is reached,
+			// - the current goal has moved (graph was optimized with new constraints)
+			if(!currentGoal_.isNull())
+			{
+				std::map<int, rtabmap::Transform>::iterator iter=poses.find(path_[currentGoalIndex_]);
+				if(iter != poses.end())
+				{
+					rtabmap::Transform currentGoal = iter->second;
+					if(plan2d_)
+					{
+						currentGoal.z() = 0;
+					}
+					float d = currentGoal.getDistance(currentGoal_);
+					if(d > graphChangeError_)
+					{
+						// reset the goal
+						ROS_INFO("Goal reset because the local graph has changed (%f m) newCurent=%s oldCurrent=%s...",
+								d, currentGoal.prettyPrint().c_str(), currentGoal_.prettyPrint().c_str());
+						currentGoal_.setNull();
+					}
+
+					if(!currentGoal_.isNull())
+					{
+						float d = currentPose.getDistance(currentGoal_);
+						if(d < goalError_)
+						{
+							//Current goal reached! Reset it.
+							currentGoal_.setNull();
+							if(currentGoalIndex_ == path_.size()-1)
+							{
+								// last goal reached!
+								ROS_INFO("Goal %d reached!", path_[currentGoalIndex_]);
+								pubGoalReached_.publish(std_msgs::Empty());
+								path_.clear();
+								return; // EXIT
+							}
+
+						}
+					}
+				}
+
+			}
+			if(currentGoal_.isNull())
+			{
+				// find the farthest node that can be reached in the local map
+				for(unsigned int i=path_.size()-1; i>=currentGoalIndex_; --i)
+				{
+					std::map<int, rtabmap::Transform>::iterator iter=poses.find(path_[i]);
+					if(iter != poses.end())
+					{
+						currentGoalIndex_ = i;
+						currentGoal_ = iter->second;
+						if(plan2d_)
+						{
+							currentGoal_.z() = 0;
+						}
+						break;
+					}
+				}
+				if(currentGoal_.isNull())
+				{
+					ROS_ERROR("Cannot update current goal!");
+				}
+				else
+				{
+					ROS_INFO("Publishing next goal: Location %d (%d/%d)",
+							path_[currentGoalIndex_], currentGoalIndex_+1, (int)path_.size());
+					geometry_msgs::PoseStamped goalMsg;
+					goalMsg.header = msg->header;
+					rtabmap_ros::transformToPoseMsg(currentGoal_, goalMsg.pose);
+					pubNextGoal_.publish(goalMsg);
+				}
+			}
+		}
+	}
+
+private:
+	std::string frameId_;
+	std::string mapFrameId_;
+	bool plan2d_;
+	double goalError_;
+	double graphChangeError_;
+
+	tf::TransformListener tfListener_;
+
+	ros::Subscriber goalTopic_;
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher pubNextGoal_;
+	ros::Publisher pubGoalReached_;
+	ros::Publisher pubPath_;
+
+	std::vector<int> path_;
+	int currentGoalIndex_;
+	rtabmap::Transform currentGoal_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "planner");
+	Planner planner;
+	ros::spin();
+	return 0;
+}

src/rviz/MapCloudDisplay.cpp

@@ -51,6 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "MapCloudDisplay.h"
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap_ros/GetMap.h>
 
@@ -258,8 +260,8 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 				float cy = map.nodes[i].cy;
 
 				//uncompress data
-				rtabmap::util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
-				rtabmap::util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
+				rtabmap::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
+				rtabmap::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
 				ctImage.start();
 				ctDepth.start();
 				ctImage.join();
@@ -323,7 +325,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
 	{
-		poses = rtabmap::util3d::radiusPosesFiltering(poses,
+		poses = rtabmap::radiusPosesFiltering(poses,
 				node_filtering_radius_->getFloat(),
 				node_filtering_angle_->getFloat()*CV_PI/180.0);
 	}