VirtualRobot::ObstaclePtr ObjectIO::createObstacleFromString(const std::string& xmlString, const std::string& basePath /*= ""*/)
{
// copy string content to char array
char* y = new char[xmlString.size() + 1];
strncpy(y, xmlString.c_str(), xmlString.size() + 1);
VirtualRobot::ObstaclePtr obj;
try
{
rapidxml::xml_document<char> doc; // character type defaults to char
doc.parse<0>(y); // 0 means default parse flags
rapidxml::xml_node<char>* objectXMLNode = doc.first_node("Obstacle");
obj = processObstacle(objectXMLNode, basePath);
}
catch (rapidxml::parse_error& e)
{
delete[] y;
THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
<< "Error message:" << e.what() << endl
<< "Position: " << endl << e.where<char>() << endl);
return ObstaclePtr();
}
catch (VirtualRobot::VirtualRobotException&)
{
// rethrow the current exception
delete[] y;
throw;
}
catch (std::exception& e)
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
<< "Error code:" << e.what() << endl);
return ObstaclePtr();
}
catch (...)
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl);
return ObstaclePtr();
}
delete[] y;
return obj;
}
void ModularStateEstimator::addObstacle(core::ConfigNode obstacleNode,
Obstacle::ObstacleType type)
{
ObstaclePtr obstacle = ObstaclePtr(new Obstacle());
math::Vector3 location;
location[0] = obstacleNode["location"][0].asDouble();
location[1] = obstacleNode["location"][1].asDouble();
location[2] = obstacleNode["location"][2].asDouble();
math::Matrix3 covariance;
covariance[0][0] = obstacleNode["covariance"][0][0].asDouble(5);
covariance[0][1] = obstacleNode["covariance"][0][1].asDouble(0);
covariance[0][2] = obstacleNode["covariance"][0][2].asDouble(0);
covariance[1][0] = obstacleNode["covariance"][1][0].asDouble(0);
covariance[1][1] = obstacleNode["covariance"][1][1].asDouble(5);
covariance[1][2] = obstacleNode["covariance"][1][2].asDouble(0);
covariance[2][0] = obstacleNode["covariance"][2][0].asDouble(0);
covariance[2][1] = obstacleNode["covariance"][2][1].asDouble(0);
covariance[2][2] = obstacleNode["covariance"][2][2].asDouble(1);
double heading = obstacleNode["attackHeading"].asDouble(0);
math::Quaternion attackOrientation = math::Quaternion(math::Degree(heading), math::Vector3::UNIT_Z);
obstacle->setLocation(location);
obstacle->setLocationCovariance(covariance);
obstacle->setAttackOrientation(attackOrientation);
m_estimatedState->addObstacle(type, obstacle);
}
