ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");
bool visuProcessed = false;
bool colProcessed = false;
VisualizationNodePtr visualizationNode;
CollisionModelPtr collisionModel;
bool useAsColModel = false;
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
// get name
std::string objName = processNameAttribute(objectXMLNode);
// first check if there is an xml file to load
rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file", 0, false);
if (xmlFileNode)
{
std::string xmlFile = processFileNode(xmlFileNode, basePath);
ObstaclePtr result = loadObstacle(xmlFile);
if (!result)
{
return result;
}
if (!objName.empty())
{
result->setName(objName);
}
// update global pose
rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose", 0, false);
if (poseNode)
{
processTransformNode(poseNode, objName, globalPose);
result->setGlobalPose(globalPose);
}
return result;
}
THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'");
rapidxml::xml_node<>* node = objectXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "visualization")
{
THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
std::string colModelName = objName;
colModelName += "_VISU_ColModel";
// todo: ID?
collisionModel.reset(new CollisionModel(visualizationNode, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
else if (nodeName == "collisionmodel")
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
processPhysicsTag(node, objName, physics);
physicsDefined = true;
}
else if (nodeName == "globalpose")
{
processTransformNode(node, objName, globalPose);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
}
node = node->next_sibling();
}
// build object
ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
object->setGlobalPose(globalPose);
return object;
}
bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneRobot");
// get name
std::string robotName = processNameAttribute(sceneXMLNode,true);
if (robotName.empty())
{
THROW_VR_EXCEPTION("Please specify the name of the robot...");
return false;
}
std::string initStr("initconfig");
std::string initConfigName = processStringAttribute(initStr,sceneXMLNode,true);
std::vector< RobotConfigPtr > configs;
std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
std::vector< std::string > configNames;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
std::string fileName;
rapidxml::xml_node<>* node = sceneXMLNode->first_node();
std::vector< rapidxml::xml_node<>* > rnsNodes;
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "file")
{
THROW_VR_EXCEPTION_IF(!fileName.empty(), "Multiple files defined in scene's robot tag '" << robotName << "'." << endl);
fileName = processFileNode(node,basePath);
} else if (nodeName == "configuration")
{
bool c*K = processConfigurationNodeList(node, configDefinitions, configNames);
THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in scene's robot tag '" << robotName << "'." << endl);
} else if (nodeName == "globalpose")
{
processTransformNode(node, robotName, globalPose);
} else if (nodeName == "robotnodeset")
{
rnsNodes.push_back(node);
} else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Robot definition <" << robotName << ">." << endl);
}
node = node->next_sibling();
}
// create & register robot
THROW_VR_EXCEPTION_IF(fileName.empty(), "Missing file definition in scene's robot tag '" << robotName << "'." << endl);
RobotPtr robot = RobotIO::loadRobot(fileName);
THROW_VR_EXCEPTION_IF(!robot, "Invalid robot file in scene's robot tag '" << robotName << "'." << endl);
robot->setGlobalPose(globalPose);
scene->registerRobot(robot);
// create & register node sets
int rnsNr = 0;
for (size_t i=0;i<rnsNodes.size();i++)
{
// registers rns to robot
RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr);
THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl);
}
// create & register configs
THROW_VR_EXCEPTION_IF(configDefinitions.size()!=configNames.size(), "Invalid RobotConfig definitions " << endl);
for (size_t i=0;i<configDefinitions.size();i++)
{
RobotConfigPtr rc(new RobotConfig(robot,configNames[i],configDefinitions[i]));
scene->registerRobotConfig(robot,rc);
}
// process init config
if (!initConfigName.empty())
{
THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl);
robot->setJointValues(scene->getRobotConfig(robot,initConfigName));
}
return true;
}
