22 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
23 #include <VirtualRobot/Scene.h>
24 #include <VirtualRobot/VirtualRobotException.h>
25 #define BOOST_NO_SCOPED_ENUMS
26 #define BOOST_NO_CXX11_SCOPED_ENUMS
30 #include <SimoxUtility/algorithm/string/string_tools.h>
31 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
32 #include <VirtualRobot/Visualization/VisualizationNode.h>
33 #include <VirtualRobot/XML/BaseIO.h>
40 #include <Inventor/lists/SbStringList.h>
47 namespace fs = std::filesystem;
53 SimoxSceneImporter::SimoxSceneImporter() :
54 TEMPDIR(
"simoxsceneimport__tmp"), LONGTERM_SNAPSHOT_PREFIX(
"Snapshot_")
72 filesDBName = getProperty<std::string>(
"FilesDbName").getValue();
73 sceneFile = getProperty<std::string>(
"SceneFile").getValue();
74 snapshotName = getProperty<std::string>(
"SnapshotName").getValue();
78 dataBasePrx = getProxy<CommonStorageInterfacePrx>(
"CommonStorage");
80 longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(
"LongtermMemory");
82 memoryPrx = getProxy<WorkingMemoryInterfacePrx>(
"WorkingMemory");
83 objectInstancesMemoryPrx =
85 ->getObjectInstancesSegment();
87 priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(
"PriorKnowledge");
88 classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
91 ScenePtr scene = loadSceneFromXML(sceneFile);
95 ARMARX_ERROR <<
"Error loading file " << sceneFile <<
" Aborting.";
101 if (!importScene(scene))
103 ARMARX_ERROR <<
"Error importing " << sceneFile <<
" Aborting.";
107 std::string prefixedSnapshotName = snapshotName;
111 SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX))
113 prefixedSnapshotName =
114 std::string(SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX) + snapshotName;
116 << SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX <<
">. Changing to "
117 << prefixedSnapshotName;
120 if (longtermMemoryPrx->saveWorkingMemorySnapshot(prefixedSnapshotName, memoryPrx))
122 ARMARX_INFO <<
"Snapshot " << prefixedSnapshotName <<
" saved. Import complete!";
127 SimoxSceneImporter::loadSceneFromXML(
const std::string& fileName)
129 fs::path fpath(fileName);
131 if (!fs::exists(fpath))
133 ARMARX_ERROR <<
"File " << fileName <<
" does not exist! Aborting.";
141 scene = SceneIO::loadScene(fileName);
143 catch (VirtualRobotException& e)
145 ARMARX_ERROR <<
"Could not load " << fileName <<
"\n\t" << e.what();
155 if (!scene || !objectInstancesMemoryPrx)
161 std::string prefixedSnapshotName =
162 std::string(memoryx::PRIOR_COLLECTION_PREFIX) + snapshotName;
163 fs::path tmpImportDirectory =
164 fs::path(SimoxSceneImporter::TEMPDIR) / fs::path(prefixedSnapshotName);
166 fs::remove_all(tmpImportDirectory);
167 fs::create_directories(tmpImportDirectory);
179 size_t robotCount = scene->getRobots().size();
184 <<
" robot defintions in scene file. Robots are not processed!";
188 std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr> objectInstances;
190 int objectInstanceCount =
191 createObjectInstances<ObstaclePtr>(tmpScene->getObstacles(), objectInstances);
193 objectInstanceCount += createObjectInstances<ManipulationObjectPtr>(
194 tmpScene->getManipulationObjects(), objectInstances);
196 importObjectsIntoPriorKnowledge(objectInstances);
198 for (
auto instancePair : objectInstances)
200 objectInstancesMemoryPrx->addEntity(instancePair.second);
203 ARMARX_INFO <<
"Added " << objectInstanceCount <<
" instances to working memory";
217 std::stringstream ss;
218 ss <<
"<Scene name='" << scene->getName() <<
"'>\n\n";
220 fs::path sceneFileComplete = sceneFile;
221 std::string scenePath = sceneFileComplete.parent_path().c_str();
222 fs::path sceneFileNew = tmpPath / sceneFileComplete.filename();
225 std::vector<ObstaclePtr> obstacles = scene->getObstacles();
227 std::vector<ManipulationObjectPtr> manipulationObjects = scene->getManipulationObjects();
228 obstacles.insert(obstacles.end(), manipulationObjects.begin(), manipulationObjects.end());
231 for (ObstaclePtr obstacle : obstacles)
233 std::vector<std::string> absoluteVisualizationFilenames;
236 getAbsoluteVisualizationFilenames(obstacle, absoluteVisualizationFilenames);
239 std::map<std::string, std::string> newFilenames;
240 copyFilesToTempDir(absoluteVisualizationFilenames, tmpPath, scenePath, newFilenames);
243 setNewVisualizationFilenames(obstacle, newFilenames);
246 std::string origFilename = obstacle->getFilename();
248 if (origFilename.empty())
251 origFilename += obstacle->getName();
252 origFilename +=
".xml";
256 fs::path localFile = tmpPath / fs::path(origFilename).filename();
257 std::string localTmpFile = localFile.string();
258 ARMARX_INFO <<
"Saving modified ManipulationObject to " << localTmpFile
259 <<
" (original file: " << scenePath <<
"/" << origFilename <<
")";
261 fs::path absPath = fs::absolute(tmpPath);
262 saveObstacleAsManipulationObject(obstacle, localTmpFile, absPath.string());
265 ss <<
"\t<ManipulationObject name='" << obstacle->getName() <<
"'>\n";
266 ss <<
"\t\t<File>" << localFile.filename().string() <<
"</File>\n";
267 ss << obstacle->getPhysics().toXML(2);
268 ss <<
"\t</ManipulationObject>\n\n";
274 ARMARX_LOG <<
"Saving scenefile from " << sceneFile <<
" to " << sceneFileNew.c_str();
275 std::string sceneString = ss.str();
276 BaseIO::writeXMLFile(sceneFileNew.c_str(), sceneString,
true);
278 return SceneIO::loadScene(sceneFileNew.string());
282 SimoxSceneImporter::getAbsoluteVisualizationFilenames(
283 ObstaclePtr obstacle,
284 std::vector<std::string>& absoluteFilenames)
286 if (obstacle->getVisualization())
289 CoinVisualizationFactory::getCoinVisualization(obstacle->getVisualization());
292 visuNode, absoluteFilenames, obstacle->getVisualization()->getFilename());
293 std::string visuFile = obstacle->getVisualization()->getFilename();
295 if (!visuFile.empty())
297 absoluteFilenames.push_back(visuFile);
301 if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
303 SoNode* colNode = CoinVisualizationFactory::getCoinVisualization(
304 obstacle->getCollisionModel()->getVisualization());
308 obstacle->getCollisionModel()->getVisualization()->getFilename());
309 std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
311 if (!visuFile.empty())
313 absoluteFilenames.push_back(visuFile);
319 SimoxSceneImporter::copyFilesToTempDir(
const std::vector<std::string>&
files,
320 const std::string& tmpPath,
321 const std::string& origPath,
322 std::map<std::string, std::string>& newFilenameMapping)
324 fs::path tmpDir = tmpPath;
326 for (
size_t i = 0; i <
files.size(); i++)
328 fs::path origFile =
files[i];
329 std::string relativePath = fs::absolute(
files[i]).string();
330 VirtualRobot::BaseIO::makeRelativePath(fs::absolute(origPath).
string(), relativePath);
331 fs::path tmpFile = tmpDir / fs::path(relativePath);
333 ARMARX_INFO <<
"\norigFile:" << origFile.c_str() <<
"\n -> relPathS:" << relativePath
334 <<
"\n -> tmpFile:" << tmpFile.c_str();
336 if (!fs::exists(origFile))
338 ARMARX_ERROR <<
"File does not exist:" << origFile.c_str();
342 if (!fs::is_regular_file(origFile))
344 ARMARX_ERROR <<
"Not a regular file: " << origFile.c_str();
348 if (fs::is_directory(origFile))
350 ARMARX_ERROR <<
"File is a directory:" << origFile.c_str();
354 ARMARX_LOG <<
"Copying file from " << origFile.c_str() <<
" to " << tmpFile.c_str();
355 fs::create_directories(tmpFile.parent_path());
361 fs::copy_file(origFile, tmpFile, fs::copy_options::overwrite_existing);
362 newFilenameMapping[origFile.string()] = fs::absolute(tmpFile).string();
367 SimoxSceneImporter::setNewVisualizationFilenames(
368 ObstaclePtr obstacle,
369 std::map<std::string, std::string>& newFilenames)
371 if (obstacle->getVisualization())
373 std::string visuFile = obstacle->getVisualization()->getFilename();
374 std::string newVisuFile = newFilenames[visuFile];
376 if (!newVisuFile.empty())
378 obstacle->getVisualization()->setFilename(
379 newVisuFile, obstacle->getVisualization()->usedBoundingBoxVisu());
383 if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
385 std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
386 std::string newVisuFile = newFilenames[visuFile];
388 if (!newVisuFile.empty())
390 obstacle->getCollisionModel()->getVisualization()->setFilename(
392 obstacle->getCollisionModel()->getVisualization()->usedBoundingBoxVisu());
394 << obstacle->getCollisionModel()->getVisualization()->getFilename();
400 SimoxSceneImporter::saveObstacleAsManipulationObject(ObstaclePtr
object,
401 const std::string& xmlFile,
402 const std::string& tmpPath)
404 THROW_VR_EXCEPTION_IF(!
object,
"NULL object");
406 std::string xmlString =
object->toXML(tmpPath);
409 ManipulationObject* manip =
dynamic_cast<ManipulationObject*
>(
object.get());
413 xmlString = manip->toXML(tmpPath);
417 xmlString = simox::alg::replace_all(xmlString,
"<Obstacle",
"<ManipulationObject");
418 xmlString = simox::alg::replace_all(xmlString,
"</Obstacle",
"</ManipulationObject");
420 return BaseIO::writeXMLFile(xmlFile, xmlString,
true);
423 template <
typename T>
425 SimoxSceneImporter::createObjectInstances(
426 std::vector<T> obstacles,
427 std::map<ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
431 for (
size_t i = 0; i < obstacles.size(); i++)
433 T obstacle = obstacles[i];
434 ObjectInstanceBasePtr obstacleInstance = createInstance(obstacle);
436 if (obstacle && obstacleInstance)
438 objectInstances[obstacle] = obstacleInstance;
446 ObjectInstanceBasePtr
454 std::string objectName = sceneObject->getName();
457 objectInstance->setExistenceCertainty(1.0f);
460 objectInstance->setClass(objectName, 1.0f);
463 Eigen::Vector3f position = sceneObject->getGlobalPose().block(0, 3, 3, 1);
464 armarx::FramedPositionBasePtr objectPosition =
466 objectInstance->setPosition(objectPosition);
469 armarx::FramedOrientationBasePtr objectOrientation =
471 objectInstance->setOrientation(objectOrientation);
473 return objectInstance;
477 SimoxSceneImporter::importObjectsIntoPriorKnowledge(
478 std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
482 std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>::iterator it;
484 for (it = objectInstances.begin(); it != objectInstances.end(); it++)
486 Obstacle* obstacle = it->first.get();
488 ManipulationObject* manip =
dynamic_cast<ManipulationObject*
>(obstacle);
495 std::string simoxFilename = obstacle->getFilename();
497 fs::path visuFName = obstacle->getVisualization()->getFilename();
498 fs::path collisionFName =
499 obstacle->getCollisionModel()->getVisualization()->getFilename();
501 if (collisionFName.empty())
503 collisionFName = visuFName;
508 newClass->setName(obstacle->getName());
511 newClass->addWrapper(
new EntityWrappers::SimoxObjectWrapper(fileManager));
512 ARMARX_INFO <<
"Store file: " << visuFName.string();
514 ARMARX_INFO <<
"visuFName.string(): " << visuFName.string();
515 ARMARX_INFO <<
"collisionFName.string(): " << collisionFName.string();
516 simoxWrapper->setAndStoreManipulationFile(simoxFilename, filesDBName);
518 EntityBasePtr ent = classesSegmentPrx->getEntityByName(newClass->getName());
523 <<
"(Id: " << ent->getId() <<
")";
524 classesSegmentPrx->updateEntity(ent->getId(), ent);
528 classesSegmentPrx->addEntity(newClass);