void MainWindow::OnSaveObjectXML(wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree* itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
if(itemData->menus.menu_positionable && m_root!=itemId)
{
wxFileDialog saveFile(this,wxT("Save Object file XML"), wxEmptyString, wxEmptyString,
wxT("XML files (*.xml)|*.xml"),wxFD_SAVE|wxFD_OVERWRITE_PROMPT);
if(saveFile.ShowModal() == wxID_OK)
{
wxString fileName = saveFile.GetPath();
char c_file[100];
strcpy(c_file,(const char*)fileName.mb_str(wxConvUTF8));
XMLfile xml_file(c_file);
itemData->pointer.positionableentity->setRelativeT3D(0);
xml_file.write(itemData->pointer.positionableentity);
xml_file.save();
wxLogMessage(wxT("Successfully save in %s"), saveFile.GetPath().c_str());
}
}
else wxLogMessage(wxT("Please select a Object."));
}
void MainWindow::SaveRobotAsRobotSimXml(wxCommandEvent& event)
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree* itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
if(itemData->menus.menu_robotsim && m_root!=itemId)
{
wxFileDialog saveFile(this,wxT("Save Object file XML"), wxEmptyString, wxEmptyString,
wxT("XML files (*.xml)|*.xml"),wxFD_SAVE|wxFD_OVERWRITE_PROMPT);
if(saveFile.ShowModal() == wxID_OK)
{
wxString fileName = saveFile.GetPath();
char c_file[100];
strcpy(c_file,(const char*)fileName.mb_str(wxConvUTF8));
XMLfile xml_file(c_file);
itemData->pointer.positionableentity->setRelativeT3D(0);
string className = "RobotSim";
XMLElement* obj_xml=new XMLElement (xml_file.getRoot(),className.c_str());
RobotSim* robot;
robot=dynamic_cast<RobotSim *>(itemData->pointer.robotsim);
robot->writeToXMLasRobotSim(obj_xml);
xml_file.getRoot()->AddElement(obj_xml);
xml_file.getXMLElementsObjects().push_back(obj_xml);
xml_file.save();
wxLogMessage(wxT("Successfully save in %s"), saveFile.GetPath().c_str());
}
}
else wxLogMessage(wxT("Please select a Object."));
}
virtual void SetUp()
{
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file("../../../src/moveit_resources/test/urdf/robot.xml", std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
urdf_ok_ = urdf_model_;
}
else
urdf_ok_ = false;
srdf_ok_ = srdf_model_->initFile(*urdf_model_, "../../../src/moveit_resources/test/srdf/robot.xml");
kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
std::map<std::string, std::vector<collision_detection::CollisionSphere> > link_body_decompositions;
crobot_.reset(new DefaultCRobotType(kmodel_, link_body_decompositions));
cworld_.reset(new DefaultCWorldType());
}
void MainWindow::OnSaveWorldXML(wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
for(unsigned int i= 0; i<listWorlds.size(); i++)
{
if(listWorlds[i]->getTreeItem()==itemId)
{
wxFileDialog saveFile(this,wxT("Save World file XML "),wxEmptyString,wxEmptyString,
wxT("XML files (*.xml)|*.xml"),wxFD_SAVE|wxFD_OVERWRITE_PROMPT);
if(saveFile.ShowModal() == wxID_OK)
{
wxString file = saveFile.GetPath();
char filec[100];
strcpy(filec, (const char*) file.mb_str(wxConvUTF8));
XMLfile xml_file(filec);
xml_file.write(listWorlds[i]->getWorld());
xml_file.save();
wxLogMessage(wxT("Successfully save in %s"), saveFile.GetPath().c_str());
}
}
else wxLogMessage(wxT("Please, select a World."));
}
}
bool VBridge::loadConfig(const std::string& urdf_name)
{
//read from file first
std::string xml_string;
std::fstream xml_file(urdf_name.c_str(), std::fstream::in);
while ( xml_file.good() ) {
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
//parse string
boost::shared_ptr<urdf::ModelInterface> robot = urdf::parseURDF(xml_string);
if(!robot)
return false;
for(std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it=robot->joints_.begin();
it!=robot->joints_.end();it++)
{
if(it->second->type==urdf::Joint::REVOLUTE || it->second->type==urdf::Joint::CONTINUOUS ||
it->second->type==urdf::Joint::PRISMATIC)
m_jntnames.push_back(it->first);
}
for(unsigned int i=0;i<m_jntnames.size();i++)
std::cout << m_jntnames[i] << std::endl;
return true;
}
virtual void SetUp()
{
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file("../planning_models/test/urdf/robot.xml", std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
urdf_ok_ = urdf_model_;
}
else
urdf_ok_ = false;
srdf_ok_ = srdf_model_->initFile(*urdf_model_, "../planning_models/test/srdf/robot.xml");
kmodel_.reset(new planning_models::KinematicModel(urdf_model_, srdf_model_));
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
crobot_.reset(new DefaultCRobotType(kmodel_));
cworld_.reset(new DefaultCWorldType());
}
virtual void SetUp()
{
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file("../kinematic_state/test/urdf/robot.xml", std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
urdf_ok_ = urdf_model_;
}
else
urdf_ok_ = false;
srdf_ok_ = srdf_model_->initFile(*urdf_model_, "../kinematic_state/test/srdf/robot.xml");
if (urdf_ok_ && srdf_ok_)
kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
};
//static
void LLFloaterAO::onClickLoad(void* user_data)
{
LLFloaterAO* floater = (LLFloaterAO*)user_data;
LLFilePicker& file_picker = LLFilePicker::instance();
if(file_picker.getOpenFile(LLFilePicker::FFLOAD_AO))
{
std::string file_name = file_picker.getFirstFile();
llifstream xml_file(file_name);
if(!xml_file.is_open()) return;
LLSD data;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
if(LLAO::isEnabled())
LLAO::runAnims(FALSE);
gSavedPerAccountSettings.setLLSD("AO.Settings", data);
LLAO::refresh();
if(LLAO::isEnabled())
LLAO::runAnims(TRUE);
floater->refresh();
}
xml_file.close();
}
}
virtual void SetUp()
{
std::string resource_dir = ros::package::getPath("moveit_resources");
if(resource_dir == "")
{
FAIL() << "Failed to find package moveit_resources.";
return;
}
boost::filesystem::path res_path(resource_dir);
srdf_model.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
if (xml_file.is_open())
{
while (xml_file.good())
{
std::string line;
std::getline(xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model = urdf::parseURDF(xml_string);
}
srdf_model->initFile(*urdf_model, (res_path / "test/srdf/robot.xml").string());
robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
};
void load(fs_t& fs) {
// this is just some silly test code - find a random model
std::auto_ptr<techtrees_t> techtrees(new techtrees_t(fs));
const strings_t techtrees_ = techtrees->get_techtrees();
const std::string techtree_ = techtrees_[rand()%techtrees_.size()];
techtree.reset(new techtree_t(fs,techtree_));
const strings_t factions = techtree->get_factions();
const std::string faction_ = factions[rand()%factions.size()];
faction_t& faction = techtree->get_faction(faction_);
const strings_t units = fs.list_dirs(faction.path+"/units");
const std::string unit_ = units[rand()%units.size()];
const std::string unit = faction.path+"/units/"+unit_;
const std::string xml_name = unit+"/"+unit_+".xml";
const strings_t models = fs.list_files(unit+"/models");
std::string g3d;
for(strings_t::const_iterator i=models.begin(); i!=models.end(); i++)
if(i->find(".g3d") == (i->size()-4)) {
g3d = unit + "/models/" + *i;
break;
}
if(!g3d.size()) data_error("no G3D models in "<<unit<<"/models");
std::cout << "loading "<<g3d<<std::endl;
fs_file_t::ptr_t g3d_file(fs.get(g3d));
istream_t::ptr_t gstream(g3d_file->reader());
model = std::auto_ptr<model_g3d_t>(new model_g3d_t(*gstream));
// and load it
std::cout << "loading "<<xml_name<<std::endl;
fs_file_t::ptr_t xml_file(fs.get(xml_name));
istream_t::ptr_t xstream(xml_file->reader());
unit_type = std::auto_ptr<unit_type_t>(new unit_type_t(faction,unit_));
unit_type->load_xml(*xstream);
//new ui_xml_editor_t(*unit_type);
}
bool ModelClient::readURDFFromFile(std::string urdf_file){
// get the entire file
std::string xml_string;
std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
std::cout << "File ["<< urdf_file << "] parsed successfully.\n";
}
else
{
std::cout << "ERROR: Could not open file ["<< urdf_file << "] for parsing.\n";
return false;
}
// Get a urdf Model from the xml string and get all the joint names.
urdf::Model robot_model;
if (!robot_model.initString( xml_string ))
{
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
}
// Set the member variable:
urdf_xml_string_ = xml_string;
robot_name_ = robot_model.getName();
}
virtual void SetUp()
{
std::string resource_dir = ros::package::getPath("moveit_resources");
if(resource_dir == "")
{
FAIL() << "Failed to find package moveit_resources.";
return;
}
boost::filesystem::path res_path(resource_dir);
srdf_model.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model = urdf::parseURDF(xml_string);
}
srdf_model->initFile(*urdf_model, (res_path / "test/srdf/robot.xml").string());
kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
pr2_kinematics_plugin_right_arm_->setRobotModel(urdf_model);
pr2_kinematics_plugin_right_arm_->initialize("",
"right_arm",
"torso_lift_link",
"r_wrist_roll_link",
.01);
pr2_kinematics_plugin_left_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
pr2_kinematics_plugin_left_arm_->setRobotModel(urdf_model);
pr2_kinematics_plugin_left_arm_->initialize("",
"left_arm",
"torso_lift_link",
"l_wrist_roll_link",
.01);
func_right_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1)));
func_left_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1)));
std::map<std::string, robot_model::SolverAllocatorFn> allocators;
allocators["right_arm"] = *func_right_arm;
allocators["left_arm"] = *func_left_arm;
allocators["whole_body"] = *func_right_arm;
allocators["base"] = *func_left_arm;
kmodel->setKinematicsAllocators(allocators);
ps.reset(new planning_scene::PlanningScene(kmodel));
};
void Scene::load_from_file(const char* file_name)
{
TiXmlDocument xml_file(file_name);
if(xml_file.LoadFile())
{
XMLSceneVisitor visitor(this);
xml_file.Accept(&visitor);
}
}
void PhoenixViewerLink::updateClientTagsLocal()
{
std::string client_list_filename = gDirUtilp->getExpandedFilename(LL_PATH_USER_SETTINGS, "client_list.xml");
llifstream xml_file(client_list_filename);
LLSD data;
if(!xml_file.is_open()) return;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
xml_file.close();
}
}
virtual void SetUp()
{
srdf_model.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file("../kinematic_state/test/urdf/robot.xml", std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model = urdf::parseURDF(xml_string);
}
srdf_model->initFile(*urdf_model, "../kinematic_state/test/srdf/robot.xml");
kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
pr2_kinematics_plugin_right_arm_->setRobotModel(urdf_model);
pr2_kinematics_plugin_right_arm_->initialize("",
"right_arm",
"torso_lift_link",
"r_wrist_roll_link",
.01);
pr2_kinematics_plugin_left_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
pr2_kinematics_plugin_left_arm_->setRobotModel(urdf_model);
pr2_kinematics_plugin_left_arm_->initialize("",
"left_arm",
"torso_lift_link",
"l_wrist_roll_link",
.01);
func_right_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1)));
func_left_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1)));
std::map<std::string, robot_model::SolverAllocatorFn> allocators;
allocators["right_arm"] = *func_right_arm;
allocators["left_arm"] = *func_left_arm;
allocators["whole_body"] = *func_right_arm;
allocators["base"] = *func_left_arm;
kmodel->setKinematicsAllocators(allocators);
ps.reset(new planning_scene::PlanningScene(kmodel));
};
void info::read_file(const std::string &filename) throw(xml::xml_file_not_found_exception,
xml::bad_xml_format_exception,
xml::empty_xml_format_exception,
xml::missing_xml_element_exception,
xml::xml_parse_exception)
{
try
{
rapidxml::file<> xml_file(filename.c_str());
parse(xml_file.data());
} catch (const std::runtime_error &ex)
{
INTEROP_THROW(xml_file_not_found_exception, ex.what());
}
}
/**
* @function readXml
*/
std::string DartLoader::readFileToString(std::string _xmlFile) {
std::string xml_string;
std::ifstream xml_file(_xmlFile.c_str());
// Read xml
while(xml_file.good()) {
std::string line;
std::getline(xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
return xml_string;
}
void FSData::updateClientTagsLocal()
{
std::string client_list_filename = gDirUtilp->getExpandedFilename(LL_PATH_USER_SETTINGS, "client_list_v2.xml");
llifstream xml_file(client_list_filename);
LLSD data;
if(!xml_file.is_open()) return;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
if(data.has("isComplete"))
{
LegacyClientList = data;
}
xml_file.close();
}
}
// static
void LLFloaterBlacklist::loadFromSave()
{
std::string file_name = gDirUtilp->getExpandedFilename(LL_PATH_USER_SETTINGS, "blacklist_sg1.xml");
llifstream xml_file(file_name);
if(!xml_file.is_open()) return;
LLSD data;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
for(LLSD::map_iterator iter = data.beginMap(); iter != data.endMap(); ++iter)
{
blacklist_entries.insert(std::pair<LLUUID,LLSD>(LLUUID(iter->first),iter->second));
}
updateBlacklists();
}
xml_file.close();
}
int main(int argc, char** argv){
ros::init(argc, argv, "memtest");
while (ros::ok()){
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf::parseURDF(xml_string);
}
}
void PhoenixViewerLink::updateClientTagsLocal()
{
std::string client_list_filename = gDirUtilp->getExpandedFilename(LL_PATH_USER_SETTINGS, "client_list.xml");
llifstream xml_file(client_list_filename);
LLSD data;
if(!xml_file.is_open()) return;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
if(data.has("phoenixTags"))
{
phoenix_tags = data["phoenixTags"];
LLPrimitive::tagstring = PhoenixViewerLink::phoenix_tags[gSavedSettings.getString("PhoenixTagColor")].asString();
}
xml_file.close();
}
}
bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
{
m_data->m_linkColors.clear();
//int argc=0;
char relativeFileName[1024];
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
std::string xml_string;
m_data->m_pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found" << std::endl;
return false;
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
}
BulletErrorLogger loggie;
//todo: quick test to see if we can re-use the URDF parser for SDF or not
m_data->m_urdfParser.setParseSDF(true);
bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
return result;
}
void LLFloaterBlacklist::onClickLoad_continued(AIFilePicker* filepicker)
{
if (filepicker->hasFilename())
{
std::string file_name = filepicker->getFilename();
llifstream xml_file(file_name);
if(!xml_file.is_open()) return;
LLSD data;
if(LLSDSerialize::fromXML(data, xml_file) >= 1)
{
for(LLSD::map_iterator iter = data.beginMap(); iter != data.endMap(); ++iter)
{
blacklist_entries.insert(std::pair<LLUUID,LLSD>(LLUUID(iter->first),iter->second));
}
updateBlacklists();
}
xml_file.close();
}
}
virtual void SetUp()
{
std::string resource_dir = ros::package::getPath("moveit_resources");
if(resource_dir == "")
{
FAIL() << "Failed to find package moveit_resources.";
return;
}
boost::filesystem::path res_path(resource_dir);
std::string urdf_file = (res_path / "test/urdf/robot.xml").string();
std::string srdf_file = (res_path / "test/srdf/robot.xml").string();
kinect_dae_resource_ = "package://moveit_resources/test/urdf/meshes/sensors/kinect_v0/kinect.dae";
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
urdf_ok_ = urdf_model_;
}
else
{
EXPECT_EQ("FAILED TO OPEN FILE", urdf_file);
urdf_ok_ = false;
}
srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_file);
kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
crobot_.reset(new DefaultCRobotType(kmodel_));
cworld_.reset(new DefaultCWorldType());
}
void SetUp() override
{
boost::filesystem::path res_path(ros::package::getPath("moveit_resources"));
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
if (xml_file.is_open())
{
while (xml_file.good())
{
std::string line;
std::getline(xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
}
srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string());
robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_));
};
void Parser_XML::load_file(const std::string file)
{
std::ifstream xml_file(file.c_str());
if(xml_file) {
file_content.assign((std::istreambuf_iterator<char>(xml_file)),
std::istreambuf_iterator<char>());
xml_file.close();
/* We erase bad characters so that we can parse correcty the file */
erase_all_characters(file_content, '\n');
erase_all_characters(file_content, '\t');
/* Analyse the xml file and parse it at the same time */
this->tag(file_content);
} else {
std::cout << "Error : file not found" << std::endl;
}
}
bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename)
{
// get the entire file
std::string xml_string;
std::fstream xml_file(filename.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while (xml_file.good())
{
std::string line;
std::getline(xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
return initString(urdf_model, xml_string);
}
else
{
logError("Could not open file [%s] for parsing.", filename.c_str());
return false;
}
}
void MainWindow::OnLoadWorldXML(wxCommandEvent& WXUNUSED(event))
{
wxFileDialog openFile(this, wxT("Load world file XML"), wxEmptyString, wxEmptyString,
wxT("XML files (*.xml)|*.xml"),wxFD_OPEN|wxFD_FILE_MUST_EXIST);
if(openFile.ShowModal() == wxID_OK)
{
wxString file = openFile.GetPath();
char filec[500];
strcpy(filec, (const char*)file.mb_str(wxConvUTF8));
XMLfile xml_file(filec);
Object *test1 = xml_file.load(filec);
World *test2 = dynamic_cast<World *>(test1);
//xml_file.save();
if(test2)
{
simuWorld = new SimulatedWorld(test2);
listWorlds.push_back(simuWorld);
}
else
{
delete test1;
}
}
wxFileInputStream input(openFile.GetPath());
if(!input.IsOk())
{
wxLogError(wxT("Cannot open file %s"), openFile.GetPath().c_str());
return;
}
tree->Expand(tree->GetRootItem());
tree->Expand(tree->GetLastChild(tree->GetRootItem()));
Search(tree->GetLastChild(tree->GetRootItem()),toolbar->GetToolState(ID_COMPRS));
OnReplaceMenuBar();
}
/** Initialize the Model using a URDF file
* @param filename The filename of the URDF file
* @return true if the model was intialized successfully
*/
bool Model::initFile(const std::string& filename)
{
// get the entire file
std::string xml_string;
std::fstream xml_file(filename.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
return Model::initString(xml_string);
}
else
{
throw CouldNotOpenFileException(filename.c_str());
}
}
virtual void SetUp()
{
console_bridge::setLogLevel(console_bridge::LOG_INFO);
boost::filesystem::path urdf_path = moveit_resources / urdf_file;
boost::filesystem::path srdf_path = moveit_resources / srdf_file;
srdf_model_.reset(new srdf::Model());
std::string xml_string;
std::fstream xml_file(urdf_path.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf_model_ = urdf::parseURDF(xml_string);
urdf_ok_ = urdf_model_;
}
else
{
EXPECT_EQ("FAILED TO OPEN FILE", urdf_path);
urdf_ok_ = false;
}
srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_path.string());
robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
crobot_.reset(new collision_detection::CollisionRobotDistanceField(robot_model_));
cworld_.reset(new collision_detection::CollisionWorldDistanceField());
}
