Support for long-term buildup of reachability data

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@345 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/examples/reachability/reachabilityScene.cpp

@@ -17,6 +17,11 @@
 
 #include "reachabilityWindow.h"
 
+// this flag can be used to build a good representation of the workspace
+// the reachability data will be extended in endless mode and 
+// after every 1 mio-th update a snapshot is saved.
+//#define ENDLESS
+
 //#define ICUB
 //#define AXIS_X
 
@@ -25,6 +30,58 @@
 using std::cout;
 using std::endl;
 using namespace VirtualRobot;
+
+
+
+void endlessExtend(std::string robotFile, std::string reachFile)
+{
+	int steps = 1000000;
+	
+	VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robotFile);
+	VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFile);
+	
+	// load robot
+	RobotPtr robot;
+	try
+	{
+		robot = RobotIO::loadRobot(robotFile);
+	}
+	catch (VirtualRobotException &e)
+	{
+		cout << " ERROR while creating robot" << endl;
+		cout << e.what();
+		return;
+	}
+	
+	if (!robot)
+	{
+		cout << " ERROR while creating robot" << endl;
+		return;
+	}
+	
+	// load reach file
+	ReachabilityPtr reachSpace(new Reachability(robot));
+	reachSpace->load(reachFile);
+	reachSpace->print();
+
+	time_t time_now = time(NULL);
+	struct tm * timeinfo;
+	timeinfo = localtime (&time_now);
+	char buffer [255];
+	strftime (buffer,255,"ReachabilityData_ENDLESS_%Y_%m_%d__%H_%M_%S_",timeinfo);
+	int nr = 0;
+	while (true)
+	{
+		reachSpace->addRandomTCPPoses(steps);
+		reachSpace->print();
+		std::stringstream ss;
+		ss << buffer << "_" << nr << ".bin"; 
+		reachSpace->save(ss.str());
+		nr++;
+	}
+	
+}
+
 int main(int argc, char *argv[])
 {
 	SoDB::init();
@@ -39,6 +96,7 @@ int main(int argc, char *argv[])
 #else
 	std::string filenameRob("robots/ArmarIII/ArmarIII.xml");
 	Eigen::Vector3f axisTCP(0,0,1.0f);	
+	filenameReach = "reachability/ArmarIII_PlatformHipRightArm.bin";
 #endif
 	VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filenameRob);
 
@@ -67,11 +125,13 @@ int main(int argc, char *argv[])
 	cout << "Using robot at " << filenameRob << endl;
 	cout << "Using reachability file from " << filenameReach << endl;
 
+#ifdef ENDLESS
+	endlessExtend(filenameRob,filenameReach);
+	return 0;
+#endif
 
 	reachabilityWindow rw(filenameRob,filenameReach,axisTCP);
-
 	rw.main();
 
 	return 0;
-
 }

VirtualRobot/examples/reachability/reachabilityWindow.cpp

@@ -26,7 +26,6 @@ using namespace VirtualRobot;
 
 float TIMER_MS = 30.0f;
 
-//#define ENDLESS
 
 reachabilityWindow::reachabilityWindow(std::string &sRobotFile, std::string &reachFile, Eigen::Vector3f &axisTCP, Qt::WFlags flags)
 :QMainWindow(NULL)
@@ -366,7 +365,7 @@ void reachabilityWindow::extendReach()
 		return;
 	}
 	int steps = UI.spinBoxExtend->value();
-#ifdef ENDLESS
+/*#ifdef ENDLESS
 	time_t time_now = time(NULL);
 	struct tm * timeinfo;
 	timeinfo = localtime (&time_now);
@@ -383,7 +382,7 @@ void reachabilityWindow::extendReach()
 		nr++;
 	}
 
-#endif
+#endif*/
 	reachSpace->addRandomTCPPoses(steps);
 	reachSpace->print();
 }