ros-pkg: Added ResetPose.srv for odometry service "reset_odom_to_pose" to set intialial pose. Updated some method interfaces with latest library changes from trunk.

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

CMakeLists.txt

@@ -51,6 +51,7 @@ add_message_files(
    FILES
    GetMap.srv
    PublishMap.srv
+   ResetPose.srv
  )
 
 ## Generate added messages and services with any dependencies listed here

src/CoreNode.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UFile.h>
 
 int main(int argc, char** argv)
 {
@@ -60,9 +61,6 @@ int main(int argc, char** argv)
 			uInsert(parameters,
 					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
 					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
-			uInsert(parameters,
-					std::make_pair(rtabmap::Parameters::kRtabmapDatabasePath(),
-					UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName())); // change default to ~/.ros
 
 			if(strcmp(argv[i], "--params") == 0)
 			{

src/CoreWrapper.cpp

@@ -66,6 +66,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		mapFrameId_("map"),
 		odomFrameId_(""),
 		configPath_(""),
+		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
 		mapToOdom_(tf::Transform::getIdentity()),
 		transformThread_(0),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
@@ -99,6 +100,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	}
 
 	pnh.param("config_path", configPath_, configPath_);
+	pnh.param("database_path", databasePath_, databasePath_);
 
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
@@ -122,7 +124,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	// update parameters with user input parameters (private)
 	uInsert(parameters, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
-	uInsert(parameters, std::make_pair(Parameters::kRtabmapDatabasePath(), UDirectory::homeDir()+"/.ros/"+Parameters::getDefaultDatabaseName())); // change default to ~/.ros
 	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		std::string vStr;
@@ -138,10 +139,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 			{
 				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
 			}
-			else if(iter->first.compare(Parameters::kRtabmapDatabasePath()) == 0)
-			{
-				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
-			}
 			else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
 			{
 				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
@@ -224,13 +221,21 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	}
 	if(paused_)
 	{
-		UWARN("Node paused... dont' forget to call service \"resume\" to start rtabmap.");
+		ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
+	}
+
+	if(deleteDbOnStart)
+	{
+		if(UFile::erase(databasePath_) == 0)
+		{
+			ROS_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start is set).", databasePath_.c_str());
+		}
 	}
 
 	// Init RTAB-Map
-	rtabmap_.init(parameters, deleteDbOnStart);
+	rtabmap_.init(parameters, databasePath_);
 
-	ROS_INFO("rtabmap: using database from \"%s\".", rtabmap_.getDatabasePath().c_str());
+	ROS_INFO("rtabmap: Using database from \"%s\".", databasePath_.c_str());
 
 	// setup services
 	updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
@@ -281,8 +286,7 @@ CoreWrapper::~CoreWrapper()
 	}
 	nh.deleteParam("is_rtabmap_paused");
 
-	std::string databasePath = rtabmap_.getDatabasePath();
+	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
-	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
 }
 
 ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
@@ -861,7 +865,7 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
 bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	ROS_INFO("rtabmap: Reset");
-	rtabmap_.resetMemory(true);
+	rtabmap_.resetMemory();
 	return true;
 }
 

src/CoreWrapper.h

@@ -63,7 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class CoreWrapper
 {
 public:
-	CoreWrapper(bool deleteDbOnStart = false);
+	CoreWrapper(bool deleteDbOnStart);
 	virtual ~CoreWrapper();
 
 private:
@@ -128,6 +128,7 @@ private:
 	std::string mapFrameId_;
 	std::string odomFrameId_;
 	std::string configPath_;
+	std::string databasePath_;
 
 	tf::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;

src/GuiWrapper.cpp

@@ -356,7 +356,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 		std_srvs::Empty emptySrv;
 		rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
 		rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
-		if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
+		if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
 		{
 			if(!ros::service::call("reset", emptySrv))
 			{

src/MapAssemblerNode.cpp

@@ -107,8 +107,8 @@ public:
 					float cy = msg->nodes[i].cy;
 
 					//uncompress data
-					util3d::CompressionThread ctImage(msg->nodes[i].image.bytes, true);
+					util3d::CompressionThread ctImage(&msg->nodes[i].image.bytes, true);
-					util3d::CompressionThread ctDepth(msg->nodes[i].depth.bytes, true);
+					util3d::CompressionThread ctDepth(&msg->nodes[i].depth.bytes, true);
 					ctImage.start();
 					ctDepth.start();
 					ctImage.join();

src/OdometryROS.cpp

@@ -64,9 +64,28 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 
 	ros::NodeHandle pnh("~");
 
+	Transform initialPose = Transform::getIdentity();
+	std::string initialPoseStr;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
 	pnh.param("publish_tf", publishTf_, publishTf_);
+	pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
+	if(initialPoseStr.size())
+	{
+		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
+		if(values.size() == 6)
+		{
+			initialPose = Transform(
+					atof(values[0].c_str()), atof(values[1].c_str()), atof(values[2].c_str()),
+					atof(values[3].c_str()), atof(values[4].c_str()), atof(values[5].c_str()));
+		}
+		else
+		{
+			ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
+					  "Identity will be used...", initialPoseStr.c_str());
+		}
+	}
+
 
 	//parameters
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -185,8 +204,13 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 		ROS_INFO("Using OdometryBOW");
 		odometry_ = new rtabmap::OdometryBOW(parameters_);
 	}
+	if(!initialPose.isIdentity())
+	{
+		odometry_->reset(initialPose);
+	}
 
 	resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
+	resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
 	pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
 	resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
 }
@@ -380,6 +404,14 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 	return true;
 }
 
+bool OdometryROS::resetToPose(rtabmap::ResetPose::Request& req, rtabmap::ResetPose::Response&)
+{
+	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
+	ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
+	odometry_->reset(pose);
+	return true;
+}
+
 bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	if(paused_)

src/OdometryROS.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_srvs/Empty.h>
 #include <std_msgs/Header.h>
 
+#include <rtabmap/ResetPose.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 
@@ -53,6 +54,7 @@ public:
 	Transform processData(SensorData & data, const std_msgs::Header & header, int & quality);
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetToPose(rtabmap::ResetPose::Request&, rtabmap::ResetPose::Response&);
 	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 
@@ -77,6 +79,7 @@ private:
 	ros::Publisher odomLastFrame_;
 	ros::Publisher odomMatches_;
 	ros::ServiceServer resetSrv_;
+	ros::ServiceServer resetToPoseSrv_;
 	ros::ServiceServer pauseSrv_;
 	ros::ServiceServer resumeSrv_;
 	tf::TransformBroadcaster tfBroadcaster_;

src/rviz/MapCloudDisplay.cpp

@@ -258,8 +258,8 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 				float cy = map.nodes[i].cy;
 
 				//uncompress data
-				util3d::CompressionThread ctImage(map.nodes[i].image.bytes, true);
+				util3d::CompressionThread ctImage(&map.nodes[i].image.bytes, true);
-				util3d::CompressionThread ctDepth(map.nodes[i].depth.bytes, true);
+				util3d::CompressionThread ctDepth(&map.nodes[i].depth.bytes, true);
 				ctImage.start();
 				ctDepth.start();
 				ctImage.join();

srv/ResetPose.srv

@@ -0,0 +1,9 @@
+#request
+float32 x
+float32 y
+float32 z
+float32 roll
+float32 pitch
+float32 yaw
+---
+#response