From b2b20b151aa8e7af54c85364a553ad0a7479c457 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Tue, 30 Apr 2013 20:25:52 +0000
Subject: [PATCH] ROS: Included UtiLite library directly in Rtabmap source (to
 simplify the installation), rtabmapviz: fixed wrong current image shown bug
 (loop is shown instead)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@858 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 ...p_closure_detection.launch => demo.launch} | 11 +++++-
 rtabmap/manifest.xml                          |  1 -
 rtabmap/src/CoreNode.cpp                      |  2 +-
 rtabmap/src/CoreWrapper.cpp                   | 38 +++++++++++++------
 rtabmap/src/CoreWrapper.h                     |  6 ++-
 rtabmap/src/GuiNode.cpp                       |  2 +-
 rtabmap/src/GuiWrapper.cpp                    |  4 +-
 rtabmap/src/GuiWrapper.h                      |  2 +-
 8 files changed, 45 insertions(+), 21 deletions(-)
 rename rtabmap/launch/{loop_closure_detection.launch => demo.launch} (77%)

diff --git a/rtabmap/launch/loop_closure_detection.launch b/rtabmap/launch/demo.launch
similarity index 77%
rename from rtabmap/launch/loop_closure_detection.launch
rename to rtabmap/launch/demo.launch
index 1dd3a2b0..35f422ff 100644
--- a/rtabmap/launch/loop_closure_detection.launch
+++ b/rtabmap/launch/demo.launch
@@ -5,9 +5,14 @@
 	<!--           See "delete_db_on_start" option below... -->
 		
 	<!-- args: "delete_db_on_start" and "udebug" -->
-	<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start"/>
+	<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
 	
-	<node name="rtabmapviz" pkg="rtabmap" type="rtabmapviz" output="screen"/>
+		<param name="config_path" value="~/.rtabmap/demo.ini" type="string"/>
+		<param name="working_directory" value="~/.rtabmap" type="string"/>  
+	</node>
+	
+	<node name="rtabmapviz" pkg="rtabmap" type="rtabmapviz" output="screen"
+	 args="-d $(find rtabmap)/launch/demo.ini"/>
 	
 	<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
 	<node name="camera" pkg="uimage" type="camera" output="screen">
@@ -19,4 +24,6 @@
 		<param name="height" value="0" type="int"/>
 		<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
 	</node>
+	
+	<node name="im_extract" pkg="rtabmap" type="im_extract"/>
 </launch>
diff --git a/rtabmap/manifest.xml b/rtabmap/manifest.xml
index c8e3ff13..0a7a1777 100644
--- a/rtabmap/manifest.xml
+++ b/rtabmap/manifest.xml
@@ -18,7 +18,6 @@
   <depend package="tf"/>
   <depend package="rtabmap_lib"/>
   <depend package="cv_bridge"/>
-  <depend package="uimage"/> <!-- from utilite stack -->
 </package>
 
 
diff --git a/rtabmap/src/CoreNode.cpp b/rtabmap/src/CoreNode.cpp
index e0a5ecd7..be241e9f 100644
--- a/rtabmap/src/CoreNode.cpp
+++ b/rtabmap/src/CoreNode.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "CoreWrapper.h"
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
 
 int main(int argc, char** argv)
 {
diff --git a/rtabmap/src/CoreWrapper.cpp b/rtabmap/src/CoreWrapper.cpp
index d2a110ef..18a602ed 100644
--- a/rtabmap/src/CoreWrapper.cpp
+++ b/rtabmap/src/CoreWrapper.cpp
@@ -14,10 +14,11 @@
 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
-#include <utilite/UEventsManager.h>
-#include <utilite/ULogger.h>
-#include <utilite/UFile.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/highgui/highgui.hpp>
 
 //msgs
@@ -27,7 +28,8 @@
 using namespace rtabmap;
 
 CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
-		rtabmap_(0)
+		rtabmap_(0),
+		configPath_(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini")
 {
 	ros::NodeHandle nh("~");
 	infoPub_ = nh.advertise<rtabmap::Info>("info", 1);
@@ -36,9 +38,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	rtabmap_ = new Rtabmap();
 
-	loadNodeParameters(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini");
+	std::string workingDir = UDirectory::homeDir()+"/.rtabmap";
+	nh.param("config_path", configPath_, configPath_);
+	nh.param("working_directory", workingDir, workingDir);
 
-	rtabmap_->init(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini", deleteDbOnStart);
+	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
+	workingDir = uReplaceChar(workingDir, '~', UDirectory::homeDir());
+
+	loadNodeParameters(configPath_, workingDir);
+
+	rtabmap_->init(configPath_, deleteDbOnStart);
 
 	resetMemorySrv_ = nh.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
 	dumpMemorySrv_ = nh.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
@@ -54,11 +63,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 CoreWrapper::~CoreWrapper()
 {
-	this->saveNodeParameters(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini");
+	this->saveNodeParameters(configPath_);
 	delete rtabmap_;
 }
 
-void CoreWrapper::loadNodeParameters(const std::string & configFile)
+ParametersMap CoreWrapper::loadNodeParameters(const std::string & configFile,
+											  const std::string & workingDirectory)
 {
 	ROS_INFO("Loading parameters from %s", configFile.c_str());
 	if(!UFile::exists(configFile.c_str()))
@@ -69,12 +79,18 @@ void CoreWrapper::loadNodeParameters(const std::string & configFile)
 	ParametersMap parameters = Parameters::getDefaultParameters();
 	Rtabmap::readParameters(configFile.c_str(), parameters);
 
+	if(workingDirectory.size())
+	{
+		parameters.at(Parameters::kRtabmapWorkingDirectory()) = workingDirectory;
+	}
+
 	ros::NodeHandle nh("~");
 	for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
 	{
 		nh.setParam(i->first, i->second);
 	}
 	parametersLoadedPub_.publish(std_msgs::Empty());
+	return parameters;
 }
 
 void CoreWrapper::saveNodeParameters(const std::string & configFile)
@@ -99,7 +115,7 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
 
 	Rtabmap::writeParameters(configFile.c_str(), parameters);
 
-	std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/LTM.db";
+	std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/rtabmap.db";
 	printf("Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
 }
 
@@ -109,7 +125,7 @@ void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
 	{
 		//ROS_INFO("Received image.");
 		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
-		rtabmap_->process(ptr->image.clone(), ptr->header.seq);
+		rtabmap_->process(ptr->image, ptr->header.seq);
 		const Statistics & stats = rtabmap_->getStatistics();
 		this->publishStats(stats);
 	}
diff --git a/rtabmap/src/CoreWrapper.h b/rtabmap/src/CoreWrapper.h
index e280a119..454e24cc 100644
--- a/rtabmap/src/CoreWrapper.h
+++ b/rtabmap/src/CoreWrapper.h
@@ -17,6 +17,7 @@
 #include <sensor_msgs/Image.h>
 #include <geometry_msgs/Twist.h>
 #include <image_transport/image_transport.h>
+#include <rtabmap/core/Parameters.h>
 
 namespace rtabmap
 {
@@ -39,7 +40,8 @@ private:
 	bool dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 
-	void loadNodeParameters(const std::string & configFile);
+	rtabmap::ParametersMap loadNodeParameters(const std::string & configFile,
+											  const std::string & workingDirectory);
 	void saveNodeParameters(const std::string & configFile);
 
 	void publishStats(const rtabmap::Statistics & stats);
@@ -53,7 +55,7 @@ private:
 	ros::Publisher infoPub_;
 	ros::Publisher infoPubEx_;
 	ros::Publisher parametersLoadedPub_;
-	std::string configFile_;
+	std::string configPath_;
 
 	ros::ServiceServer resetMemorySrv_;
 	ros::ServiceServer dumpMemorySrv_;
diff --git a/rtabmap/src/GuiNode.cpp b/rtabmap/src/GuiNode.cpp
index 427ee142..9512613c 100644
--- a/rtabmap/src/GuiNode.cpp
+++ b/rtabmap/src/GuiNode.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "GuiWrapper.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 
 #include <QApplication>
 #include <rtabmap/gui/MainWindow.h>
diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp
index 75ecdc82..cfc26e5c 100644
--- a/rtabmap/src/GuiWrapper.cpp
+++ b/rtabmap/src/GuiWrapper.cpp
@@ -12,7 +12,7 @@
 #include <std_srvs/Empty.h>
 #include <std_msgs/Empty.h>
 
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
 
 #include <opencv2/highgui/highgui.hpp>
 
@@ -104,7 +104,7 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 #else
 		image = cv::imdecode(msg->loopImage, -1);
 #endif
-		stat.setRefImage(image);
+		stat.setLoopImage(image);
 		//stat.setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
 	}
 
diff --git a/rtabmap/src/GuiWrapper.h b/rtabmap/src/GuiWrapper.h
index 9d026b06..61e5d5b2 100644
--- a/rtabmap/src/GuiWrapper.h
+++ b/rtabmap/src/GuiWrapper.h
@@ -10,7 +10,7 @@
 
 #include <ros/ros.h>
 #include "rtabmap/InfoEx.h"
-#include "utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsHandler.h"
 #include <geometry_msgs/TwistStamped.h>
 
 namespace rtabmap