Skip to content
Snippets Groups Projects
Commit 17cb0764 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Auto format (pure)

parent 638ba465
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
VirtualRobot/XML/RobotIO.cpp
+ 427
− 242
View file @ 17cb0764
#include "RobotIO.h"
#include "../RobotFactory.h"
#include "../RobotNodeSet.h"
#include "../VirtualRobotException.h"
#include <fstream>
#include <iostream>
#include <vector>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/filesystem/remove_trailing_separator.h>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
#include <VirtualRobot/Nodes/FourBar/Joint.h>
#include "../CollisionDetection/CollisionModel.h"
#include "../EndEffector/EndEffector.h"
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNodeFactory.h"
#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodeFixedFactory.h"
#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodePrismatic.h"
#include "../RobotConfig.h"
#include "../RobotFactory.h"
#include "../RobotNodeSet.h"
#include "../RuntimeEnvironment.h"
#include "../Transformation/DHParameter.h"
#include "../VirtualRobotException.h"
#include "../Visualization/TriMeshModel.h"
#include "../Visualization/VisualizationFactory.h"
#include "../Visualization/VisualizationNode.h"
#include "../Visualization/TriMeshModel.h"
#include "../RobotConfig.h"
#include "../RuntimeEnvironment.h"
#include "Nodes/RobotNodeFourBar.h"
#include "VirtualRobot.h"
#include "rapidxml.hpp"
#include "mujoco/RobotMjcf.h"
#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
#include <VirtualRobot/Nodes/FourBar/Joint.h>
#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include <SimoxUtility/filesystem/remove_trailing_separator.h>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <vector>
#include <fstream>
#include <iostream>
#include "rapidxml.hpp"
namespace VirtualRobot
{
......@@ -37,15 +39,13 @@ namespace VirtualRobot
std::map<std::string, int> RobotIO::robot_name_counter;
RobotIO::RobotIO()
= default;
RobotIO::~RobotIO()
= default;
RobotIO::RobotIO() = default;
RobotIO::~RobotIO() = default;
void RobotIO::processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames)
void
RobotIO::processChildNode(rapidxml::xml_node<char>* childXMLNode,
std::vector<std::string>& childrenNames)
{
THROW_VR_EXCEPTION_IF(!childXMLNode, "NULL data for childXMLNode in processChildNode()")
......@@ -57,11 +57,15 @@ namespace VirtualRobot
childrenNames.push_back(s);
}
void RobotIO::processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot)
void
RobotIO::processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
const std::string& nodeName,
std::vector<ChildFromRobotDef>& childrenFromRobot)
{
rapidxml::xml_attribute<>* attr;
THROW_VR_EXCEPTION_IF(!childXMLNode, "NULL data for childXMLNode in processChildFromRobotNode()")
THROW_VR_EXCEPTION_IF(!childXMLNode,
"NULL data for childXMLNode in processChildFromRobotNode()")
ChildFromRobotDef d;
......@@ -87,10 +91,11 @@ namespace VirtualRobot
fileXMLNode = fileXMLNode->next_sibling("file", 0, false);
}
THROW_VR_EXCEPTION_IF(((!counter) == 0), "Missing file for <ChildFromRobot> tag (in node '" << nodeName << "')." << endl);
THROW_VR_EXCEPTION_IF(((!counter) == 0),
"Missing file for <ChildFromRobot> tag (in node '" << nodeName
<< "')." << endl);
}
/**
* This method processes Limits tags.
* The values for the attributes "lo" and "hi" are extracted based on the
......@@ -98,7 +103,11 @@ namespace VirtualRobot
*
* The values are stored in \p jointLimitLo and \p jointLimitHi
*/
void RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless)
void
RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
float& jointLimitLo,
float& jointLimitHi,
bool& limitless)
{
THROW_VR_EXCEPTION_IF(!limitsXMLNode, "NULL data for limitsXMLNode in processLimitsNode()");
......@@ -112,13 +121,15 @@ namespace VirtualRobot
{
if (unit.isLength())
{
VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -1000 [mm]." << std::endl;
VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -1000 [mm]."
<< std::endl;
jointLimitLo = -1000.0f;
unit = Units("mm");
}
else
{
VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]." << std::endl;
VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]."
<< std::endl;
jointLimitLo = float(-M_PI);
unit = Units("rad");
}
......@@ -164,36 +175,39 @@ namespace VirtualRobot
if (llAttr != nullptr)
{
limitless = isTrue(llAttr->value());
if (limitless && unit.isAngle() && (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
if (limitless && unit.isAngle() &&
(unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
{
VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes should equal 2*pi [rad] or 360 [deg]." << endl
VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes "
"should equal 2*pi [rad] or 360 [deg]."
<< endl
<< "Setting 'lo' to -pi and 'hi' to pi [rad]..." << std::endl;
jointLimitLo = float(-M_PI);
jointLimitHi = float(M_PI);
}
}
}
RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
RobotPtr robot,
VisualizationNodePtr visualizationNode,
CollisionModelPtr collisionModel,
SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype,
Eigen::Matrix4f& transformationMatrix
)
RobotNodePtr
RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode,
const std::string& robotNodeName,
RobotPtr robot,
VisualizationNodePtr visualizationNode,
CollisionModelPtr collisionModel,
SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype,
Eigen::Matrix4f& transformationMatrix)
{
float jointLimitLow = float(- M_PI);
float jointLimitLow = float(-M_PI);
float jointLimitHigh = float(M_PI);
bool limitless = false;
Eigen::Matrix4f preJointTransform = transformationMatrix;//Eigen::Matrix4f::Identity();
Eigen::Matrix4f preJointTransform = transformationMatrix; //Eigen::Matrix4f::Identity();
Eigen::Vector3f axis = Eigen::Vector3f::Zero();
Eigen::Vector3f translationDir = Eigen::Vector3f::Zero();
std::vector< std::string > propagateJVName;
std::vector< float > propagateJVFactor;
std::vector<std::string> propagateJVName;
std::vector<float> propagateJVFactor;
rapidxml::xml_attribute<>* attr;
float jointOffset = 0.0f;
......@@ -203,14 +217,22 @@ namespace VirtualRobot
{
// no <Joint> tag -> fixed joint
RobotNodePtr robotNode;
RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr);
RobotNodeFactoryPtr fixedNodeFactory =
RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr);
if (fixedNodeFactory)
{
robotNode = fixedNodeFactory->createRobotNode(
robot, robotNodeName, visualizationNode, collisionModel,
jointLimitLow, jointLimitHigh, jointOffset,
preJointTransform, axis, translationDir,
physics, rntype);
robotNode = fixedNodeFactory->createRobotNode(robot,
robotNodeName,
visualizationNode,
collisionModel,
jointLimitLow,
jointLimitHigh,
jointOffset,
preJointTransform,
axis,
translationDir,
physics,
rntype);
}
return robotNode;
}
......@@ -224,7 +246,8 @@ namespace VirtualRobot
else
{
VR_WARNING << "No 'type' attribute for <Joint> tag. "
<< "Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl;
<< "Assuming fixed joint for RobotNode " << robotNodeName << "!"
<< std::endl;
jointType = RobotNodeFixedFactory::getName();
}
......@@ -261,37 +284,50 @@ namespace VirtualRobot
if (nodeName == "dh")
{
THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! "
"Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure.")
"Use "
"<RobotNode><Transform><DH>...</DH></Transform><Joint>...</"
"Joint></RobotNode> structure.")
//THROW_VR_EXCEPTION_IF(dhXMLNode, "Multiple DH definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//dhXMLNode = node;
}
else if (nodeName == "limits")
{
THROW_VR_EXCEPTION_IF(limitsNode, "Multiple limits definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
THROW_VR_EXCEPTION_IF(limitsNode,
"Multiple limits definitions in <Joint> tag of robot node <"
<< robotNodeName << ">." << endl);
limitsNode = node;
processLimitsNode(limitsNode, jointLimitLow, jointLimitHigh, limitless);
}
else if (nodeName == "prejointtransform")
{
THROW_VR_EXCEPTION("PreJointTransform is DEPRECATED! "
"Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
THROW_VR_EXCEPTION(
"PreJointTransform is DEPRECATED! "
"Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> "
"structure")
//THROW_VR_EXCEPTION_IF(prejointTransformNode, "Multiple preJoint definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//prejointTransformNode = node;
}
else if (nodeName == "axis")
{
THROW_VR_EXCEPTION_IF(tmpXMLNodeAxis, "Multiple axis definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
THROW_VR_EXCEPTION_IF(tmpXMLNodeAxis,
"Multiple axis definitions in <Joint> tag of robot node <"
<< robotNodeName << ">." << endl);
tmpXMLNodeAxis = node;
}
else if (nodeName == "translationdirection")
{
THROW_VR_EXCEPTION_IF(tmpXMLNodeTranslation, "Multiple translation definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
THROW_VR_EXCEPTION_IF(
tmpXMLNodeTranslation,
"Multiple translation definitions in <Joint> tag of robot node <"
<< robotNodeName << ">." << endl);
tmpXMLNodeTranslation = node;
}
else if (nodeName == "postjointtransform")
{
THROW_VR_EXCEPTION("postjointtransform is DEPRECATED and not longer allowed! "
"Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
THROW_VR_EXCEPTION(
"postjointtransform is DEPRECATED and not longer allowed! "
"Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> "
"structure")
//THROW_VR_EXCEPTION_IF(postjointTransformNode, "Multiple postjointtransform definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//postjointTransformNode = node;
}
......@@ -300,7 +336,7 @@ namespace VirtualRobot
maxVelocity = getFloatByAttributeName(node, "value");
// convert to m/s
std::vector< Units > unitsAttr = getUnitsAttributes(node);
std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uTime("sec");
Units uLength("m");
Units uAngle("rad");
......@@ -346,14 +382,13 @@ namespace VirtualRobot
}
maxVelocity *= factor;
}
else if (nodeName == "maxacceleration")
{
maxAcceleration = getFloatByAttributeName(node, "value");
// convert to m/s^2
std::vector< Units > unitsAttr = getUnitsAttributes(node);
std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uTime("sec");
Units uLength("m");
Units uAngle("rad");
......@@ -399,13 +434,12 @@ namespace VirtualRobot
}
maxAcceleration *= factor;
}
else if (nodeName == "maxtorque")
{
maxTorque = getFloatByAttributeName(node, "value");
// convert to Nm
std::vector< Units > unitsAttr = getUnitsAttributes(node);
std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uLength("m");
for (auto& i : unitsAttr)
......@@ -429,7 +463,9 @@ namespace VirtualRobot
{
rapidxml::xml_attribute<>* attrPropa;
attrPropa = node->first_attribute("name", 0, false);
THROW_VR_EXCEPTION_IF(!attrPropa, "Expecting 'name' attribute in <PropagateJointValue> tag..." << endl);
THROW_VR_EXCEPTION_IF(!attrPropa,
"Expecting 'name' attribute in <PropagateJointValue> tag..."
<< endl);
std::string s(attrPropa->value());
propagateJVName.push_back(s);
......@@ -467,12 +503,13 @@ namespace VirtualRobot
switch (hemisphere->role)
{
case RobotNodeHemisphere::Role::FIRST:
hemisphere->lever = getFloatByAttributeName(node, "lever");
hemisphere->theta0Rad = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
break;
case RobotNodeHemisphere::Role::SECOND:
break;
case RobotNodeHemisphere::Role::FIRST:
hemisphere->lever = getFloatByAttributeName(node, "lever");
hemisphere->theta0Rad =
simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
break;
case RobotNodeHemisphere::Role::SECOND:
break;
}
}
else if (nodeName == "four_bar")
......@@ -490,28 +527,28 @@ namespace VirtualRobot
THROW_VR_EXCEPTION("Invalid role in four_bar joint: " << e.what())
}
const rapidxml::xml_node<>* dimensionsNode = node->first_node("dimensions", 0, false);
const rapidxml::xml_node<>* dimensionsNode =
node->first_node("dimensions", 0, false);
if(fourBarXmlInfo->role == RobotNodeFourBar::Role::ACTIVE)
if (fourBarXmlInfo->role == RobotNodeFourBar::Role::ACTIVE)
{
if(dimensionsNode == nullptr)
if (dimensionsNode == nullptr)
{
THROW_VR_EXCEPTION("Missing <dimensions> node for four_bar joint.");
}
fourBarXmlInfo->dimensions = four_bar::Joint::Dimensions
{
fourBarXmlInfo->dimensions = four_bar::Joint::Dimensions{
.shank = getFloatByAttributeName(dimensionsNode, "shank"),
.p1 = getFloatByAttributeName(dimensionsNode, "p1"),
.p2 = getFloatByAttributeName(dimensionsNode, "p2"),
.p3 = getFloatByAttributeName(dimensionsNode, "p3")
};
.p3 = getFloatByAttributeName(dimensionsNode, "p3")};
}
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Joint> tag of RobotNode <" << robotNodeName << ">." << endl);
THROW_VR_EXCEPTION("XML definition <"
<< nodeName << "> not supported in <Joint> tag of RobotNode <"
<< robotNodeName << ">." << endl);
}
node = node->next_sibling();
......@@ -562,7 +599,10 @@ namespace VirtualRobot
}
else
{
THROW_VR_EXCEPTION("Prismatic joint '" << robotNodeName << "' wrongly defined, expecting 'TranslationDirection' tag." << endl);
THROW_VR_EXCEPTION("Prismatic joint '"
<< robotNodeName
<< "' wrongly defined, expecting 'TranslationDirection' tag."
<< endl);
}
if (scaleVisu)
......@@ -586,12 +626,18 @@ namespace VirtualRobot
} else
{*/
// create nodes that are not defined via DH parameters
robotNode = robotNodeFactory->createRobotNode(
robot, robotNodeName, visualizationNode, collisionModel,
jointLimitLow, jointLimitHigh, jointOffset,
preJointTransform, axis, translationDir,
physics, rntype
);
robotNode = robotNodeFactory->createRobotNode(robot,
robotNodeName,
visualizationNode,
collisionModel,
jointLimitLow,
jointLimitHigh,
jointOffset,
preJointTransform,
axis,
translationDir,
physics,
rntype);
//}
}
else
......@@ -623,7 +669,7 @@ namespace VirtualRobot
node->setXmlInfo(hemisphere.value());
}
if(robotNode->isFourBarJoint() and fourBarXmlInfo.has_value())
if (robotNode->isFourBarJoint() and fourBarXmlInfo.has_value())
{
auto node = std::dynamic_pointer_cast<RobotNodeFourBar>(robotNode);
node->setXmlInfo(fourBarXmlInfo.value());
......@@ -648,16 +694,15 @@ namespace VirtualRobot
return robotNode;
}
RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo,
const std::string& basePath,
int& robotNodeCounter,
std::vector< std::string >& childrenNames,
std::vector< ChildFromRobotDef >& childrenFromRobot,
RobotDescription loadMode,
RobotNode::RobotNodeType rntype)
RobotNodePtr
RobotIO::processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo,
const std::string& basePath,
int& robotNodeCounter,
std::vector<std::string>& childrenNames,
std::vector<ChildFromRobotDef>& childrenFromRobot,
RobotDescription loadMode,
RobotNode::RobotNodeType rntype)
{
childrenFromRobot.clear();
THROW_VR_EXCEPTION_IF(!robotNodeXMLNode, "NULL data in processRobotNode");
......@@ -671,7 +716,8 @@ namespace VirtualRobot
ss << robo->getType() << "_Node_" << robotNodeCounter;
robotNodeName = ss.str();
robotNodeCounter++;
VR_WARNING << "RobotNode definition expects attribute 'name'. Setting name to " << robotNodeName << std::endl;
VR_WARNING << "RobotNode definition expects attribute 'name'. Setting name to "
<< robotNodeName << std::endl;
}
......@@ -696,7 +742,7 @@ namespace VirtualRobot
rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
rapidxml::xml_node<>* jointNodeXML = nullptr;
std::vector< rapidxml::xml_node<>* > sensorTags;
std::vector<rapidxml::xml_node<>*> sensorTags;
std::vector<GraspSetPtr> graspSets;
while (node)
......@@ -707,34 +753,44 @@ namespace VirtualRobot
{
if (loadMode == eFull || loadMode == eFullVisAsCol)
{
THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in RobotNode '" << robotNodeName << "'." << endl);
visualizationNode = processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
THROW_VR_EXCEPTION_IF(visuProcessed,
"Two visualization tags defined in RobotNode '"
<< robotNodeName << "'." << endl);
visualizationNode =
processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel && visualizationNode)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(colProcessed,
"Two collision tags defined in RobotNode '"
<< robotNodeName << "'." << endl);
std::string colModelName = robotNodeName;
colModelName += "_VISU_ColModel";
// clone model
VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
// todo: ID?
collisionModel.reset(new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
collisionModel.reset(new CollisionModel(
visualizationNodeClone, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
else if (loadMode == eCollisionModel)
{
VisualizationNodePtr visualizationNodeCM = checkUseAsColModel(node, robotNodeName, basePath);
VisualizationNodePtr visualizationNodeCM =
checkUseAsColModel(node, robotNodeName, basePath);
if (visualizationNodeCM)
{
useAsColModel = true;
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(colProcessed,
"Two collision tags defined in RobotNode '"
<< robotNodeName << "'." << endl);
std::string colModelName = robotNodeName;
colModelName += "_VISU_ColModel";
// todo: ID?
collisionModel.reset(new CollisionModel(visualizationNodeCM, colModelName, CollisionCheckerPtr()));
collisionModel.reset(new CollisionModel(
visualizationNodeCM, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
......@@ -750,7 +806,9 @@ namespace VirtualRobot
{
if (loadMode == eFull || loadMode == eCollisionModel || loadMode == eFullVisAsCol)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(colProcessed,
"Two collision tags defined in RobotNode '"
<< robotNodeName << "'." << endl);
collisionModel = processCollisionTag(node, robotNodeName, basePath);
colProcessed = true;
}
......@@ -775,12 +833,16 @@ namespace VirtualRobot
}
else if (nodeName == "joint")
{
THROW_VR_EXCEPTION_IF(jointNodeXML, "Two joint tags defined in RobotNode '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(jointNodeXML,
"Two joint tags defined in RobotNode '" << robotNodeName
<< "'." << endl);
jointNodeXML = node;
}
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in RobotNode '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(physicsDefined,
"Two physics tags defined in RobotNode '" << robotNodeName
<< "'." << endl);
processPhysicsTag(node, robotNodeName, physics);
physicsDefined = true;
}
......@@ -795,13 +857,15 @@ namespace VirtualRobot
else if (nodeName == "graspset")
{
GraspSetPtr gs = processGraspSet(node, robotNodeName);
THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << robotNodeName << "'." << endl);
THROW_VR_EXCEPTION_IF(!gs,
"Invalid grasp set in '" << robotNodeName << "'." << endl);
graspSets.push_back(gs);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in RobotNode <" << robotNodeName << ">." << endl);
THROW_VR_EXCEPTION("XML definition <"
<< nodeName << "> not supported in RobotNode <" << robotNodeName
<< ">." << endl);
}
node = node->next_sibling();
......@@ -815,11 +879,19 @@ namespace VirtualRobot
// clone model
VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
// todo: ID?
collisionModel.reset(new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
collisionModel.reset(
new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
}
//create joint from xml data
robotNode = processJointNode(jointNodeXML, robotNodeName, robo, visualizationNode, collisionModel, physics, rntype, transformMatrix);
robotNode = processJointNode(jointNodeXML,
robotNodeName,
robo,
visualizationNode,
collisionModel,
physics,
rntype,
transformMatrix);
robotNode->basePath = basePath;
robotNode->visualizationModelXML = visualizationModelXML;
robotNode->collisionModelXML = collisionModelXML;
......@@ -840,8 +912,10 @@ namespace VirtualRobot
return robotNode;
}
VisualizationNodePtr RobotIO::checkUseAsColModel(rapidxml::xml_node<>* visuXMLNode, const std::string& /*robotNodeName*/, const std::string& basePath)
VisualizationNodePtr
RobotIO::checkUseAsColModel(rapidxml::xml_node<>* visuXMLNode,
const std::string& /*robotNodeName*/,
const std::string& basePath)
{
bool enableVisu = true;
//bool coordAxis = false;
......@@ -872,17 +946,20 @@ namespace VirtualRobot
if (visuFileXMLNode)
{
attr = visuFileXMLNode->first_attribute("type", 0, false);
THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <Visualization> tag." << endl);
THROW_VR_EXCEPTION_IF(!attr,
"Missing 'type' attribute in <Visualization> tag." << endl);
visuFileType = attr->value();
getLowerCase(visuFileType);
visuFile = processFileNode(visuFileXMLNode, basePath);
}
rapidxml::xml_node<>* useColModel = visuXMLNode->first_node("useascollisionmodel", 0, false);
rapidxml::xml_node<>* useColModel =
visuXMLNode->first_node("useascollisionmodel", 0, false);
if (useColModel && visuFile != "")
{
VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFileType, nullptr);
VisualizationFactoryPtr visualizationFactory =
VisualizationFactory::fromName(visuFileType, nullptr);
if (visualizationFactory)
{
......@@ -890,7 +967,8 @@ namespace VirtualRobot
}
else
{
VR_WARNING << "VisualizationFactory of type '" << visuFileType << "' not present. Ignoring Visualization data." << std::endl;
VR_WARNING << "VisualizationFactory of type '" << visuFileType
<< "' not present. Ignoring Visualization data." << std::endl;
}
}
}
......@@ -898,9 +976,13 @@ namespace VirtualRobot
return visualizationNode;
}
RobotPtr RobotIO::processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode)
RobotPtr
RobotIO::processRobot(rapidxml::xml_node<char>* robotXMLNode,
const std::string& basePath,
RobotDescription loadMode)
{
THROW_VR_EXCEPTION_IF(!robotXMLNode, "No <Robot> tag in XML definition! base path = " << basePath);
THROW_VR_EXCEPTION_IF(!robotXMLNode,
"No <Robot> tag in XML definition! base path = " << basePath);
// process Attributes
std::string robotRoot;
......@@ -908,23 +990,34 @@ namespace VirtualRobot
robo = processRobotAttributes(robotXMLNode, robotRoot);
// process xml nodes
std::map< RobotNodePtr, std::vector< ChildFromRobotDef > > childrenFromRobotFilesMap;
std::vector<rapidxml::xml_node<char>* > robotNodeSetNodes;
std::vector<rapidxml::xml_node<char>* > endeffectorNodes;
std::map<RobotNodePtr, std::vector<ChildFromRobotDef>> childrenFromRobotFilesMap;
std::vector<rapidxml::xml_node<char>*> robotNodeSetNodes;
std::vector<rapidxml::xml_node<char>*> endeffectorNodes;
NodeMapping nodeMapping;
std::optional<HumanMapping> humanMapping;
std::map<std::string, std::vector<std::string>> attachments;
processRobotChildNodes(robotXMLNode, robo, robotRoot, basePath, childrenFromRobotFilesMap, robotNodeSetNodes, endeffectorNodes, nodeMapping, humanMapping, attachments, loadMode);
processRobotChildNodes(robotXMLNode,
robo,
robotRoot,
basePath,
childrenFromRobotFilesMap,
robotNodeSetNodes,
endeffectorNodes,
nodeMapping,
humanMapping,
attachments,
loadMode);
// process childfromrobot tags
std::map< RobotNodePtr, std::vector< ChildFromRobotDef > >::iterator iter = childrenFromRobotFilesMap.begin();
std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>::iterator iter =
childrenFromRobotFilesMap.begin();
while (iter != childrenFromRobotFilesMap.end())
{
std::vector< ChildFromRobotDef > childrenFromRobot = iter->second;
std::vector<ChildFromRobotDef> childrenFromRobot = iter->second;
RobotNodePtr node = iter->first;
for (auto& i : childrenFromRobot)
......@@ -936,21 +1029,29 @@ namespace VirtualRobot
try
{
THROW_VR_EXCEPTION_IF(!std::filesystem::exists(filenameNewComplete), "File <" << filenameNewComplete.string() << "> does not exist." << endl);
THROW_VR_EXCEPTION_IF(!std::filesystem::exists(filenameNewComplete),
"File <" << filenameNewComplete.string()
<< "> does not exist." << endl);
}
catch (...)
{
THROW_VR_EXCEPTION("Error while processing file <" << filenameNewComplete.string() << ">." << endl);
THROW_VR_EXCEPTION("Error while processing file <"
<< filenameNewComplete.string() << ">." << endl);
}
RobotPtr r = loadRobot(filenameNewComplete.string(), loadMode);
THROW_VR_EXCEPTION_IF(!r, "Could not add child-from-robot due to failed loading of robot from file" << i.filename);
THROW_VR_EXCEPTION_IF(
!r,
"Could not add child-from-robot due to failed loading of robot from file"
<< i.filename);
RobotNodePtr root = r->getRootNode();
THROW_VR_EXCEPTION_IF(!root, "Could not add child-from-robot. No root node in file" << i.filename);
THROW_VR_EXCEPTION_IF(
!root, "Could not add child-from-robot. No root node in file" << i.filename);
RobotNodePtr rootNew = root->clone(robo, true, node);
THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed. Could not add child-from-robot from file " << i.filename);
THROW_VR_EXCEPTION_IF(!rootNew,
"Clone failed. Could not add child-from-robot from file "
<< i.filename);
std::vector<EndEffectorPtr> eefs;
r->getEndEffectors(eefs);
......@@ -980,22 +1081,26 @@ namespace VirtualRobot
const auto robotNodes = robo->getRobotNodes();
// extend children map with attachments
for(const auto& [parentNodeName, childrenNodeNames]: attachments)
for (const auto& [parentNodeName, childrenNodeNames] : attachments)
{
// find parent node
const auto parentNodeIt = std::find_if(robotNodes.begin(), robotNodes.end(), [&](const auto& robotNode){
return robotNode->getName() == parentNodeName;
});
const auto parentNodeIt = std::find_if(
robotNodes.begin(),
robotNodes.end(),
[&](const auto& robotNode) { return robotNode->getName() == parentNodeName; });
if(parentNodeIt == robotNodes.end())
if (parentNodeIt == robotNodes.end())
{
THROW_VR_EXCEPTION("Robot node `" << parentNodeName << "` was defined as as parent node but is is not known!" << std::endl);
THROW_VR_EXCEPTION("Robot node `"
<< parentNodeName
<< "` was defined as as parent node but is is not known!"
<< std::endl);
}
// add all children to the mapping
const auto& parentNode = *parentNodeIt;
for(const auto& childName : childrenNodeNames)
for (const auto& childName : childrenNodeNames)
{
robo->getRobotNode(childName)->initialize(parentNode);
}
......@@ -1013,7 +1118,8 @@ namespace VirtualRobot
for (auto& robotNodeSetNode : robotNodeSetNodes)
{
RobotNodeSetPtr rns = processRobotNodeSet(robotNodeSetNode, robo, robotRoot, rnsCounter);
RobotNodeSetPtr rns =
processRobotNodeSet(robotNodeSetNode, robo, robotRoot, rnsCounter);
//nodeSets.push_back(rns);
}
......@@ -1031,30 +1137,33 @@ namespace VirtualRobot
robo->registerNodeMapping(nodeMapping);
if(humanMapping.has_value())
if (humanMapping.has_value())
{
robo->registerHumanMapping(humanMapping.value());
}
return robo;
}
inline bool toBool(const std::string& strRepr) noexcept
inline bool
toBool(const std::string& strRepr) noexcept
{
try {
try
{
const int passiveIntRepr = std::stoi(strRepr);
return static_cast<bool>(passiveIntRepr);
} catch (std::invalid_argument&) {
}
catch (std::invalid_argument&)
{
// nothing to do here
}
return strRepr == "true";
}
RobotPtr RobotIO::processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot)
RobotPtr
RobotIO::processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot)
{
std::string robotName;
std::string robotType;
......@@ -1099,7 +1208,7 @@ namespace VirtualRobot
if (robot_name_counter[robotType] == 1)
{
ss << robotType; // first one
ss << robotType; // first one
}
else
{
......@@ -1121,11 +1230,11 @@ namespace VirtualRobot
attr = robotXMLNode->first_attribute("passive", 0, false);
if(attr != nullptr)
if (attr != nullptr)
{
const std::string passiveStrRep = attr->value();
passive = toBool(passiveStrRep);
VR_INFO << "Robot is 'passive' according to config" << std::endl;
}
......@@ -1137,7 +1246,8 @@ namespace VirtualRobot
robo->setRadianToMMfactor(atof(attr->value()));
}*/
if(passive){
if (passive)
{
robo->setPassive();
}
......@@ -1145,22 +1255,22 @@ namespace VirtualRobot
return robo;
}
void RobotIO::processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map < RobotNodePtr,
std::vector<ChildFromRobotDef> >& childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode)
void
RobotIO::processRobotChildNodes(
rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode)
{
std::vector<RobotNodePtr> robotNodes;
std::map< RobotNodePtr, std::vector< std::string > > childrenMap;
std::map<RobotNodePtr, std::vector<std::string>> childrenMap;
RobotNodePtr rootNode;
int robotNodeCounter = 0; // used for robotnodes without names
......@@ -1175,10 +1285,12 @@ namespace VirtualRobot
std::string nodeName_ = XMLNode->name();
std::string nodeName = getLowerCase(XMLNode->name());
if (nodeName == "robotnode" || nodeName == "jointnode" || nodeName == "transformationnode" || nodeName == "bodynode" || nodeName == "modelnode")
if (nodeName == "robotnode" || nodeName == "jointnode" ||
nodeName == "transformationnode" || nodeName == "bodynode" ||
nodeName == "modelnode")
{
std::vector< ChildFromRobotDef > childrenFromRobot;
std::vector< std::string > childrenNames;
std::vector<ChildFromRobotDef> childrenFromRobot;
std::vector<std::string> childrenNames;
// check for type
RobotNode::RobotNodeType rntype = RobotNode::Generic;
......@@ -1197,7 +1309,14 @@ namespace VirtualRobot
rntype = RobotNode::Transform;
}
RobotNodePtr n = processRobotNode(XMLNode, robo, basePath, robotNodeCounter, childrenNames, childrenFromRobot, loadMode, rntype);
RobotNodePtr n = processRobotNode(XMLNode,
robo,
basePath,
robotNodeCounter,
childrenNames,
childrenFromRobot,
loadMode,
rntype);
if (!n)
{
......@@ -1209,7 +1328,10 @@ namespace VirtualRobot
// double name check
for (auto& robotNode : robotNodes)
{
THROW_VR_EXCEPTION_IF((robotNode->getName() == n->getName()), "At least two RobotNodes with name <" << n->getName() << "> defined in robot definition");
THROW_VR_EXCEPTION_IF((robotNode->getName() == n->getName()),
"At least two RobotNodes with name <"
<< n->getName()
<< "> defined in robot definition");
}
childrenMap[n] = childrenNames;
......@@ -1264,7 +1386,9 @@ namespace VirtualRobot
}
else
{
THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl);
THROW_VR_EXCEPTION("XML node of type <"
<< nodeName_ << "> is not supported. Ignoring contents..."
<< endl);
}
XMLNode = XMLNode->next_sibling(nullptr, 0, false);
......@@ -1277,10 +1401,8 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION("Error while initializing Robot" << endl);
}
}
/**
* This method parses the EndEffector which are child tags of the Robot tag.
* Each EndEffector has a name, a base node, a list of static robot nodes and a list of actors.
......@@ -1291,7 +1413,8 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::EndEffector
*/
EndEffectorPtr RobotIO::processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo)
EndEffectorPtr
RobotIO::processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo)
{
std::string endeffectorName("");
std::string baseNodeName;
......@@ -1310,37 +1433,60 @@ namespace VirtualRobot
if ("name" == attributeName)
{
THROW_VR_EXCEPTION_IF(!endeffectorName.empty(), "Endeffector tag has more than one <name> tag. Value of the first one is: " + endeffectorName);
THROW_VR_EXCEPTION_IF(
!endeffectorName.empty(),
"Endeffector tag has more than one <name> tag. Value of the first one is: " +
endeffectorName);
endeffectorName = attr->value();
THROW_VR_EXCEPTION_IF(endeffectorName.empty(), "Endeffector tag does not specify a <name> tag.");
THROW_VR_EXCEPTION_IF(endeffectorName.empty(),
"Endeffector tag does not specify a <name> tag.");
}
else if ("base" == attributeName)
{
THROW_VR_EXCEPTION_IF(!baseNodeName.empty(), "Endeffector tag has more than one <base> tag. Value of the first one is: " + baseNodeName);
THROW_VR_EXCEPTION_IF(
!baseNodeName.empty(),
"Endeffector tag has more than one <base> tag. Value of the first one is: " +
baseNodeName);
baseNodeName = attr->value();
THROW_VR_EXCEPTION_IF(baseNodeName.empty(), "Endeffector tag does not specify a <base> tag.");
THROW_VR_EXCEPTION_IF(baseNodeName.empty(),
"Endeffector tag does not specify a <base> tag.");
baseNode = robo->getRobotNode(baseNodeName);
THROW_VR_EXCEPTION_IF(!baseNode, "base associated with <Endeffector> not available in the robot model.");
THROW_VR_EXCEPTION_IF(
!baseNode,
"base associated with <Endeffector> not available in the robot model.");
}
else if ("tcp" == attributeName)
{
THROW_VR_EXCEPTION_IF(!tcpNodeName.empty(), "Endeffector tag has more than one <tcp> tag. Value of the first one is: " + tcpNodeName);
THROW_VR_EXCEPTION_IF(
!tcpNodeName.empty(),
"Endeffector tag has more than one <tcp> tag. Value of the first one is: " +
tcpNodeName);
tcpNodeName = attr->value();
THROW_VR_EXCEPTION_IF(tcpNodeName.empty(), "Endeffector tag does not specify a <tcp> tag.");
THROW_VR_EXCEPTION_IF(tcpNodeName.empty(),
"Endeffector tag does not specify a <tcp> tag.");
tcpNode = robo->getRobotNode(tcpNodeName);
THROW_VR_EXCEPTION_IF(!tcpNode, "tcp associated with <Endeffector> not available in the robot model.");
THROW_VR_EXCEPTION_IF(
!tcpNode,
"tcp associated with <Endeffector> not available in the robot model.");
}
else if ("gcp" == attributeName)
{
THROW_VR_EXCEPTION_IF(!gcpNodeName.empty(), "Endeffector tag has more than one <gcp> tag. Value of the first one is: " + gcpNodeName);
THROW_VR_EXCEPTION_IF(
!gcpNodeName.empty(),
"Endeffector tag has more than one <gcp> tag. Value of the first one is: " +
gcpNodeName);
gcpNodeName = attr->value();
THROW_VR_EXCEPTION_IF(gcpNodeName.empty(), "Endeffector tag does not specify a <gcp> tag.");
THROW_VR_EXCEPTION_IF(gcpNodeName.empty(),
"Endeffector tag does not specify a <gcp> tag.");
gcpNode = robo->getRobotNode(gcpNodeName);
THROW_VR_EXCEPTION_IF(!gcpNode, "gcp associated with <Endeffector> not available in the robot model.");
THROW_VR_EXCEPTION_IF(
!gcpNode,
"gcp associated with <Endeffector> not available in the robot model.");
}
else
{
VR_WARNING << "Ignoring unknown attribute in EEF <" << endeffectorName << "> definition:" << attributeName << std::endl;
VR_WARNING << "Ignoring unknown attribute in EEF <" << endeffectorName
<< "> definition:" << attributeName << std::endl;
}
attr = attr->next_attribute();
......@@ -1348,9 +1494,9 @@ namespace VirtualRobot
std::vector<RobotNodePtr> staticParts;
std::vector<EndEffectorActorPtr> actors;
std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
std::vector< std::string > configNames;
std::vector< std::string > tcpNames;
std::vector<std::vector<RobotConfig::Configuration>> configDefinitions;
std::vector<std::string> configNames;
std::vector<std::string> tcpNames;
rapidxml::xml_node<>* node = endeffectorXMLNode->first_node();
while (node)
......@@ -1369,17 +1515,22 @@ namespace VirtualRobot
}
else
{
VR_ERROR << "There should only be one <static> tag inside <endeffector> tags" << std::endl;
VR_ERROR << "There should only be one <static> tag inside <endeffector> tags"
<< std::endl;
}
}
else if ("preshape" == nodeName)
{
bool cOK = processConfigurationNodeList(node, configDefinitions, configNames, tcpNames);
THROW_VR_EXCEPTION_IF(!cOK, "Invalid Preshape defined in robot's eef tag '" << nodeName << "'." << endl);
bool cOK =
processConfigurationNodeList(node, configDefinitions, configNames, tcpNames);
THROW_VR_EXCEPTION_IF(!cOK,
"Invalid Preshape defined in robot's eef tag '"
<< nodeName << "'." << endl);
}
else
{
THROW_VR_EXCEPTION("XML tag <" << nodeName << "> not supported in endeffector <" << nodeName << ">");
THROW_VR_EXCEPTION("XML tag <" << nodeName << "> not supported in endeffector <"
<< nodeName << ">");
}
node = node->next_sibling();
......@@ -1395,10 +1546,12 @@ namespace VirtualRobot
gcpNode = tcpNode;
}
EndEffectorPtr endEffector(new EndEffector(endeffectorName, actors, staticParts, baseNode, tcpNode, gcpNode));
EndEffectorPtr endEffector(
new EndEffector(endeffectorName, actors, staticParts, baseNode, tcpNode, gcpNode));
// create & register configs
THROW_VR_EXCEPTION_IF(configDefinitions.size() != configNames.size(), "Invalid Preshape definitions " << endl);
THROW_VR_EXCEPTION_IF(configDefinitions.size() != configNames.size(),
"Invalid Preshape definitions " << endl);
for (size_t i = 0; i < configDefinitions.size(); i++)
{
......@@ -1413,17 +1566,20 @@ namespace VirtualRobot
return endEffector;
}
/**
* This method processes the attributes and the children of an actor tag which
* itself is a child of the endeffector tag.
* First the name attribute is retrieved and afterwards the child nodes are
* processed which make up the actor.
*/
EndEffectorActorPtr RobotIO::processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo)
EndEffectorActorPtr
RobotIO::processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
RobotPtr robo)
{
std::string actorName = processNameAttribute(endeffectorActorXMLNode);
THROW_VR_EXCEPTION_IF(actorName.empty(), "<Actor> tag inside <Endeffector> does not specify a <name> attribute.");
THROW_VR_EXCEPTION_IF(
actorName.empty(),
"<Actor> tag inside <Endeffector> does not specify a <name> attribute.");
std::vector<EndEffectorActor::ActorDefinition> actors;
processActorNodeList(endeffectorActorXMLNode, robo, actors);
......@@ -1431,17 +1587,18 @@ namespace VirtualRobot
return actor;
}
/**
* This method processes the children of the static tag which
* itself is a child of the endeffector tag.
*/
void RobotIO::processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList)
void
RobotIO::processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode,
RobotPtr robo,
std::vector<RobotNodePtr>& staticNodesList)
{
processNodeList(endeffectorStaticXMLNode, robo, staticNodesList, false);
}
/**
* This method processes the \p parentNode Tag and extracts a list of \<Node name="xyz" speed="0123" /\> tags.
* All other child tags raise a VirtualRobot::VirtualRobotException.
......@@ -1449,7 +1606,11 @@ namespace VirtualRobot
*
* If the parameter \p clearList is true all elements from \p nodeList are removed.
*/
void RobotIO::processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList /*= true*/)
void
RobotIO::processActorNodeList(rapidxml::xml_node<char>* parentNode,
RobotPtr robot,
std::vector<EndEffectorActor::ActorDefinition>& actorList,
bool clearList /*= true*/)
{
if (clearList)
{
......@@ -1468,9 +1629,13 @@ namespace VirtualRobot
{
EndEffectorActor::ActorDefinition actor;
std::string nodeNameAttr = processNameAttribute(node, true);
THROW_VR_EXCEPTION_IF(nodeNameAttr.empty(), "Missing name attribute for <Node> belonging to Robot node set " << parentName);
THROW_VR_EXCEPTION_IF(
nodeNameAttr.empty(),
"Missing name attribute for <Node> belonging to Robot node set " << parentName);
actor.robotNode = robot->getRobotNode(nodeNameAttr);
THROW_VR_EXCEPTION_IF(!actor.robotNode, "<node> tag with name '" << nodeNameAttr << "' not present in the current robot");
THROW_VR_EXCEPTION_IF(!actor.robotNode,
"<node> tag with name '"
<< nodeNameAttr << "' not present in the current robot");
actor.directionAndSpeed = processFloatAttribute(speedname, node, true);
if (actor.directionAndSpeed == 0.0f)
......@@ -1483,15 +1648,18 @@ namespace VirtualRobot
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Actor> with name " << parentName);
THROW_VR_EXCEPTION("XML definition <" << nodeName
<< "> not supported in <Actor> with name "
<< parentName);
}
node = node->next_sibling();
}
}
EndEffectorActor::CollisionMode RobotIO::processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes /* = false */)
EndEffectorActor::CollisionMode
RobotIO::processEEFColAttributes(rapidxml::xml_node<char>* node,
bool allowOtherAttributes /* = false */)
{
THROW_VR_EXCEPTION_IF(!node, "Can not process name attribute of NULL node" << endl);
......@@ -1527,15 +1695,17 @@ namespace VirtualRobot
}
else
{
THROW_VR_EXCEPTION("<" << node->name() << "> considerCollisions attribute is unknowne: " << name);
THROW_VR_EXCEPTION("<"
<< node->name()
<< "> considerCollisions attribute is unknowne: " << name);
}
}
else
{
if (!allowOtherAttributes)
{
THROW_VR_EXCEPTION("<" << node->name() << "> tag contains unknown attribute: " << attr->name());
THROW_VR_EXCEPTION("<" << node->name()
<< "> tag contains unknown attribute: " << attr->name());
}
}
......@@ -1551,9 +1721,10 @@ namespace VirtualRobot
return result;
}
VirtualRobot::RobotPtr RobotIO::createRobotFromString(const std::string& xmlString, const std::string& basePath, RobotDescription loadMode)
VirtualRobot::RobotPtr
RobotIO::createRobotFromString(const std::string& xmlString,
const std::string& basePath,
RobotDescription loadMode)
{
// copy string content to char array
char* y = new char[xmlString.size() + 1];
......@@ -1563,8 +1734,8 @@ namespace VirtualRobot
try
{
rapidxml::xml_document<char> doc; // character type defaults to char
doc.parse<0>(y); // 0 means default parse flags
rapidxml::xml_document<char> doc; // character type defaults to char
doc.parse<0>(y); // 0 means default parse flags
rapidxml::xml_node<char>* robotXMLNode = doc.first_node("robot", 0, false);
if (!robotXMLNode)
......@@ -1577,9 +1748,11 @@ namespace VirtualRobot
catch (rapidxml::parse_error& e)
{
delete[] y;
THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
THROW_VR_EXCEPTION("Could not parse data in xml definition"
<< endl
<< "Error message:" << e.what() << endl
<< "Position: " << endl << e.where<char>() << endl);
<< "Position: " << endl
<< e.where<char>() << endl);
return RobotPtr();
}
catch (VirtualRobot::VirtualRobotException&)
......@@ -1592,7 +1765,8 @@ namespace VirtualRobot
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
<< "Error code:" << e.what() << endl);
<< "Error code:" << e.what()
<< endl);
return RobotPtr();
}
catch (...)
......@@ -1613,8 +1787,8 @@ namespace VirtualRobot
return robot;
}
VirtualRobot::RobotPtr RobotIO::loadRobot(const std::string& modelFile, RobotDescription loadMode)
VirtualRobot::RobotPtr
RobotIO::loadRobot(const std::string& modelFile, RobotDescription loadMode)
{
std::string fullFile = modelFile;
......@@ -1625,10 +1799,9 @@ namespace VirtualRobot
{
VR_ERROR << "Could not open " + fileType + " file:" << modelFile << std::endl;
return RobotPtr();
}
if(fileType == "xml")
if (fileType == "xml")
{
std::ifstream in(fullFile.c_str());
......@@ -1658,9 +1831,8 @@ namespace VirtualRobot
res->applyJointValues();
res->setFilename(fullFile);
return res;
}
else if(fileType == "urdf")
else if (fileType == "urdf")
{
SimoxURDFFactory f;
......@@ -1677,17 +1849,23 @@ namespace VirtualRobot
VirtualRobot::RobotPtr r = f.loadFromFile(modelFile);
return r;
}
else
{
std::cout << "File does not have correct file Type!" << std::endl;
std::cout << "File does not have correct file Type!" << std::endl;
return RobotPtr();
}
}
bool RobotIO::saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir, bool storeEEF, bool storeRNS, bool storeSensors, bool storeModelFiles)
bool
RobotIO::saveXML(RobotPtr robot,
const std::string& filename,
const std::string& basePath,
const std::string& modelDir,
bool storeEEF,
bool storeRNS,
bool storeSensors,
bool storeModelFiles)
{
THROW_VR_EXCEPTION_IF(!robot, "NULL data");
......@@ -1698,9 +1876,11 @@ namespace VirtualRobot
std::filesystem::path fnComplete = p / fn;
std::filesystem::path modelDirComplete = p / pModelDir;
if (std::filesystem::exists(modelDirComplete) && !std::filesystem::is_directory(modelDirComplete))
if (std::filesystem::exists(modelDirComplete) &&
!std::filesystem::is_directory(modelDirComplete))
{
VR_ERROR << "Could not create model directory (existing & !dir) " << pModelDir.string() << std::endl;
VR_ERROR << "Could not create model directory (existing & !dir) " << pModelDir.string()
<< std::endl;
return false;
}
......@@ -1708,7 +1888,8 @@ namespace VirtualRobot
{
if (!std::filesystem::create_directories(modelDirComplete))
{
VR_ERROR << "Could not create model dir " << modelDirComplete.string() << std::endl;
VR_ERROR << "Could not create model dir " << modelDirComplete.string()
<< std::endl;
return false;
}
}
......@@ -1721,7 +1902,8 @@ namespace VirtualRobot
return false;
}
std::string xmlRob = robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors, storeModelFiles);
std::string xmlRob =
robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors, storeModelFiles);
f << xmlRob;
f.close();
......@@ -1738,8 +1920,11 @@ namespace VirtualRobot
return true;
}
void RobotIO::saveMJCF(RobotPtr robot, const std::string& filename,
const std::string& basePath, const std::string& meshDir)
void
RobotIO::saveMJCF(RobotPtr robot,
const std::string& filename,
const std::string& basePath,
const std::string& meshDir)
{
mujoco::RobotMjcf mjcf(robot);
mjcf.setOutputPaths(std::filesystem::path(basePath) / filename, meshDir);
......
VirtualRobot/XML/RobotIO.h
+ 74
− 43
View file @ 17cb0764
......@@ -22,22 +22,20 @@
*/
#pragma once
#include "../VirtualRobot.h"
#include "../Robot.h"
#include "../Nodes/RobotNode.h"
#include "../EndEffector/EndEffectorActor.h"
#include "BaseIO.h"
#include <map>
#include <string>
#include <vector>
#include <map>
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNode.h"
#include "../Robot.h"
#include "../VirtualRobot.h"
#include "BaseIO.h"
// using forward declarations here, so that the rapidXML header does not have to be parsed when this file is included
namespace rapidxml
{
template<class Ch>
template <class Ch>
class xml_node;
}
......@@ -47,7 +45,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotIO : public BaseIO
{
public:
/*!
Loads robot from file.
@param xmlFile The file
......@@ -62,7 +59,9 @@ namespace VirtualRobot
@param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified.
@param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access.
*/
static RobotPtr createRobotFromString(const std::string& xmlString, const std::string& basePath = "", RobotDescription loadMode = eFull);
static RobotPtr createRobotFromString(const std::string& xmlString,
const std::string& basePath = "",
RobotDescription loadMode = eFull);
/*!
......@@ -72,7 +71,14 @@ namespace VirtualRobot
@param basePath The directory to store the robot to
@param modelDir The local directory where all visualization files should be stored to.
*/
static bool saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true, bool storeModelFiles = true);
static bool saveXML(RobotPtr robot,
const std::string& filename,
const std::string& basePath,
const std::string& modelDir = "models",
bool storeEEF = true,
bool storeRNS = true,
bool storeSensors = true,
bool storeModelFiles = true);
/*!
......@@ -83,12 +89,12 @@ namespace VirtualRobot
@param meshDir The local directory where all mesh files should be stored to.
*/
static void saveMJCF(RobotPtr robot,
const std::string& filename, const std::string& basePath,
const std::string& filename,
const std::string& basePath,
const std::string& meshDir = "mesh");
protected:
struct ChildFromRobotDef
{
std::string filename;
......@@ -99,42 +105,67 @@ namespace VirtualRobot
RobotIO();
~RobotIO() override;
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode = eFull);
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot);
static void processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map < RobotNodePtr,
std::vector<ChildFromRobotDef> > & childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode = eFull);
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode,
const std::string& basePath,
RobotDescription loadMode = eFull);
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode,
std::string& robotRoot);
static void processRobotChildNodes(
rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode = eFull);
static RobotNodePtr processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo,
const std::string& basePath,
int& robotNodeCounter,
std::vector< std::string >& childrenNames,
std::vector< ChildFromRobotDef >& childrenFromRobot,
std::vector<std::string>& childrenNames,
std::vector<ChildFromRobotDef>& childrenFromRobot,
RobotDescription loadMode = eFull,
RobotNode::RobotNodeType rntype = RobotNode::Generic);
static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo);
static EndEffectorActorPtr processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo);
static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList);
static EndEffectorActor::CollisionMode processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false);
static void processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList = true);
static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode,
RobotPtr robo);
static EndEffectorActorPtr
processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
RobotPtr robo);
static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode,
RobotPtr robo,
std::vector<RobotNodePtr>& staticNodesList);
static EndEffectorActor::CollisionMode
processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false);
static void processActorNodeList(rapidxml::xml_node<char>* parentNode,
RobotPtr robot,
std::vector<EndEffectorActor::ActorDefinition>& actorList,
bool clearList = true);
//static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char> *setXMLNode, RobotPtr robo, const std::string &rootName, int &robotNodeSetCounter);
static void processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames);
static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel,
SceneObject::Physics& physics, RobotNode::RobotNodeType rntype, Eigen::Matrix4f& transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot);
static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless);
static void processChildNode(rapidxml::xml_node<char>* childXMLNode,
std::vector<std::string>& childrenNames);
static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode,
const std::string& robotNodeName,
RobotPtr robot,
VisualizationNodePtr visualizationNode,
CollisionModelPtr collisionModel,
SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype,
Eigen::Matrix4f& transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
const std::string& nodeName,
std::vector<ChildFromRobotDef>& childrenFromRobot);
static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
float& jointLimitLo,
float& jointLimitHi,
bool& limitless);
static std::map<std::string, int> robot_name_counter;
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string& robotNodeName, const std::string& basePath);
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode,
const std::string& robotNodeName,
const std::string& basePath);
};
}
} // namespace VirtualRobot
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment