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; }