const char *rname(const region * r, const struct locale *lang)
{
if (r->land && r->land->name) {
return r->land->name;
}
return LOC(lang, terrain_name(r));
}
bool RobotEnvironment::loadObstaclesXML(std::string& obstacles_file)
{
if (!frapu::fileExists(obstacles_file)) {
cout << "RobotEnvironment: ERROR: Environment file '" << obstacles_file << "' doesn't exist" << endl;
return false;
}
std::vector<frapu::ObstacleSharedPtr> obstacles;
std::vector<ObstacleStruct> obstaclesStruct;
TiXmlDocument xml_doc;
xml_doc.LoadFile(obstacles_file);
TiXmlElement* env_xml = xml_doc.FirstChildElement("Environment");
for (TiXmlElement* obst_xml = env_xml->FirstChildElement("KinBody"); obst_xml; obst_xml = obst_xml->NextSiblingElement("KinBody")) {
std::string name(obst_xml->Attribute("name"));
TiXmlElement* body_xml = obst_xml->FirstChildElement("Body");
std::string enable_str(body_xml->Attribute("enable"));
if (enable_str == "true") {
TiXmlElement* body_xml = obst_xml->FirstChildElement("Body");
if (body_xml) {
TiXmlElement* geom_xml = body_xml->FirstChildElement("Geom");
TiXmlElement* terrain_xml = body_xml->FirstChildElement("Terrain");
//Can play with different shapes here in the future
if (geom_xml) {
std::string type(geom_xml->Attribute("type"));
TiXmlElement* trans_xml = geom_xml->FirstChildElement("Translation");
if (trans_xml) {
obstaclesStruct.push_back(ObstacleStruct());
const char* xyz_str = trans_xml->GetText();
std::vector<std::string> pieces;
std::vector<double> xyz_vec;
boost::split(pieces, xyz_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i) {
if (pieces[i] != "") {
xyz_vec.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
}
TiXmlElement* dcolor_xml = geom_xml->FirstChildElement("diffuseColor");
if (dcolor_xml) {
const char* color_string = dcolor_xml->GetText();
std::vector<std::string> pieces;
std::vector<double> color_vec;
boost::split(pieces, color_string, boost::is_any_of(" "));
for (unsigned i = 0; i < pieces.size(); i++) {
color_vec.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
obstaclesStruct[obstaclesStruct.size() - 1].d_color.push_back(color_vec[0]);
obstaclesStruct[obstaclesStruct.size() - 1].d_color.push_back(color_vec[1]);
obstaclesStruct[obstaclesStruct.size() - 1].d_color.push_back(color_vec[2]);
}
TiXmlElement* acolor_xml = geom_xml->FirstChildElement("ambientColor");
if (acolor_xml) {
const char* color_string = acolor_xml->GetText();
std::vector<std::string> pieces;
std::vector<double> color_vec;
boost::split(pieces, color_string, boost::is_any_of(" "));
for (unsigned i = 0; i < pieces.size(); i++) {
color_vec.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
obstaclesStruct[obstaclesStruct.size() - 1].a_color.push_back(color_vec[0]);
obstaclesStruct[obstaclesStruct.size() - 1].a_color.push_back(color_vec[1]);
obstaclesStruct[obstaclesStruct.size() - 1].a_color.push_back(color_vec[2]);
}
obstaclesStruct[obstaclesStruct.size() - 1].name = name;
obstaclesStruct[obstaclesStruct.size() - 1].type = type;
obstaclesStruct[obstaclesStruct.size() - 1].x = xyz_vec[0];
obstaclesStruct[obstaclesStruct.size() - 1].y = xyz_vec[1];
obstaclesStruct[obstaclesStruct.size() - 1].z = xyz_vec[2];
if (type == "box") {
TiXmlElement* ext_xml = geom_xml->FirstChildElement("extents");
if (ext_xml) {
const char* ext_str = ext_xml->GetText();
std::vector<double> extends_vec;
pieces.clear();
boost::split(pieces, ext_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i) {
if (pieces[i] != "") {
extends_vec.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
}
obstaclesStruct[obstaclesStruct.size() - 1].extends.push_back(extends_vec[0]);
obstaclesStruct[obstaclesStruct.size() - 1].extends.push_back(extends_vec[1]);
obstaclesStruct[obstaclesStruct.size() - 1].extends.push_back(extends_vec[2]);
}
} else if (type == "sphere") {
TiXmlElement* rad_xml = geom_xml->FirstChildElement("Radius");
if (rad_xml) {
obstaclesStruct[obstaclesStruct.size() - 1].extends.push_back(boost::lexical_cast<double>(rad_xml->GetText()));
}
}
}
}
if (terrain_xml) {
TerrainStruct terrain;
std::string terrain_name(terrain_xml->Attribute("name"));
TiXmlElement* damping_xml = terrain_xml->FirstChildElement("Damping");
double damping = boost::lexical_cast<double>(damping_xml->GetText());
TiXmlElement* cost_xml = terrain_xml->FirstChildElement("Cost");
double cost = boost::lexical_cast<double>(cost_xml->GetText());
TiXmlElement* traversable_xml = terrain_xml->FirstChildElement("Traversable");
bool traversable = false;
if (traversable_xml) {
if (boost::lexical_cast<std::string>(traversable_xml->GetText()) == "true") {
traversable = true;
}
}
bool observable = true;
TiXmlElement* observable_xml = terrain_xml->FirstChildElement("Observable");
if (observable_xml) {
if (boost::lexical_cast<std::string>(observable_xml->GetText()) == "false") {
observable = false;
}
}
terrain.name = terrain_name;
terrain.velocityDamping = damping;
terrain.traversalCost = cost;
terrain.traversable = traversable;
terrain.observable = observable;
obstaclesStruct[obstaclesStruct.size() - 1].terrain = terrain;
}
}
}
}
for (size_t i = 0; i < obstaclesStruct.size(); i++) {
frapu::TerrainSharedPtr terrain = std::make_shared<frapu::TerrainImpl>(obstaclesStruct[i].terrain.name,
obstaclesStruct[i].terrain.traversalCost,
obstaclesStruct[i].terrain.velocityDamping,
obstaclesStruct[i].terrain.traversable,
obstaclesStruct[i].terrain.observable);
if (obstaclesStruct[i].type == "box") {
frapu::ObstacleSharedPtr obstacle = std::make_shared<frapu::BoxObstacle>(obstaclesStruct[i].name,
obstaclesStruct[i].x,
obstaclesStruct[i].y,
obstaclesStruct[i].z,
obstaclesStruct[i].extends[0],
obstaclesStruct[i].extends[1],
obstaclesStruct[i].extends[2],
terrain);
obstacles.push_back(obstacle);
}
else if (obstaclesStruct[i].type == "sphere") {
frapu::ObstacleSharedPtr obstacle = std::make_shared<frapu::SphereObstacle>(obstaclesStruct[i].name,
obstaclesStruct[i].x,
obstaclesStruct[i].y,
obstaclesStruct[i].z,
obstaclesStruct[i].extends[0],
terrain);
obstacles.push_back(obstacle);
} else {
assert(false && "Utils: ERROR: Obstacle has an unknown type!");
}
static_cast<frapu::ObstacleImpl*>(obstacles[obstacles.size() - 1].get())->setStandardColor(obstaclesStruct[i].d_color,
obstaclesStruct[i].a_color);
//obstacles_[obstacles_.size() - 1]->setStandardColor(obstacles[i].d_color, obstacles[i].a_color);
}
scene_->setObstacles(obstacles);
return true;
}
