diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
index 5481c3ed4740986076122945dfa8dbb7ab2f2a54..c5978454f635973af9041ebe606c9d597aef1230 100644
--- a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
+++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
@@ -235,6 +235,7 @@ void RrtGuiWindow::loadScene()
VR_ERROR << " no scene ..." << endl;
return;
}
+ //SceneIO::saveScene(scene,"testSaveScene.xml");
std::vector< RobotPtr > robots = scene->getRobots();
if (robots.size()!=1)
{
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.cpp b/VirtualRobot/CollisionDetection/CollisionModel.cpp
index 4e759423d5c499bc656aaa22aa8d8b5492e843fa..9b9596dce76d0427e013e23e50e1243bb959cb66 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.cpp
+++ b/VirtualRobot/CollisionDetection/CollisionModel.cpp
@@ -3,6 +3,7 @@
#include "CollisionChecker.h"
#include "../Visualization/TriMeshModel.h"
#include "../Visualization/VisualizationNode.h"
+#include "../XML/BaseIO.h"
#include <algorithm>
namespace VirtualRobot {
@@ -168,7 +169,30 @@ VirtualRobot::VisualizationNodePtr CollisionModel::getModelDataVisualization()
return modelVisualization;
}
+std::string CollisionModel::getXMLString(const std::string &basePath, int tabs)
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
+ ss << pre << "<CollisionModel";
+ if (getVisualization()->usedBoundingBoxVisu())
+ {
+ ss << " BoundingBox='true'";
+ }
+ ss << ">\n";
+ std::string fnC = getVisualization()->getFilename();
+ if (!fnC.empty())
+ {
+ if (!basePath.empty())
+ BaseIO::makeRelativePath(basePath,fnC);
+ ss << pre << t << "<File type='" << getVisualization()->getType() << "'>" << fnC << "</File>\n";
+ }
+ ss << pre << "</CollisionModel>\n";
+ return ss.str();
+}
/*
void CollisionModel::GetAABB( SbBox3f& store_aabb )
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.h b/VirtualRobot/CollisionDetection/CollisionModel.h
index 9ebb2c81e13dc74c32222c65d4affa0c0169e80f..e6f477a906920c2c556320ea925a3b5b02e0051c 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.h
+++ b/VirtualRobot/CollisionDetection/CollisionModel.h
@@ -118,11 +118,12 @@ public:
TriMeshModelPtr getTriMeshModel(){return collisionModelImplementation->getTriMeshModel();}
+ std::string getXMLString(const std::string &basePath, int tabs);
+
protected:
//! delete all data
void destroyData();
-
VisualizationNodePtr visualization; // this is the original visualization
VisualizationNodePtr modelVisualization; // this is the visualization of the trimeshmodel
bool updateVisualization;
diff --git a/VirtualRobot/Grasp.cpp b/VirtualRobot/Grasp.cpp
index d47d89d8deb7bd74808a440f08670d47ce65c6c3..544b6acf17edcffc570fc1fa15757eb0759304da 100644
--- a/VirtualRobot/Grasp.cpp
+++ b/VirtualRobot/Grasp.cpp
@@ -82,20 +82,23 @@ void Grasp::setName( const std::string &name )
this->name = name;
}
-std::string Grasp::getXMLString()
+std::string Grasp::getXMLString(int tabs)
{
std::stringstream ss;
- std::string t = "\t";
- std::string tt = "\t\t";
- std::string ttt = "\t\t\t";
- std::string tttt = "\t\t\t\t";
-
- ss << tt << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation << "'>\n";
- ss << ttt<< "<Transform>\n";
- ss << MathTools::getTransformXMLString(poseTcp, tttt);
- ss << ttt << "</Transform>\n";
-
- ss << tt << "</Grasp>\n";
+ std::string t;
+ for (int i=0;i<tabs;i++)
+ t += "\t";
+ std::string tt = t;
+ tt += "\t";
+ std::string ttt = tt;
+ ttt += "\t";
+
+ ss << t << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation << "'>\n";
+ ss << tt<< "<Transform>\n";
+ ss << MathTools::getTransformXMLString(poseTcp, ttt);
+ ss << tt << "</Transform>\n";
+
+ ss << t << "</Grasp>\n";
return ss.str();
}
diff --git a/VirtualRobot/Grasp.h b/VirtualRobot/Grasp.h
index 1e9531d6fbdca5e2044d194b7bfbc0be73627a6d..e969cbc63af5da5fffa48fd775375102f43df87f 100644
--- a/VirtualRobot/Grasp.h
+++ b/VirtualRobot/Grasp.h
@@ -97,7 +97,7 @@ public:
*/
Eigen::Matrix4f getTransformation();
- std::string getXMLString();
+ std::string getXMLString(int tabs=2);
GraspPtr clone();
diff --git a/VirtualRobot/GraspSet.cpp b/VirtualRobot/GraspSet.cpp
index 6ca9aef87f24b22f72d9b46482fa1ad7c2a9bb32..4fad46b43f54164c8da7caaf549791b6deefbeee 100644
--- a/VirtualRobot/GraspSet.cpp
+++ b/VirtualRobot/GraspSet.cpp
@@ -120,14 +120,16 @@ std::string GraspSet::getEndEffector()
return eef;
}
-std::string GraspSet::getXMLString()
+std::string GraspSet::getXMLString(int tabs)
{
std::stringstream ss;
- std::string t = "\t";
+ std::string t;
+ for (int i=0;i<tabs;i++)
+ t += "\t";
ss << t << "<GraspSet name='" << name << "' RobotType='" << robotType << "' EndEffector='" << eef << "'>\n";
for (size_t i=0;i<grasps.size();i++)
- ss << grasps[i]->getXMLString();
+ ss << grasps[i]->getXMLString(tabs+1);
ss << t << "</GraspSet>\n";
return ss.str();
diff --git a/VirtualRobot/GraspSet.h b/VirtualRobot/GraspSet.h
index 190080b342fe31653f75fc4ee501e704f6a52794..6246d654b350b6fd042294b7b237847fb0102f00 100644
--- a/VirtualRobot/GraspSet.h
+++ b/VirtualRobot/GraspSet.h
@@ -73,7 +73,7 @@ public:
void print();
- std::string getXMLString();
+ std::string getXMLString(int tabs=1);
GraspSetPtr clone();
diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp
index 0db5aaf340afdd0d34f84fd628b2f8585a74877a..524afcf90a8a4eefaec627e3b5ed3d5f01fb6fd1 100644
--- a/VirtualRobot/ManipulationObject.cpp
+++ b/VirtualRobot/ManipulationObject.cpp
@@ -76,54 +76,44 @@ VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet( const std::string &ro
return GraspSetPtr();
}
-std::string ManipulationObject::getXMLString(const std::string &basePath)
+std::string ManipulationObject::getXMLString(const std::string &basePath, int tabs, bool storeLinkToFile)
{
std::stringstream ss;
std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
- ss << "<ManipulationObject name='" << name << "'>\n";
- if (visualizationModel)
+ ss << pre << "<ManipulationObject name='" << name << "'>\n";
+
+ if (storeLinkToFile)
{
- ss << t << "<Visualization";
- if (visualizationModel->usedBoundingBoxVisu())
+ std::string relFile = filename;
+ if (!basePath.empty())
{
- ss << " BoundingBox='true'";
+ BaseIO::makeRelativePath(basePath,relFile);
}
- ss << ">\n";
- std::string fnV = visualizationModel->getFilename();
- if (!fnV.empty())
+ ss << pre << t << "<File>" << relFile << "</File>\n";
+ Eigen::Matrix4f gp = getGlobalPose();
+ if (!gp.isIdentity())
{
- if (!basePath.empty())
- BaseIO::makeRelativePath(basePath,fnV);
- ss << t << t << "<File type='" << visualizationModel->getType() << "'>" << fnV << "</File>\n";
+ ss << pre << t << "<GlobalPose>\n";
+ ss << pre << t << t << "<Transform>\n";
+ ss << MathTools::getTransformXMLString(gp,tabs+3);
+ ss << pre << t << t << "</Transform>\n";
+ ss << pre << t << "</GlobalPose>\n";
}
- ss << t << "</Visualization>\n";
- }
-
- if (collisionModel && collisionModel->getVisualization())
+ } else
{
- ss << t << "<CollisionModel";
- if (collisionModel->getVisualization()->usedBoundingBoxVisu())
+ ss << getSceneObjectXMLString(basePath,tabs+1);
+
+ for (size_t i=0;i<graspSets.size();i++)
{
- ss << " BoundingBox='true'";
+ ss << graspSets[i]->getXMLString(tabs+1) << "\n";
}
- ss << ">\n";
- std::string fnC = collisionModel->getVisualization()->getFilename();
- if (!fnC.empty())
- {
- if (!basePath.empty())
- BaseIO::makeRelativePath(basePath,fnC);
- ss << t << t << "<File type='" << collisionModel->getVisualization()->getType() << "'>" << fnC << "</File>\n";
- }
- ss << t << "</CollisionModel>\n";
- }
- ss << "\n";
- for (size_t i=0;i<graspSets.size();i++)
- {
- ss << graspSets[i]->getXMLString() << "\n";
}
- ss << "</ManipulationObject>\n";
+ ss << pre << "</ManipulationObject>\n";
return ss.str();
}
@@ -152,6 +142,16 @@ ManipulationObjectPtr ManipulationObject::clone( const std::string &name, Collis
return result;
}
+void ManipulationObject::setFilename( const std::string &filename )
+{
+ this->filename = filename;
+}
+
+std::string ManipulationObject::getFilename()
+{
+ return filename;
+}
+
} // namespace
diff --git a/VirtualRobot/ManipulationObject.h b/VirtualRobot/ManipulationObject.h
index 400ce2083e1cf4fe1f194ef04d692358af2f5cc9..4afd1adf0d18b0d53242b2fc2f3f71fe2bcdc988 100644
--- a/VirtualRobot/ManipulationObject.h
+++ b/VirtualRobot/ManipulationObject.h
@@ -67,14 +67,26 @@ public:
*/
GraspSetPtr getGraspSet(const std::string &robotType,const std::string &eefName);
- std::string getXMLString(const std::string &basePath = std::string());
+ /*!
+ Creates an XML representation of this object.
+ \param basePath If set, all visualization and collision model files are made relative to this path.
+ \param tabs Create indention at the beginning of each line.
+ \param storeLinkToFile If set, the data (e.g. grasps) are not explicitly listed, but an xml tag directing to the XML file,
+ from which this instance was loaded, is set. If not set a deep description is created.
+ */
+ std::string getXMLString(const std::string &basePath = std::string(), int tabs = 0, bool storeLinkToFile = false);
/*!
Clones this object. If no col checker is given, the one of the original object is used.
*/
ManipulationObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() );
+ void setFilename(const std::string &filename);
+ std::string getFilename();
+
protected:
+
+ std::string filename;
std::vector< GraspSetPtr > graspSets;
};
diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 4c7e302c565366ea6606403bba84b702b758691b..b43d4730385ebbef256badf788d448d27f625cd5 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -620,15 +620,46 @@ Eigen::Matrix4f MathTools::quat2eigen4f( float x, float y, float z, float w )
//m(3,3) = w*w + x*x + y*y + z*z;*/
}
-std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs)
+std::string MathTools::getTransformXMLString(const Eigen::Matrix3f &m, int tabs, bool skipMatrixTag)
+{
+ std::string t;
+ for (int i=0;i<tabs;i++)
+ t += "\t";
+ return getTransformXMLString(m,t,skipMatrixTag);
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, int tabs, bool skipMatrixTag)
+{
+ std::string t;
+ for (int i=0;i<tabs;i++)
+ t += "\t";
+ return getTransformXMLString(m,t,skipMatrixTag);
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs, bool skipMatrixTag)
{
std::stringstream ss;
- ss << tabs << "<Matrix4x4>\n";
+ if (!skipMatrixTag)
+ ss << tabs << "<Matrix4x4>\n";
ss << tabs << "\t<row1 c1='" << m(0,0) << "' c2='" << m(0,1) << "' c3='" << m(0,2) << "' c4='" << m(0,3) << "'/>\n";
ss << tabs << "\t<row2 c1='" << m(1,0) << "' c2='" << m(1,1) << "' c3='" << m(1,2) << "' c4='" << m(1,3) << "'/>\n";
ss << tabs << "\t<row3 c1='" << m(2,0) << "' c2='" << m(2,1) << "' c3='" << m(2,2) << "' c4='" << m(2,3) << "'/>\n";
ss << tabs << "\t<row4 c1='" << m(3,0) << "' c2='" << m(3,1) << "' c3='" << m(3,2) << "' c4='" << m(3,3) << "'/>\n";
- ss << tabs << "</Matrix4x4>\n";
+ if (!skipMatrixTag)
+ ss << tabs << "</Matrix4x4>\n";
+ return ss.str();
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix3f &m, const std::string &tabs, bool skipMatrixTag)
+{
+ std::stringstream ss;
+ if (!skipMatrixTag)
+ ss << tabs << "<Matrix3x3>\n";
+ ss << tabs << "\t<row1 c1='" << m(0,0) << "' c2='" << m(0,1) << "' c3='" << m(0,2) << "'/>\n";
+ ss << tabs << "\t<row2 c1='" << m(1,0) << "' c2='" << m(1,1) << "' c3='" << m(1,2) << "'/>\n";
+ ss << tabs << "\t<row3 c1='" << m(2,0) << "' c2='" << m(2,1) << "' c3='" << m(2,2) << "'/>\n";
+ if (!skipMatrixTag)
+ ss << tabs << "</Matrix3x3>\n";
return ss.str();
}
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index c8a834a89c93adba0e1d42332b5400603774798e..1e2378c9824658c334be2a5678753e9ae640069c 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -349,7 +349,10 @@ namespace MathTools
void VIRTUAL_ROBOT_IMPORT_EXPORT print (const std::vector<ContactPoint> &points);
void VIRTUAL_ROBOT_IMPORT_EXPORT print( const Eigen::Vector3f &v, bool endline = true );
void VIRTUAL_ROBOT_IMPORT_EXPORT print( const std::vector<float> &v, bool endline = true);
- std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs);
+ std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, int tabs, bool skipMatrixTag = false);
+ std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs, bool skipMatrixTag = false);
+ std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix3f &m, int tabs, bool skipMatrixTag = false);
+ std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix3f &m, const std::string &tabs, bool skipMatrixTag = false);
void VIRTUAL_ROBOT_IMPORT_EXPORT convertMM2M( const std::vector<ContactPoint> points, std::vector<ContactPoint> &storeResult );
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 8aaf9dff2e9a7ca4680e17ec7d17f71e44fd47e8..bceda143638fe7ce235db76ab1a444a064960ac0 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -113,6 +113,23 @@ ObstaclePtr Obstacle::clone( const std::string &name, CollisionCheckerPtr colChe
return result;
}
+std::string Obstacle::getXMLString(const std::string &basePath, int tabs)
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
+
+ ss << pre << "<Obstacle name='" << name << "'>\n";
+
+ ss << getSceneObjectXMLString(basePath,tabs);
+
+ ss << pre << "</Obstacle>\n";
+
+ return ss.str();
+}
+
} // namespace
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index 90a9b1bbac78ca050ad8d653213144abd862c814..ec5671cc7df14f108aa4be30b39fdc4ff342a399 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -60,7 +60,7 @@ public:
*/
static ObstaclePtr createBox(float width, float height, float depth, float colorR = 1.0f, float colorG = 0.0f, float colorB=0.0f, std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
-
+ std::string getXMLString(const std::string &basePath, int tabs=0);
protected:
// a counter for internal ids
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index b1d0db84a31bf05360745ede3a6e83d88fbe33be..19868c234d873ec422694151a50bbccdddb5cf9c 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -666,6 +666,16 @@ bool Robot::setConfig( RobotConfigPtr c )
return false;
}
+void Robot::setFilename( const std::string &filename )
+{
+ this->filename = filename;
+}
+
+std::string Robot::getFilename()
+{
+ return filename;
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 1e9f9897d767e2a6aa8f6a8917b755f666b1ee73..6a01675caee0264d89b3ccf4c8f126fd415a0d61 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -227,6 +227,9 @@ public:
*/
RobotPtr clone(const std::string &name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr());
+ void setFilename(const std::string &filename);
+ std::string getFilename();
+
protected:
Robot();
/*!
@@ -236,7 +239,7 @@ protected:
*/
void createVisualizationFromCollisionModels();
-
+ std::string filename; // RobotIO stores the filename here
std::string name;
std::string type;
diff --git a/VirtualRobot/RobotConfig.cpp b/VirtualRobot/RobotConfig.cpp
index 6efca1ed2b86d26c69c069beffdc50eb83d88520..83f99c7ad03dd4836cadab0b451ac07b6e7a52d5 100644
--- a/VirtualRobot/RobotConfig.cpp
+++ b/VirtualRobot/RobotConfig.cpp
@@ -216,7 +216,30 @@ bool RobotConfig::applyToRobot( RobotPtr r )
return true;
}
+std::string RobotConfig::getXMLString(int tabs)
+{
+ std::stringstream ss;
+ std::string t;
+ for (int i=0;i<tabs;i++)
+ t += "\t";
+ std::string tt = t;
+ tt += "\t";
+ std::string ttt = tt;
+ ttt += "\t";
+
+ ss << t << "<Configuration name='" << name << "'>\n";
+
+ std::map < std::string, float > jv = getRobotNodeJointvalueMap();
+ std::map< std::string, float >::const_iterator i = jv.begin();
+ while (i != jv.end())
+ {
+ ss << tt << "<Node name='" << i->first << "' unit='radian' value='" << i->second << "'/>\n";
+ i++;
+ }
+ ss << t << "</Configuration>\n";
+ return ss.str();
+}
diff --git a/VirtualRobot/RobotConfig.h b/VirtualRobot/RobotConfig.h
index 2061f781b8b876795672f147d69c09eedb7c318c..83c9e05eb6145522438088910ec1846c67473d88 100644
--- a/VirtualRobot/RobotConfig.h
+++ b/VirtualRobot/RobotConfig.h
@@ -101,6 +101,8 @@ public:
apply the joint values to a cloned robot. Therefore this method can be used.
*/
bool applyToRobot(RobotPtr r);
+
+ std::string getXMLString(int tabs=0);
protected:
diff --git a/VirtualRobot/Scene.cpp b/VirtualRobot/Scene.cpp
index 037bf8709c3d785442896b219f4c82a3296b2d99..ee0d3257a4a7863995fee5a66ec0a4771a1939fb 100644
--- a/VirtualRobot/Scene.cpp
+++ b/VirtualRobot/Scene.cpp
@@ -3,6 +3,7 @@
#include "VirtualRobotException.h"
#include "ManipulationObject.h"
#include "SceneObjectSet.h"
+#include "XML/BaseIO.h"
namespace VirtualRobot
{
@@ -446,6 +447,77 @@ std::vector< SceneObjectSetPtr > Scene::getSceneObjectSets()
return sceneObjectSets;
}
+std::string Scene::getXMLString( const std::string &basePath )
+{
+ std::stringstream ss;
+ ss << "<Scene";
+ if (!name.empty())
+ ss << " name='" << name << "'";
+ ss << ">\n";
+ ss << "\n";
+
+ // process robots
+ for (size_t i=0;i<robots.size();i++)
+ {
+ std::string rob = robots[i]->getName();
+ RobotConfigPtr currentConfig = robots[i]->getConfig();
+ ss << "\t<Robot name='" << rob << "' initConfig='" << currentConfig->getName() << "'>\n";
+ std::string robFile = robots[i]->getFilename();
+ if (!basePath.empty())
+ BaseIO::makeRelativePath(basePath,robFile);
+
+ ss << "\t\t<File>" << robFile << "</File>\n";
+
+ // store global pose (if not identity)
+ Eigen::Matrix4f gp = robots[i]->getGlobalPose();
+ if (!gp.isIdentity())
+ {
+ ss << "\t\t<GlobalPose>\n";
+ ss << "\t\t\t<Transform>\n";
+ ss << MathTools::getTransformXMLString(gp,4);
+ ss << "\t\t\t</Transform>\n";
+ ss << "\t\t</GlobalPose>\n";
+ }
+ // store current config
+ ss << currentConfig->getXMLString(2);
+
+ // store all other configs for robot
+ std::vector<RobotConfigPtr> rc = getRobotConfigs(robots[i]);
+ for (size_t j=0;j<rc.size();j++)
+ {
+ ss << rc[j]->getXMLString(2);
+ }
+ ss << "\t</Robot>\n";
+ ss << "\n";
+ }
+
+ // process manipulation objects
+ for (size_t i=0;i<manipulationObjects.size();i++)
+ {
+ ss << manipulationObjects[i]->getXMLString(basePath,1,true);
+ ss << "\n";
+ }
+
+ // process obstacles
+ for (size_t i=0;i<obstacles.size();i++)
+ {
+ ss << obstacles[i]->getXMLString(basePath,1);
+ ss << "\n";
+ }
+
+
+ // process sceneObjectSets
+ for (size_t i=0;i<sceneObjectSets.size();i++)
+ {
+ ss << sceneObjectSets[i]->getXMLString(1);
+ ss << "\n";
+ }
+
+ ss << "</Scene>\n";
+
+ return ss.str();
+}
+
} // namespace
diff --git a/VirtualRobot/Scene.h b/VirtualRobot/Scene.h
index 918080ee268a43e88da48319c78d658c07853b5b..2d50918d947548b0253e297e678fbcd261b89fb7 100644
--- a/VirtualRobot/Scene.h
+++ b/VirtualRobot/Scene.h
@@ -165,6 +165,13 @@ public:
*/
template <typename T> boost::shared_ptr<T> getVisualization(SceneObject::VisualizationType visuType = SceneObject::Full);
+
+ /*!
+ Creates an XML string that describes this scene.
+ \param basePath All paths to robots or objects are stored relative to this path.
+ \return The xml string.
+ */
+ std::string getXMLString(const std::string &basePath);
protected:
std::string name;
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index c01fe879c4b1fb48c34b2c78cec7e9f40294ef1b..ff0fb0f842c287a7093f66ca4c62d6407567d254 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -457,5 +457,37 @@ Eigen::Matrix3f SceneObject::getInertiaMatrix()
return physics.intertiaMatrix;
}
+std::string SceneObject::getSceneObjectXMLString(const std::string &basePath, int tabs)
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
+ if (visualizationModel)
+ {
+ ss << visualizationModel->getXMLString(basePath,tabs);
+ }
+
+ if (collisionModel && collisionModel->getVisualization())
+ {
+ ss << collisionModel->getXMLString(basePath,tabs);
+ }
+ Eigen::Matrix4f gp = getGlobalPose();
+ if (!gp.isIdentity())
+ {
+ ss << pre << "<GlobalPose>\n";
+ ss << pre << "\t<Transform>\n";
+ ss << MathTools::getTransformXMLString(gp,tabs+2);
+ ss << pre << "\t</Transform>\n";
+ ss << pre << "</GlobalPose>\n";
+ }
+ if (physics.isSet())
+ {
+ ss << physics.getXMLString(tabs);
+ }
+
+ return ss.str();
+}
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 99ded24ef3dcaf81cd0cd540c7a9cf106563a92d..826f56e7f08692636625c992816b3c38590449a6 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -69,6 +69,34 @@ public:
std::cout << " ** local CoM [mm]: " << localCoM(0) << localCoM(1) << localCoM(2) << std::endl;
std::cout << " ** inertia matrix [kg*m^2] :\n " << intertiaMatrix << std::endl;
}
+
+ bool isSet()
+ {
+ return (massKg!=0.0f || comLocation!=eCustom || !localCoM.isZero() || !intertiaMatrix.isIdentity());
+ }
+
+ std::string getXMLString(int tabs)
+ {
+ std::string ta;
+ std::stringstream ss;
+ for (int i=0;i<tabs;i++)
+ ta += "\t";
+ ss << ta << "<Physics>\n";
+ ss << ta << "\t<Mass unit='kg' value='" << massKg << "'/>\n";
+ ss << ta << "\t<CoM location=";
+ if (comLocation==eVisuBBoxCenter)
+ ss << "'VisualizationBBoxCenter'/>\n";
+ else
+ {
+ ss << "'Custom' x='" << localCoM(0) << "' y='" << localCoM(1) << "' z='" << localCoM(2) << "'/>\n";
+ }
+ ss << ta << "\t<InertiaMatrix>\n";
+ ss << MathTools::getTransformXMLString(intertiaMatrix,tabs+2,true);
+ ss << ta << "\t</InertiaMatrix>\n";
+ ss << ta << "</Physics>\n";
+ return ss.str();
+ }
+
Eigen::Vector3f localCoM; //!< Defined in the local coordinate system of this object [mm]
float massKg; //!< The mass of this object
CoMLocation comLocation; //!< Where is the CoM located
@@ -255,6 +283,10 @@ public:
protected:
SceneObject(){};
+
+ //! basic data, used by Obstacle and ManipulationObject
+ std::string getSceneObjectXMLString(const std::string &basePath, int tabs);
+
///////////////////////// SETUP ////////////////////////////////////
std::string name;
bool initialized; //< Invalid object when false
diff --git a/VirtualRobot/SceneObjectSet.cpp b/VirtualRobot/SceneObjectSet.cpp
index 1261c352a8cb8f630a318cccd45475c98a692cb7..795932c032db4889473dacfb8bff68117bc03b32 100644
--- a/VirtualRobot/SceneObjectSet.cpp
+++ b/VirtualRobot/SceneObjectSet.cpp
@@ -244,6 +244,24 @@ VirtualRobot::SceneObjectPtr SceneObjectSet::getSceneObject( unsigned int nr )
return sceneObjects[nr];
}
+std::string SceneObjectSet::getXMLString( int tabs )
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
+
+ ss << pre << "<SceneObjectSet name='" << name << "'>\n";
+ for (size_t i=0;i<sceneObjects.size();i++)
+ {
+ ss << pre << t << "<SceneObject name='" << sceneObjects[i]->getName() << "'/>\n";
+ }
+
+ ss << pre << "</SceneObjectSet>\n";
+ return ss.str();
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/SceneObjectSet.h b/VirtualRobot/SceneObjectSet.h
index e895c602806f19a0436356b66a6c6e5f69aa9e84..0218da57ccfc68498d2bba3eaffd188fe60b61af 100644
--- a/VirtualRobot/SceneObjectSet.h
+++ b/VirtualRobot/SceneObjectSet.h
@@ -100,6 +100,7 @@ public:
//! fills the current globalPose of all associated sceneobjects to map.
bool getCurrentSceneObjectConfig( std::map< SceneObjectPtr, Eigen::Matrix4f > &storeConfig );
+ std::string getXMLString( int tabs);
protected:
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 5ca1ad640e6bb28225b708647c48908a64de37e7..89443701cb1cc69cab223cdc673acd7cc33968d4 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -11,6 +11,7 @@
#include "VirtualRobot/VirtualRobot.h"
#include "VirtualRobot/VirtualRobotException.h"
+#include "VirtualRobot/XML/BaseIO.h"
namespace VirtualRobot {
@@ -118,4 +119,31 @@ std::string VisualizationNode::getFilename()
return filename;
}
+
+std::string VisualizationNode::getXMLString(const std::string &basePath, int tabs)
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += "\t";
+
+ ss << pre << "<Visualization";
+ if (usedBoundingBoxVisu())
+ {
+ ss << " BoundingBox='true'";
+ }
+ ss << ">\n";
+ std::string fnV = getFilename();
+ if (!fnV.empty())
+ {
+ if (!basePath.empty())
+ BaseIO::makeRelativePath(basePath,fnV);
+ ss << pre << t << "<File type='" << getType() << "'>" << fnV << "</File>\n";
+ }
+ ss << pre << "</Visualization>\n";
+
+ return ss.str();
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/VisualizationNode.h b/VirtualRobot/Visualization/VisualizationNode.h
index 199275bae9283ffd6225f20093e49e104d8791df..96e3745a46bd4be8c05824e6a5aec625844a2579 100644
--- a/VirtualRobot/Visualization/VisualizationNode.h
+++ b/VirtualRobot/Visualization/VisualizationNode.h
@@ -118,6 +118,7 @@ public:
virtual std::string getType(){return VisualizationFactory::getName();}
+ std::string getXMLString(const std::string &basePath, int tabs);
protected:
bool boundingBox; //!< Indicates, if the bounding box model was used
std::string filename; //!< if the visualization was build from a file, the filename is stored here
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index 25eae0891e110a30bb91f99c408cf5d72c8c5005..a235ed6f61afa92cf5a5407053e60df23ca5a283 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -39,7 +39,7 @@ VirtualRobot::ManipulationObjectPtr ObjectIO::loadManipulationObject( const std:
VirtualRobot::ManipulationObjectPtr res = createManipulationObjectFromString(objectXML, basePath);
THROW_VR_EXCEPTION_IF (!res,"Error while parsing file " << xmlFile);
-
+ res->setFilename(xmlFile);
return res;
}
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index fc1debec561475a4061c9d82381845d9f35d3c20..5e14d6301c88777a404b821132cc654b6be07271 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -1046,6 +1046,7 @@ VirtualRobot::RobotPtr RobotIO::loadRobot(const std::string &xmlFile, RobotDescr
}
res->applyJointValues();
+ res->setFilename(xmlFile);
return res;
}
diff --git a/VirtualRobot/XML/SceneIO.cpp b/VirtualRobot/XML/SceneIO.cpp
index 8597888730f4a750de46eac518c796f16d980fab..319c1be27821e437ef4235d0bc22c9e517db30d7 100644
--- a/VirtualRobot/XML/SceneIO.cpp
+++ b/VirtualRobot/XML/SceneIO.cpp
@@ -319,5 +319,32 @@ VirtualRobot::ScenePtr SceneIO::createSceneFromString( const std::string &xmlStr
return scene;
}
+bool SceneIO::saveScene( ScenePtr s, const std::string &xmlFile )
+{
+ if (!s)
+ {
+ VR_ERROR << "NULL data..." << endl;
+ return false;
+ }
+
+ boost::filesystem::path filenameBaseComplete(xmlFile);
+ filenameBaseComplete = boost::filesystem::complete(filenameBaseComplete);
+ boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path();
+ std::string basePath = filenameBasePath.string();
+
+ std::string xmlString = s->getXMLString(basePath);
+
+ // save file
+ std::ofstream out(xmlFile.c_str());
+
+ if (!out.is_open())
+ return false;
+
+ out << xmlString;
+ out.close();
+
+ return true;
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/XML/SceneIO.h b/VirtualRobot/XML/SceneIO.h
index 9a0a310a5276b3816ef85ce4a395c49916d164f7..029bb4fb6d4bc74f8045b49174676418d2831006 100644
--- a/VirtualRobot/XML/SceneIO.h
+++ b/VirtualRobot/XML/SceneIO.h
@@ -42,15 +42,23 @@ class VIRTUAL_ROBOT_IMPORT_EXPORT SceneIO : public BaseIO
public:
/*!
- Loads scene from file.
- @param xmlFile The file
- @return Returns an empty pointer, when file access failed.
+ Load scene from file.
+ \param xmlFile The file
+ \return Returns an empty pointer, when file access failed.
*/
- static ScenePtr loadScene(const std::string &xmlFile);
+ static ScenePtr loadScene(const std::string &xmlFile);
+
+ /*!
+ Save a scene to file.
+ \param s The scen to be saved.
+ \param xmlFile The absolute filename.
+ \return true on success.
+ */
+ static bool saveScene(ScenePtr s, const std::string &xmlFile);
/*!
Creates scene from string.
- @param basePath If any robot tags are given, the base path for searching the robot files can be specified.
+ \param basePath If any robot tags are given, the base path for searching the robot files can be specified.
*/
static ScenePtr createSceneFromString(const std::string &xmlString, const std::string &basePath = "");
diff --git a/VirtualRobot/examples/SceneViewer/SceneViewer.cpp b/VirtualRobot/examples/SceneViewer/SceneViewer.cpp
index 30aae3be41d136e811f81672222102259e84dbec..08e17534055dbbc2dcd90a753ddb93356fcba319 100644
--- a/VirtualRobot/examples/SceneViewer/SceneViewer.cpp
+++ b/VirtualRobot/examples/SceneViewer/SceneViewer.cpp
@@ -30,7 +30,8 @@ int main(int argc, char *argv[])
SoDB::init();
SoQt::init(argc,argv,"showRobot");
cout << " --- START --- " << endl;
- std::string filename(VR_BASE_DIR "/examples/showScene/scene1.xml");
+ std::string filename(VR_BASE_DIR "/examples/SceneViewer/scene1.xml");
+ // --scene scenes/lego.xml
//std::string filename(VR_BASE_DIR "/examples/showScene/sceneiCub.xml");
VirtualRobot::RuntimeEnvironment::considerKey("scene");