From 09ff77e30dbc0034823a00a5a554a76167db0460 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Sat, 17 Mar 2012 12:20:44 +0000
Subject: [PATCH] Fixing cyclic references between RobotNodes and Robot which
resulted in memory leaks. Adding trajectory support to Scene. Adding
trajectory IO and visualization methods.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@216 042f3d55-54a8-47e9-b7fb-15903f145c44
---
VirtualRobot/Nodes/RobotNode.cpp | 3 +-
VirtualRobot/Nodes/RobotNode.h | 4 +-
VirtualRobot/Robot.cpp | 8 +-
VirtualRobot/Scene.cpp | 93 ++++++++++-
VirtualRobot/Scene.h | 26 +++-
VirtualRobot/Trajectory.cpp | 34 +++-
VirtualRobot/Trajectory.h | 13 +-
.../CoinVisualizationFactory.cpp | 104 +++++++++++++
.../CoinVisualizationFactory.h | 3 +
.../OSGVisualizationFactory.cpp | 6 +
.../OSGVisualizationFactory.h | 1 +
.../Visualization/VisualizationFactory.h | 1 +
.../Visualization/VisualizationNode.cpp | 1 +
VirtualRobot/XML/BaseIO.cpp | 145 ++++++++++++++++++
VirtualRobot/XML/BaseIO.h | 11 ++
VirtualRobot/XML/ObjectIO.cpp | 10 +-
VirtualRobot/XML/SceneIO.cpp | 39 +++--
VirtualRobot/XML/SceneIO.h | 1 +
18 files changed, 476 insertions(+), 27 deletions(-)
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index d0864f4e5..59bbf814e 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -549,7 +549,8 @@ std::vector<RobotNodePtr> RobotNode::getAllParents( RobotNodeSetPtr rns )
VirtualRobot::RobotNodePtr RobotNode::getParent()
{
- return parent;
+ RobotNodePtr p = parent.lock();
+ return p;
}
void RobotNode::setJointLimits( float lo, float hi )
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 3284ee1e5..2e7133709 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -274,7 +274,7 @@ private: // Use the private setters and getters instead
float jointValue; //< The joint value
std::vector<std::string> childrenNames;
std::vector< RobotNodePtr > children;
- RobotNodePtr parent;
+ RobotNodeWeakPtr parent;
Eigen::Matrix4f preJointTransformation;
Eigen::Matrix4f postJointTransformation;
@@ -290,7 +290,7 @@ protected:
//Stefan
virtual std::vector<std::string> getChildrenNames() const {return childrenNames;};
- virtual std::string getParentName() const {if (parent) return parent->getName(); else return std::string();};
+ virtual std::string getParentName() const {RobotNodePtr p = parent.lock();if (p) return p->getName(); else return std::string();};
float jointValueOffset;
float jointLimitLo,jointLimitHi;
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 19868c234..9b439aebe 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -19,11 +19,17 @@ Robot::Robot()
{
}
-Robot::~Robot(){}
+Robot::~Robot()
+{
+}
LocalRobot::~LocalRobot()
{
+ //int i = (int)rootNode.use_count();
robotNodeSetMap.clear();
+ robotNodeMap.clear();
+ rootNode.reset();
+ //i = (int)rootNode.use_count();
}
LocalRobot::LocalRobot(const std::string &name, const std::string &type) : Robot (name, type){
diff --git a/VirtualRobot/Scene.cpp b/VirtualRobot/Scene.cpp
index ee0d3257a..26de7daaa 100644
--- a/VirtualRobot/Scene.cpp
+++ b/VirtualRobot/Scene.cpp
@@ -3,6 +3,7 @@
#include "VirtualRobotException.h"
#include "ManipulationObject.h"
#include "SceneObjectSet.h"
+#include "Trajectory.h"
#include "XML/BaseIO.h"
namespace VirtualRobot
@@ -17,6 +18,12 @@ Scene::Scene(const std::string &name)
Scene::~Scene()
{
+ robots.clear();
+ robotConfigs.clear();
+ obstacles.clear();
+ manipulationObjects.clear();
+ sceneObjectSets.clear();
+ trajectories.clear();
}
void Scene::registerRobot( RobotPtr robot )
@@ -94,6 +101,44 @@ void Scene::deRegisterObstacle( const std::string &name )
}
+
+void Scene::registerTrajectory( TrajectoryPtr t )
+{
+ THROW_VR_EXCEPTION_IF(!t,"NULL data");
+ if (hasTrajectory(t))
+ return;
+ trajectories.push_back(t);
+}
+
+void Scene::deRegisterTrajectory( TrajectoryPtr t )
+{
+ THROW_VR_EXCEPTION_IF(!t,"NULL data");
+ if (!hasTrajectory(t))
+ return;
+ for (std::vector< TrajectoryPtr >::iterator i=trajectories.begin();i!=trajectories.end();i++)
+ {
+ if ( (*i) == t )
+ {
+ trajectories.erase(i);
+ break;
+ }
+ }
+}
+
+void Scene::deRegisterTrajectory( const std::string &name )
+{
+ if (!hasTrajectory(name))
+ return;
+ for (std::vector< TrajectoryPtr >::iterator i=trajectories.begin();i!=trajectories.end();i++)
+ {
+ if ((*i)->getName() == name)
+ {
+ trajectories.erase(i);
+ break;
+ }
+ }
+}
+
void Scene::registerManipulationObject( ManipulationObjectPtr manipulationObject )
{
THROW_VR_EXCEPTION_IF(!manipulationObject,"NULL data");
@@ -174,6 +219,29 @@ bool Scene::hasObstacle( const std::string &name ) const
}
+
+bool Scene::hasTrajectory( TrajectoryPtr t ) const
+{
+ THROW_VR_EXCEPTION_IF(!t,"NULL data");
+ for (std::vector< TrajectoryPtr >::const_iterator i=trajectories.begin();i!=trajectories.end();i++)
+ {
+ if (*i == t)
+ return true;
+ }
+ return false;
+}
+
+bool Scene::hasTrajectory( const std::string &name ) const
+{
+ for (std::vector< TrajectoryPtr >::const_iterator i=trajectories.begin();i!=trajectories.end();i++)
+ {
+ if ((*i)->getName() == name)
+ return true;
+ }
+ return false;
+}
+
+
bool Scene::hasManipulationObject( ManipulationObjectPtr manipulationObject ) const
{
THROW_VR_EXCEPTION_IF(!manipulationObject,"NULL data");
@@ -219,6 +287,18 @@ VirtualRobot::ObstaclePtr Scene::getObstacle( const std::string &name )
return ObstaclePtr();
}
+VirtualRobot::TrajectoryPtr Scene::getTrajectory( const std::string &name )
+{
+ for (std::vector< TrajectoryPtr >::const_iterator i=trajectories.begin();i!=trajectories.end();i++)
+ {
+ if ((*i)->getName() == name)
+ return *i;
+ }
+
+ VR_WARNING << "No Trajectory with name " << name << " registered in scene " << this->name << endl;
+ return TrajectoryPtr();
+}
+
VirtualRobot::ManipulationObjectPtr Scene::getManipulationObject( const std::string &name )
{
for (std::vector< ManipulationObjectPtr >::const_iterator i=manipulationObjects.begin();i!=manipulationObjects.end();i++)
@@ -349,6 +429,11 @@ std::vector< ObstaclePtr > Scene::getObstacles()
return obstacles;
}
+std::vector< TrajectoryPtr > Scene::getTrajectories()
+{
+ return trajectories;
+}
+
std::vector< ManipulationObjectPtr > Scene::getManipulationObjects()
{
return manipulationObjects;
@@ -505,7 +590,6 @@ std::string Scene::getXMLString( const std::string &basePath )
ss << "\n";
}
-
// process sceneObjectSets
for (size_t i=0;i<sceneObjectSets.size();i++)
{
@@ -513,6 +597,13 @@ std::string Scene::getXMLString( const std::string &basePath )
ss << "\n";
}
+ // process trajectories
+ for (size_t i=0;i<trajectories.size();i++)
+ {
+ ss << trajectories[i]->getXMLString(1);
+ ss << "\n";
+ }
+
ss << "</Scene>\n";
return ss.str();
diff --git a/VirtualRobot/Scene.h b/VirtualRobot/Scene.h
index 2d50918d9..e6468ed88 100644
--- a/VirtualRobot/Scene.h
+++ b/VirtualRobot/Scene.h
@@ -29,6 +29,7 @@
#include "RobotConfig.h"
#include "Nodes/RobotNode.h"
#include "Obstacle.h"
+#include "Trajectory.h"
#include "ManipulationObject.h"
#include "RobotConfig.h"
#include <string>
@@ -127,6 +128,25 @@ public:
std::vector< ObstaclePtr > getObstacles();
+ /*!
+ Registers the Trajectory to this scene. If an Trajectory with the same name is already registered nothing happens.
+ */
+ void registerTrajectory(TrajectoryPtr t);
+
+ /*!
+ Removes the Trajectory to from this scene. If the Trajectory is not registered nothing happens.
+ */
+ void deRegisterTrajectory(TrajectoryPtr t);
+ void deRegisterTrajectory(const std::string &name);
+
+ bool hasTrajectory(TrajectoryPtr t) const;
+ bool hasTrajectory(const std::string &name) const;
+
+ TrajectoryPtr getTrajectory(const std::string &name);
+
+ std::vector< TrajectoryPtr > getTrajectories();
+
+
/*!
@@ -181,6 +201,7 @@ protected:
std::vector< ObstaclePtr > obstacles;
std::vector< ManipulationObjectPtr > manipulationObjects;
std::vector< SceneObjectSetPtr > sceneObjectSets;
+ std::vector< TrajectoryPtr > trajectories;
};
@@ -198,19 +219,22 @@ boost::shared_ptr<T> Scene::getVisualization(SceneObject::VisualizationType visu
std::vector<VirtualRobot::RobotPtr> collectedRobots = getRobots();
std::vector<VirtualRobot::ObstaclePtr> collectedObstacles = getObstacles();
std::vector<VirtualRobot::ManipulationObjectPtr> collectedManipulationObjects = getManipulationObjects();
+ std::vector<VirtualRobot::TrajectoryPtr> collectedTrajectories = getTrajectories();
// collect all robotnodes
std::vector<VirtualRobot::RobotNodePtr> collectedRobotNodes;
for (size_t i=0;i<collectedRobots.size();i++)
collectedRobots[i]->getRobotNodes(collectedRobotNodes,false);
- std::vector<VisualizationNodePtr> collectedVisualizationNodes(collectedRobotNodes.size() + collectedObstacles.size() + collectedManipulationObjects.size());
+ std::vector<VisualizationNodePtr> collectedVisualizationNodes(collectedRobotNodes.size() + collectedObstacles.size() + collectedManipulationObjects.size() + collectedTrajectories.size());
for (size_t i=0;i<collectedRobotNodes.size();i++)
collectedVisualizationNodes[i] = collectedRobotNodes[i]->getVisualization(visuType);
for (size_t i=0;i<collectedObstacles.size();i++)
collectedVisualizationNodes[i+collectedRobotNodes.size()] = collectedObstacles[i]->getVisualization(visuType);
for (size_t i=0;i<collectedManipulationObjects.size();i++)
collectedVisualizationNodes[i+collectedRobotNodes.size()+collectedObstacles.size()] = collectedManipulationObjects[i]->getVisualization(visuType);
+ for (size_t i=0;i<collectedTrajectories.size();i++)
+ collectedVisualizationNodes[i+collectedRobotNodes.size()+collectedObstacles.size()+collectedManipulationObjects.size()] = collectedTrajectories[i]->getVisualization(T::getFactoryName());
boost::shared_ptr<T> visualization(new T(collectedVisualizationNodes));
diff --git a/VirtualRobot/Trajectory.cpp b/VirtualRobot/Trajectory.cpp
index 8a4e2b65b..aa1ce070a 100644
--- a/VirtualRobot/Trajectory.cpp
+++ b/VirtualRobot/Trajectory.cpp
@@ -1,4 +1,5 @@
#include "Trajectory.h"
+#include "Robot.h"
#include "VirtualRobotException.h"
#include <iostream>
#include <sstream>
@@ -322,7 +323,12 @@ std::string Trajectory::getXMLString(int tabs) const
for (int i=0;i<tabs;i++)
tab += "\t";
- ss << tab << "<Trajectory RobotNodeSet='" << rns->getName() << "' name='" << name << "' dim=" << dimension << ">\n";
+ RobotPtr robot = rns->getRobot();
+
+ THROW_VR_EXCEPTION_IF(( !robot || !rns),"Need a valid robot and rns");
+
+
+ ss << tab << "<Trajectory Robot='" << robot->getType() << "' RobotNodeSet='" << rns->getName() << "' name='" << name << "' dim='" << dimension << "'>\n";
for (unsigned int i = 0; i < path.size(); i++)
{
@@ -344,4 +350,30 @@ void Trajectory::print() const
VR_INFO << s << endl;
}
+std::string Trajectory::getName() const
+{
+ return name;
+}
+
+VirtualRobot::RobotNodeSetPtr Trajectory::getRobotNodeSet()
+{
+ return rns;
+}
+
+VisualizationNodePtr Trajectory::getVisualization(std::string visualizationFactoryName)
+{
+ VisualizationFactoryPtr visualizationFactory;
+ if (visualizationFactoryName.empty())
+ visualizationFactory=VisualizationFactory::first(NULL);
+ else
+ visualizationFactory = VisualizationFactory::fromName(visualizationFactoryName, NULL);
+ if (!visualizationFactory)
+ {
+ VR_ERROR << "Could not create factory for visu type " << visualizationFactoryName << endl;
+ return VisualizationNodePtr();
+ }
+
+ return visualizationFactory->createTrajectory(shared_from_this());
+}
+
}
diff --git a/VirtualRobot/Trajectory.h b/VirtualRobot/Trajectory.h
index 3744753f7..7f8d9c545 100644
--- a/VirtualRobot/Trajectory.h
+++ b/VirtualRobot/Trajectory.h
@@ -35,7 +35,7 @@ namespace VirtualRobot {
/*!
A representation of a trajectory in joint space, associated with a RobotNodeSet.
*/
-class VIRTUAL_ROBOT_IMPORT_EXPORT Trajectory
+class VIRTUAL_ROBOT_IMPORT_EXPORT Trajectory : public boost::enable_shared_from_this<Trajectory>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -135,7 +135,7 @@ public:
//! For quick access to data.
const std::vector <Eigen::VectorXf >& getData() const;
-
+ VirtualRobot::RobotNodeSetPtr getRobotNodeSet();
/*!
Creates the corresponding trajectory in workspace.
@@ -150,6 +150,14 @@ public:
*/
virtual std::string getXMLString(int tabs = 0) const;
+ std::string getName() const;
+
+ /*!
+ Gte a visualization for this trajectory.
+ \param visualizationFactoryName The string that identifies the factory. If not given, the first registered factory (which is usually the only one) is used.
+ */
+ VisualizationNodePtr getVisualization(std::string visualizationFactoryName = "");
+
protected:
std::vector < Eigen::VectorXf > path; //!< vector with configurations which represent the path
RobotNodeSetPtr rns;
@@ -157,6 +165,7 @@ protected:
unsigned int dimension; //!< dimension of rns
};
+
} // namespace VirtualRobot
#endif /* _VirtualRobot_Trajectory_h_ */
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 94980fef2..707b4bf95 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -12,6 +12,7 @@
#include "CoinVisualization.h"
#include "../../Robot.h"
#include "../../Grasp.h"
+#include "../../Trajectory.h"
#include "../../GraspSet.h"
#include "../../SceneObject.h"
#include "../TriMeshModel.h"
@@ -804,6 +805,7 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInf
}
}
}*/
+ result->unrefNoDelete();
return result;
}
@@ -1140,6 +1142,14 @@ VirtualRobot::VisualizationNodePtr CoinVisualizationFactory::createArrow( const
return node;
}
+VirtualRobot::VisualizationNodePtr CoinVisualizationFactory::createTrajectory(TrajectoryPtr t, Color colorNode, Color colorLine, float nodeSize, float lineSize )
+{
+ SoNode *res = getCoinVisualization(t,colorNode,colorLine,nodeSize,lineSize);
+
+ VisualizationNodePtr node(new CoinVisualizationNode(res));
+ return node;
+}
+
SoNode* CoinVisualizationFactory::getCoinVisualization(WorkspaceRepresentationPtr reachSpace, int a, int b, int c, /*const Eigen::Vector3f &positionGlobal,*/ int nrBestEntries, SoSeparator* arrow, const VirtualRobot::ColorMap &cm, bool transformToGlobalPose, unsigned char minValue)
{
@@ -1446,6 +1456,7 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(WorkspaceRepresentationPt
}
}
}
+ res->unrefNoDelete();
return res;
}
@@ -1521,6 +1532,99 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(WorkspaceRepresentationPt
}
}
}
+ res->unrefNoDelete();
+ return res;
+}
+
+SoNode * CoinVisualizationFactory::getCoinVisualization( TrajectoryPtr t, Color colorNode, Color colorLine, float nodeSize, float lineSize )
+{
+ SoSeparator *res = new SoSeparator;
+ if (!t)
+ return res;
+ res->ref();
+ RobotNodeSetPtr rns = t->getRobotNodeSet();
+ Eigen::VectorXf c;
+ rns->getJointValues(c);
+ std::vector<Eigen::Matrix4f> ws = t->createWorkspaceTrajectory();
+
+ SoMaterial *materialNodeSolution = new SoMaterial();
+ SoMaterial *materialLineSolution = new SoMaterial();
+ materialNodeSolution->ambientColor.setValue(colorNode.r,colorNode.g,colorNode.b);
+ materialNodeSolution->diffuseColor.setValue(colorNode.r,colorNode.g,colorNode.b);
+ materialLineSolution->ambientColor.setValue(colorLine.r,colorLine.g,colorLine.b);
+ materialLineSolution->diffuseColor.setValue(colorLine.r,colorLine.g,colorLine.b);
+ SoSphere *sphereNodeSolution = new SoSphere();
+ sphereNodeSolution->radius.setValue(nodeSize);
+ SoDrawStyle *lineSolutionStyle = new SoDrawStyle();
+ lineSolutionStyle->lineWidth.setValue(lineSize);
+
+ Eigen::VectorXf actConfig;
+ Eigen::VectorXf parentConfig;
+ float x,y,z;
+ float x2 = 0.0f,y2 = 0.0f,z2 = 0.0f;
+
+ SoSeparator *sep = new SoSeparator();
+
+ SoComplexity *comple;
+ comple = new SoComplexity();
+ comple->value = 1.0f;
+ sep->addChild(comple);
+
+
+ for (size_t i = 0; i <ws.size(); i++)
+ {
+ // create 3D model for nodes
+ SoSeparator *s = new SoSeparator();
+ s->addChild(materialNodeSolution);
+ SoTranslation *t = new SoTranslation();
+
+ Eigen::Matrix4f m;
+ m = ws[i];
+ x = m(0,3);
+ y = m(1,3);
+ z = m(2,3);
+
+ t->translation.setValue(x,y,z);
+ s->addChild(t);
+ // display a solution node different
+ s->addChild(sphereNodeSolution);
+ sep->addChild(s);
+
+ if (i>0) // lines for all configurations
+ {
+ // create line to parent
+ SoSeparator *s2 = new SoSeparator();
+
+ SbVec3f points[2];
+ points[0].setValue(x2,y2,z2);
+ points[1].setValue(x,y,z);
+
+ s2->addChild(lineSolutionStyle);
+ s2->addChild(materialLineSolution);
+
+ SoCoordinate3* coordinate3 = new SoCoordinate3;
+ coordinate3->point.set1Value(0,points[0]);
+ coordinate3->point.set1Value(1,points[1]);
+ s2->addChild(coordinate3);
+
+ SoLineSet* lineSet = new SoLineSet;
+ lineSet->numVertices.setValue(2);
+ lineSet->startIndex.setValue(0);
+ s2->addChild(lineSet);
+
+ sep->addChild(s2);
+ }
+ x2 = x;
+ y2 = y;
+ z2 = z;
+ } // for
+
+
+ res->addChild(sep);
+
+ rns->setJointValues(c);
+
+ res->unrefNoDelete();
return res;
}
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 0a1e7c659..c4a138fa5 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -65,6 +65,7 @@ public:
virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, bool showNormals, Eigen::Matrix4f &pose);
virtual VisualizationNodePtr createPlane(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
virtual VisualizationNodePtr createArrow(const Eigen::Vector3f &n, float length = 50.0f, float width = 2.0f, const Color &color = Color::Gray());
+ virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f);
/*!
Create an empty VisualizationNode.
@@ -129,6 +130,8 @@ public:
static SoNode *getCoinVisualization(TriMeshModelPtr model, bool shownormals, VisualizationFactory::Color color = VisualizationFactory::Color::Gray());
+
+ static SoNode *getCoinVisualization(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f);
/*!
Create a visualization of the reachability data.
*/
diff --git a/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.cpp b/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.cpp
index a22fad2e4..336133209 100644
--- a/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.cpp
@@ -305,6 +305,12 @@ VirtualRobot::VisualizationNodePtr OSGVisualizationFactory::createPlane( const E
return VisualizationNodePtr();
}
+VirtualRobot::VisualizationNodePtr OSGVisualizationFactory::createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f)
+{
+ VR_INFO << "init nyi..." << endl;
+ return VisualizationNodePtr();
+}
+
void OSGVisualizationFactory::switchToWireframe( osg::Node *srcNode )
{
if( !srcNode )
diff --git a/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.h b/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.h
index 011e0f23a..2643105ff 100644
--- a/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.h
+++ b/VirtualRobot/Visualization/OSGVisualization/OSGVisualizationFactory.h
@@ -62,6 +62,7 @@ public:
virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, bool showNormals, Eigen::Matrix4f &pose);
virtual VisualizationNodePtr createPlane(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
virtual VisualizationNodePtr createArrow(const Eigen::Vector3f &n, float length = 50.0f, float width = 2.0f, const Color &color = Color::Gray());
+ virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f);
/*!
Create an empty VisualizationNode.
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index 2b18375e7..49e689c1c 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -70,6 +70,7 @@ public:
virtual VisualizationNodePtr createPlane(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f){return VisualizationNodePtr();}
virtual VisualizationNodePtr createPlane(const MathTools::Plane &plane, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f){return createPlane(plane.p, plane.n,extend,transparency,colorR,colorG,colorB);}
virtual VisualizationNodePtr createArrow(const Eigen::Vector3f &n, float length = 50.0f, float width = 2.0f, const Color &color = Color::Gray()){return VisualizationNodePtr();}
+ virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f){return VisualizationNodePtr();}
/*!
Create an empty VisualizationNode.
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 89443701c..4068e4c87 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -24,6 +24,7 @@ VisualizationNode::VisualizationNode()
VisualizationNode::~VisualizationNode()
{
+ attachedVisualizations.clear();
}
VirtualRobot::VisualizationNodePtr VisualizationNode::clone(bool deepCopy)
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 7a5e4a220..adfbbe2d6 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -1,7 +1,9 @@
#include "BaseIO.h"
+#include "../Robot.h"
#include "../RobotFactory.h"
#include "../RobotNodeSet.h"
+#include "../Trajectory.h"
#include "../RuntimeEnvironment.h"
#include "../VirtualRobotException.h"
#include "../EndEffector/EndEffector.h"
@@ -61,6 +63,21 @@ float BaseIO::convertToFloat(const char* s)
return result;
}
+int BaseIO::convertToInt(const char* s)
+{
+ THROW_VR_EXCEPTION_IF(NULL == s, "Passing Null string to convertToInt()");
+ std::stringstream intStream;
+ intStream << std::string(s);
+ int result;
+
+ if (!(intStream >> result))
+ {
+ THROW_VR_EXCEPTION("The string can not be parsed into an int value");
+ }
+
+ return result;
+}
+
/**
* This method gets the attribute \p attributeName from xml_node \p xmlNode and
@@ -972,5 +989,133 @@ RobotNodeSetPtr BaseIO::processRobotNodeSet(rapidxml::xml_node<char>* setXMLNode
return rns;
}
+bool BaseIO::processFloatValueTags(rapidxml::xml_node<char> *XMLNode, int dim, Eigen::VectorXf &stroreResult)
+{
+ if (!XMLNode || dim<=0)
+ return false;
+ stroreResult.resize(dim);
+ int entry = 0;
+ rapidxml::xml_node<>* node = XMLNode->first_node();
+ while (node)
+ {
+ std::string nodeName = getLowerCase(node->name());
+ if (nodeName == "c")
+ {
+ THROW_VR_EXCEPTION_IF(entry>=dim, "Too many entries in trajectory's point definition..." << endl);
+ stroreResult(entry) = getFloatByAttributeName(node,"value");
+ } else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in trajectory point definition" << endl);
+ }
+ entry++;
+ node = node->next_sibling();
+ }
+ THROW_VR_EXCEPTION_IF(entry<dim, "Not enough entries in trajectory's point definition..." << endl);
+ return true;
+}
+
+TrajectoryPtr BaseIO::processTrajectory(rapidxml::xml_node<char> *trajectoryXMLNode, std::vector<RobotPtr> &robots)
+{
+ THROW_VR_EXCEPTION_IF(!trajectoryXMLNode, "NULL data for trajectoryXMLNode");
+
+ std::string robotName;
+ std::string nodeSetName;
+ std::string trajName;
+ int dim = -1;
+
+ // get name and root
+ rapidxml::xml_attribute<> *attr = trajectoryXMLNode->first_attribute();
+ while (attr)
+ {
+ std::string name = getLowerCase(attr->name());
+ if (name=="name")
+ {
+ THROW_VR_EXCEPTION_IF(!trajName.empty(), "Trajectory contains multiple definitions of attribute name. First value is: " << trajName);
+ trajName = attr->value();
+ } else if (name=="robot")
+ {
+ THROW_VR_EXCEPTION_IF(!robotName.empty(), "Trajectory contains multiple definitions of attribute Robot. First value is: " << robotName);
+ robotName = attr->value();
+ } else if (name=="robotnodeset")
+ {
+ THROW_VR_EXCEPTION_IF(!nodeSetName.empty(), "Trajectory contains multiple definitions of attribute RobotNodeSet. First value is: " << nodeSetName);
+ nodeSetName = attr->value();
+ } else if (name=="dim" || name=="dimension")
+ {
+ THROW_VR_EXCEPTION_IF(dim!=-1, "Trajectory contains multiple definitions of attribute dim. First value of dim: " << dim);
+ dim = convertToInt(attr->value());
+ }
+ attr = attr->next_attribute();
+ }
+
+ THROW_VR_EXCEPTION_IF(robotName.empty(),"Invalid or missing Robot attribute");
+ THROW_VR_EXCEPTION_IF(nodeSetName.empty(),"Invalid or missing RobotNodeSet attribute");
+
+ if (trajName.empty())
+ {
+ trajName = "Trajectory";
+ VR_WARNING << "Trajectory definition expects attribute 'RobotNodeSet'. Setting to " << trajName << endl;
+ }
+ RobotPtr r;
+ for (size_t i=0;i<robots.size();i++)
+ {
+ if (robots[i]->getType() == robotName)
+ {
+ r = robots[i];
+ break;
+ }
+ }
+ THROW_VR_EXCEPTION_IF(!r,"Could not find robot with name " << robotName);
+ RobotNodeSetPtr rns = r->getRobotNodeSet(nodeSetName);
+ THROW_VR_EXCEPTION_IF(!rns,"Could not find RNS with name " << nodeSetName << " in robot " << robotName);
+ if (dim != rns->getSize())
+ {
+ VR_WARNING << " Invalid dim attribute (" << dim << "). Setting dimension to " << rns->getSize() << endl;
+ dim = rns->getSize();
+ }
+
+ TrajectoryPtr res(new Trajectory(rns,trajName));
+ rapidxml::xml_node<>* node = trajectoryXMLNode->first_node();
+ while (node)
+ {
+ std::string nodeName = getLowerCase(node->name());
+ if (nodeName == "point")
+ {
+ Eigen::VectorXf p;
+
+ if (!processFloatValueTags(node,dim,p))
+ {
+ VR_ERROR << "Error in processing configuration. Skipping entry" << endl;
+ } else
+ {
+ res->addPoint(p);
+ }
+
+ } else
+ {
+ THROW_VR_EXCEPTION ("XML definition <" << nodeName << "> not supported in trajectory definition with name '" << trajName << "'." << endl);
+ }
+
+ node = node->next_sibling();
+ }
+ return res;
+}
+
+bool BaseIO::writeXMLFile(const std::string &filename, const std::string &content, bool overwrite)
+{
+ if (!overwrite && boost::filesystem::exists(filename))
+ return false;
+
+ // save file
+ std::ofstream out(filename.c_str(),std::ios::out|std::ios::trunc);
+
+ if (!out.is_open())
+ return false;
+
+ out << content;
+ out.close();
+ return true;
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 1bb544645..74e079a90 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -54,6 +54,14 @@ public:
static void makeAbsolutePath( const std::string &basePath, std::string &filename );
static void makeRelativePath( const std::string &basePath, std::string &filename );
+ /*!
+ Create a file and store XML content.
+ \param filename The filename
+ \param content The XML content as string. No checks are performed.
+ \param overwrite If true, a potentionally existing file is silently overwritten.
+ \return True on success
+ */
+ static bool writeXMLFile(const std::string &filename, const std::string &content, bool overwrite = true);
protected:
// instantiation not allowed
@@ -62,6 +70,7 @@ protected:
static bool isTrue(const char* s);
static float convertToFloat(const char* s);
+ static int convertToInt(const char* s);
static void processNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<RobotNodePtr>& nodeList, bool clearList = true);
static void processLimitsNode(rapidxml::xml_node<char> *limitsXMLNode, float &jointLimitLo, float &jointLimitHi);
static std::string processFileNode(rapidxml::xml_node<char> *fileNode, const std::string &basePath);
@@ -82,7 +91,9 @@ protected:
static CollisionModelPtr processCollisionTag(rapidxml::xml_node<char> *colXMLNode, const std::string &tagName, const std::string &basePath);
static void processPhysicsTag(rapidxml::xml_node<char> *physicsXMLNode, const std::string &nodeName, SceneObject::Physics &physics);
static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char>* setXMLNode, RobotPtr robo, const std::string& robotRootNode, int& robotNodeSetCounter );
+ static TrajectoryPtr processTrajectory(rapidxml::xml_node<char> *trajectoryXMLNode,std::vector<RobotPtr> &robots);
static Eigen::Matrix3f process3x3Matrix(rapidxml::xml_node<char> *matrixXMLNode);
+ static bool processFloatValueTags(rapidxml::xml_node<char> *XMLNode, int dim, Eigen::VectorXf &stroreResult);
static bool hasUnitsAttribute(rapidxml::xml_node<char> *node);
static std::vector< Units > getUnitsAttributes(rapidxml::xml_node<char> *node);
static void getAllAttributes(rapidxml::xml_node<char> *node, const std::string &attrString, std::vector<std::string> &storeValues);
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index a235ed6f6..7f911cc83 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -334,15 +334,7 @@ bool ObjectIO::saveManipulationObject( ManipulationObjectPtr object, const std::
std::string xmlString = object->getXMLString(basePath);
// save file
- std::ofstream out(xmlFile.c_str());
-
- if (!out.is_open())
- return false;
-
- out << xmlString;
- out.close();
-
- return true;
+ return BaseIO::writeXMLFile(xmlFile,xmlString,true);
}
diff --git a/VirtualRobot/XML/SceneIO.cpp b/VirtualRobot/XML/SceneIO.cpp
index 319c1be27..e6a32aee5 100644
--- a/VirtualRobot/XML/SceneIO.cpp
+++ b/VirtualRobot/XML/SceneIO.cpp
@@ -6,6 +6,7 @@
#include <boost/filesystem.hpp>
#include "RobotIO.h"
#include "ObjectIO.h"
+#include "../Trajectory.h"
#include "../SceneObjectSet.h"
namespace VirtualRobot {
@@ -203,6 +204,18 @@ bool SceneIO::processSceneObstacle(rapidxml::xml_node<char>* sceneXMLNode, Scene
return true;
}
+bool SceneIO::processSceneTrajectory(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene )
+{
+ THROW_VR_EXCEPTION_IF(!sceneXMLNode || !scene, "NULL data in processSceneTrajectory");
+
+ TrajectoryPtr o = BaseIO::processTrajectory(sceneXMLNode, scene->getRobots());
+ if (!o)
+ return false;
+
+ scene->registerTrajectory(o);
+ return true;
+}
+
ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std::string& basePath)
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "No <Scene> tag in XML definition");
@@ -213,6 +226,7 @@ ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std
// process xml nodes
std::vector<rapidxml::xml_node<char>* > sceneSetNodes;
+ std::vector<rapidxml::xml_node<char>* > trajectoryNodes;
rapidxml::xml_node<> *XMLNode = sceneXMLNode->first_node(NULL,0,false);
@@ -245,6 +259,9 @@ ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create ManipulationObject " << failedNodeName << " in scene " << scene->getName() << endl);
}
+ } else if (nodeName=="trajectory")
+ {
+ trajectoryNodes.push_back(XMLNode);
} else if (nodeName=="sceneobjectset")
{
sceneSetNodes.push_back(XMLNode);
@@ -267,6 +284,18 @@ ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std
}
}
+
+ // process all trajectories
+ for (size_t i=0;i<trajectoryNodes.size();i++)
+ {
+ bool r = processSceneTrajectory(trajectoryNodes[i], scene);
+ if (!r)
+ {
+ std::string failedNodeName = processNameAttribute(XMLNode);
+ THROW_VR_EXCEPTION("Failed to create trajectory " << failedNodeName << " in scene " << scene->getName() << endl);
+ }
+ }
+
return scene;
}
@@ -335,15 +364,7 @@ bool SceneIO::saveScene( ScenePtr s, const std::string &xmlFile )
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;
+ return BaseIO::writeXMLFile(xmlFile,xmlString,true);
}
diff --git a/VirtualRobot/XML/SceneIO.h b/VirtualRobot/XML/SceneIO.h
index 029bb4fb6..5c66d17b9 100644
--- a/VirtualRobot/XML/SceneIO.h
+++ b/VirtualRobot/XML/SceneIO.h
@@ -71,6 +71,7 @@ protected:
static ScenePtr processSceneAttributes(rapidxml::xml_node<char>* sceneXMLNode);
static bool processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath );
static bool processSceneObstacle(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath );
+ static bool processSceneTrajectory(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene );
static bool processSceneManipulationObject(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath );
static bool processSceneObjectSet(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene );
};
--
GitLab