SimoxSceneImporter.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::SimoxSceneImporter
17 * @author (vahrenkamp at kit dot edu)
18 * @date 2013
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
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
27 #include <filesystem>
28 #include <fstream>
29 
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>
34 
36 
38 
39 #include "SimoxSceneImporter.h"
40 #include <Inventor/lists/SbStringList.h>
45 
46 
47 namespace fs = std::filesystem;
48 
49 using namespace VirtualRobot;
50 
51 namespace memoryx
52 {
53  SimoxSceneImporter::SimoxSceneImporter() :
54  TEMPDIR("simoxsceneimport__tmp"), LONGTERM_SNAPSHOT_PREFIX("Snapshot_")
55  {
56  }
57 
58  void
60  {
61  usingProxy("WorkingMemory");
62  usingProxy("LongtermMemory");
63  usingProxy("PriorKnowledge");
64  usingProxy("CommonStorage");
65  }
66 
67  void
69  {
70  ARMARX_INFO << "Starting SimoxSceneImporter";
71 
72  filesDBName = getProperty<std::string>("FilesDbName").getValue();
73  sceneFile = getProperty<std::string>("SceneFile").getValue();
74  snapshotName = getProperty<std::string>("SnapshotName").getValue();
75 
76  dbSerializer = new MongoSerializer(getIceManager()->getCommunicator(), true);
77 
78  dataBasePrx = getProxy<CommonStorageInterfacePrx>("CommonStorage");
79 
80  longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
81 
82  memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
83  objectInstancesMemoryPrx =
84  memoryPrx
85  ->getObjectInstancesSegment(); // Fails if WorkingMemory.UsePriorKnowledge = false
86 
87  priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
88  classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
89 
90 
91  ScenePtr scene = loadSceneFromXML(sceneFile);
92 
93  if (!scene)
94  {
95  ARMARX_ERROR << "Error loading file " << sceneFile << " Aborting.";
96  return;
97  }
98 
99  memoryPrx->clear();
100 
101  if (!importScene(scene))
102  {
103  ARMARX_ERROR << "Error importing " << sceneFile << " Aborting.";
104  return;
105  }
106 
107  std::string prefixedSnapshotName = snapshotName;
108 
109  // check name
110  if (!simox::alg::starts_with(prefixedSnapshotName,
111  SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX))
112  {
113  prefixedSnapshotName =
114  std::string(SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX) + snapshotName;
115  ARMARX_WARNING << "Snapshot name must start with <"
116  << SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX << ">. Changing to "
117  << prefixedSnapshotName;
118  }
119 
120  if (longtermMemoryPrx->saveWorkingMemorySnapshot(prefixedSnapshotName, memoryPrx))
121  {
122  ARMARX_INFO << "Snapshot " << prefixedSnapshotName << " saved. Import complete!";
123  }
124  }
125 
127  SimoxSceneImporter::loadSceneFromXML(const std::string& fileName)
128  {
129  fs::path fpath(fileName);
130 
131  if (!fs::exists(fpath))
132  {
133  ARMARX_ERROR << "File " << fileName << " does not exist! Aborting.";
134  return ScenePtr();
135  }
136 
137  ScenePtr scene;
138 
139  try
140  {
141  scene = SceneIO::loadScene(fileName);
142  }
143  catch (VirtualRobotException& e)
144  {
145  ARMARX_ERROR << "Could not load " << fileName << "\n\t" << e.what();
146  return ScenePtr();
147  }
148 
149  return scene;
150  }
151 
152  bool
153  SimoxSceneImporter::importScene(VirtualRobot::ScenePtr scene)
154  {
155  if (!scene || !objectInstancesMemoryPrx)
156  {
157  return false;
158  }
159 
160  // create temp directory
161  std::string prefixedSnapshotName =
162  std::string(memoryx::PRIOR_COLLECTION_PREFIX) + snapshotName;
163  fs::path tmpImportDirectory =
164  fs::path(SimoxSceneImporter::TEMPDIR) / fs::path(prefixedSnapshotName);
165 
166  fs::remove_all(tmpImportDirectory);
167  fs::create_directories(tmpImportDirectory);
168 
169  // copy all scene files into the temp directory
170  VirtualRobot::ScenePtr tmpScene = createTempScene(tmpImportDirectory.string(), scene);
171 
172  if (!tmpScene)
173  {
174  return false;
175  }
176 
177  // warn about robots in the scene
178  // the tmpScene should not have any robots any more
179  size_t robotCount = scene->getRobots().size();
180 
181  if (robotCount > 0)
182  {
183  ARMARX_WARNING << "Skipping " << robotCount
184  << " robot defintions in scene file. Robots are not processed!";
185  }
186 
187  // process Obstacle and ManipulationObject instances
188  std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr> objectInstances;
189  // Obstacles
190  int objectInstanceCount =
191  createObjectInstances<ObstaclePtr>(tmpScene->getObstacles(), objectInstances);
192  // ManipulationObjects
193  objectInstanceCount += createObjectInstances<ManipulationObjectPtr>(
194  tmpScene->getManipulationObjects(), objectInstances);
195 
196  importObjectsIntoPriorKnowledge(objectInstances);
197 
198  for (auto instancePair : objectInstances)
199  {
200  objectInstancesMemoryPrx->addEntity(instancePair.second);
201  }
202 
203  ARMARX_INFO << "Added " << objectInstanceCount << " instances to working memory";
204 
205  return true;
206  }
207 
208  ScenePtr
209  SimoxSceneImporter::createTempScene(const std::string& tmpPath, VirtualRobot::ScenePtr scene)
210  {
211  if (!scene)
212  {
213  return ScenePtr();
214  }
215 
216  // store scene string for saving scene file at the end
217  std::stringstream ss;
218  ss << "<Scene name='" << scene->getName() << "'>\n\n";
219 
220  fs::path sceneFileComplete = sceneFile;
221  std::string scenePath = sceneFileComplete.parent_path().c_str();
222  fs::path sceneFileNew = tmpPath / sceneFileComplete.filename();
223 
224  // Obstacles
225  std::vector<ObstaclePtr> obstacles = scene->getObstacles();
226  // ManipulationObjects
227  std::vector<ManipulationObjectPtr> manipulationObjects = scene->getManipulationObjects();
228  obstacles.insert(obstacles.end(), manipulationObjects.begin(), manipulationObjects.end());
229 
230  // loop over all scene objects
231  for (ObstaclePtr obstacle : obstacles)
232  {
233  std::vector<std::string> absoluteVisualizationFilenames;
234 
235  // get absolute paths to the required visualization files
236  getAbsoluteVisualizationFilenames(obstacle, absoluteVisualizationFilenames);
237 
238  // create old/new file mapping
239  std::map<std::string, std::string> newFilenames;
240  copyFilesToTempDir(absoluteVisualizationFilenames, tmpPath, scenePath, newFilenames);
241 
242  // update visu and col model filenames to new locations
243  setNewVisualizationFilenames(obstacle, newFilenames);
244 
245  // set the name of the original name if it none was present
246  std::string origFilename = obstacle->getFilename();
247 
248  if (origFilename.empty())
249  {
250  // create new filename
251  origFilename += obstacle->getName();
252  origFilename += ".xml";
253  }
254 
255  // copy the objects XML file to the temp directory
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 << ")";
260 
261  fs::path absPath = fs::absolute(tmpPath);
262  saveObstacleAsManipulationObject(obstacle, localTmpFile, absPath.string());
263 
264  // add XML to the stringstream
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";
269  }
270 
271  ss << "</Scene>\n";
272 
273  // save scene file (not needed within memory since only the manipulation objects are used in workingMemory), just for testing
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);
277 
278  return SceneIO::loadScene(sceneFileNew.string());
279  }
280 
281  void
282  SimoxSceneImporter::getAbsoluteVisualizationFilenames(
283  ObstaclePtr obstacle,
284  std::vector<std::string>& absoluteFilenames)
285  {
286  if (obstacle->getVisualization())
287  {
288  SoNode* visuNode =
289  CoinVisualizationFactory::getCoinVisualization(obstacle->getVisualization());
290 
292  visuNode, absoluteFilenames, obstacle->getVisualization()->getFilename());
293  std::string visuFile = obstacle->getVisualization()->getFilename();
294 
295  if (!visuFile.empty())
296  {
297  absoluteFilenames.push_back(visuFile);
298  }
299  }
300 
301  if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
302  {
303  SoNode* colNode = CoinVisualizationFactory::getCoinVisualization(
304  obstacle->getCollisionModel()->getVisualization());
306  colNode,
307  absoluteFilenames,
308  obstacle->getCollisionModel()->getVisualization()->getFilename());
309  std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
310 
311  if (!visuFile.empty())
312  {
313  absoluteFilenames.push_back(visuFile);
314  }
315  }
316  }
317 
318  void
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)
323  {
324  fs::path tmpDir = tmpPath;
325 
326  for (size_t i = 0; i < files.size(); i++)
327  {
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);
332 
333  ARMARX_INFO << "\norigFile:" << origFile.c_str() << "\n -> relPathS:" << relativePath
334  << "\n -> tmpFile:" << tmpFile.c_str();
335 
336  if (!fs::exists(origFile))
337  {
338  ARMARX_ERROR << "File does not exist:" << origFile.c_str();
339  continue;
340  }
341 
342  if (!fs::is_regular_file(origFile))
343  {
344  ARMARX_ERROR << "Not a regular file: " << origFile.c_str();
345  continue;
346  }
347 
348  if (fs::is_directory(origFile))
349  {
350  ARMARX_ERROR << "File is a directory:" << origFile.c_str();
351  continue;
352  }
353 
354  ARMARX_LOG << "Copying file from " << origFile.c_str() << " to " << tmpFile.c_str();
355  fs::create_directories(tmpFile.parent_path());
356  // int r = system(std::string("cp \"" + origFile.string() + "\" \"" + tmpFile.string() + "\"").c_str());
357  // if (r != 0)
358  // {
359  // ARMARX_INFO << "Coyping failed";
360  // }
361  fs::copy_file(origFile, tmpFile, fs::copy_options::overwrite_existing);
362  newFilenameMapping[origFile.string()] = fs::absolute(tmpFile).string(); //relativePath;
363  }
364  }
365 
366  void
367  SimoxSceneImporter::setNewVisualizationFilenames(
368  ObstaclePtr obstacle,
369  std::map<std::string, std::string>& newFilenames)
370  {
371  if (obstacle->getVisualization())
372  {
373  std::string visuFile = obstacle->getVisualization()->getFilename();
374  std::string newVisuFile = newFilenames[visuFile];
375 
376  if (!newVisuFile.empty())
377  {
378  obstacle->getVisualization()->setFilename(
379  newVisuFile, obstacle->getVisualization()->usedBoundingBoxVisu());
380  }
381  }
382 
383  if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
384  {
385  std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
386  std::string newVisuFile = newFilenames[visuFile];
387 
388  if (!newVisuFile.empty())
389  {
390  obstacle->getCollisionModel()->getVisualization()->setFilename(
391  newVisuFile,
392  obstacle->getCollisionModel()->getVisualization()->usedBoundingBoxVisu());
393  ARMARX_INFO << " COLLISION> "
394  << obstacle->getCollisionModel()->getVisualization()->getFilename();
395  }
396  }
397  }
398 
399  bool
400  SimoxSceneImporter::saveObstacleAsManipulationObject(ObstaclePtr object,
401  const std::string& xmlFile,
402  const std::string& tmpPath)
403  {
404  THROW_VR_EXCEPTION_IF(!object, "NULL object");
405 
406  std::string xmlString = object->toXML(tmpPath);
407 
408  // This fix is needed until Simox uses virtual toXML() method
409  ManipulationObject* manip = dynamic_cast<ManipulationObject*>(object.get());
410 
411  if (manip)
412  {
413  xmlString = manip->toXML(tmpPath);
414  }
415 
416  // HACK: right now we ware only able to process manipulation objects -> convert it the hard way
417  xmlString = simox::alg::replace_all(xmlString, "<Obstacle", "<ManipulationObject");
418  xmlString = simox::alg::replace_all(xmlString, "</Obstacle", "</ManipulationObject");
419 
420  return BaseIO::writeXMLFile(xmlFile, xmlString, true);
421  }
422 
423  template <typename T>
424  int
425  SimoxSceneImporter::createObjectInstances(
426  std::vector<T> obstacles,
427  std::map<ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
428  {
429  int objectCount = 0;
430 
431  for (size_t i = 0; i < obstacles.size(); i++)
432  {
433  T obstacle = obstacles[i];
434  ObjectInstanceBasePtr obstacleInstance = createInstance(obstacle);
435 
436  if (obstacle && obstacleInstance)
437  {
438  objectInstances[obstacle] = obstacleInstance;
439  objectCount++;
440  }
441  }
442 
443  return objectCount;
444  }
445 
446  ObjectInstanceBasePtr
447  SimoxSceneImporter::createInstance(SceneObjectPtr sceneObject)
448  {
449  if (!sceneObject)
450  {
451  return ObjectInstancePtr();
452  }
453 
454  std::string objectName = sceneObject->getName();
455  ObjectInstancePtr objectInstance = new ObjectInstance(objectName);
456 
457  objectInstance->setExistenceCertainty(1.0f);
458 
459  // assume object class = object name for now
460  objectInstance->setClass(objectName, 1.0f);
461 
462  // position
463  Eigen::Vector3f position = sceneObject->getGlobalPose().block(0, 3, 3, 1);
464  armarx::FramedPositionBasePtr objectPosition =
465  new armarx::FramedPosition(position, armarx::GlobalFrame, "");
466  objectInstance->setPosition(objectPosition);
467 
468  //orientation
469  armarx::FramedOrientationBasePtr objectOrientation =
470  new armarx::FramedOrientation(sceneObject->getGlobalPose(), armarx::GlobalFrame, "");
471  objectInstance->setOrientation(objectOrientation);
472 
473  return objectInstance;
474  }
475 
476  void
477  SimoxSceneImporter::importObjectsIntoPriorKnowledge(
478  std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
479  {
480  GridFileManagerPtr fileManager(new GridFileManager(dataBasePrx));
481 
482  std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>::iterator it;
483 
484  for (it = objectInstances.begin(); it != objectInstances.end(); it++)
485  {
486  Obstacle* obstacle = it->first.get();
487 
488  ManipulationObject* manip = dynamic_cast<ManipulationObject*>(obstacle);
489 
490  if (manip)
491  {
492  obstacle = manip;
493  }
494 
495  std::string simoxFilename = obstacle->getFilename();
496 
497  fs::path visuFName = obstacle->getVisualization()->getFilename();
498  fs::path collisionFName =
499  obstacle->getCollisionModel()->getVisualization()->getFilename();
500 
501  if (collisionFName.empty())
502  {
503  collisionFName = visuFName;
504  }
505 
506  {
507  ObjectClassPtr newClass = new ObjectClass();
508  newClass->setName(obstacle->getName());
509 
511  newClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
512  ARMARX_INFO << "Store file: " << visuFName.string();
513  ARMARX_INFO << "simoxFilename: " << simoxFilename;
514  ARMARX_INFO << "visuFName.string(): " << visuFName.string();
515  ARMARX_INFO << "collisionFName.string(): " << collisionFName.string();
516  simoxWrapper->setAndStoreManipulationFile(simoxFilename, filesDBName);
517 
518  EntityBasePtr ent = classesSegmentPrx->getEntityByName(newClass->getName());
519 
520  if (ent)
521  {
522  ARMARX_IMPORTANT << "Updating existing entity " << ent->getName()
523  << "(Id: " << ent->getId() << ")";
524  classesSegmentPrx->updateEntity(ent->getId(), ent);
525  }
526  else
527  {
528  classesSegmentPrx->addEntity(newClass);
529  }
530  }
531  }
532  }
533 } // namespace memoryx
files
Use of this software is granted under one of the following two to be chosen freely by the user Boost Software License Version Marcin Kalicinski Permission is hereby free of to any person or organization obtaining a copy of the software and accompanying documentation covered by this and transmit the and to prepare derivative works of the and to permit third parties to whom the Software is furnished to do all subject to the including the above license this restriction and the following must be included in all copies of the in whole or in and all derivative works of the unless such copies or derivative works are solely in the form of machine executable object code generated by a source language processor THE SOFTWARE IS PROVIDED AS WITHOUT WARRANTY OF ANY EXPRESS OR INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF FITNESS FOR A PARTICULAR TITLE AND NON INFRINGEMENT IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER WHETHER IN TORT OR ARISING OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE The MIT Marcin Kalicinski Permission is hereby free of to any person obtaining a copy of this software and associated documentation files(the "Software")
armarx::ManagedIceObject::getIceManager
IceManagerPtr getIceManager() const
Returns the IceManager.
Definition: ManagedIceObject.cpp:366
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
JSONObject.h
VirtualRobot
Definition: FramedPose.h:42
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
GridFileManager.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
ObjectClass.h
scene3D::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: PointerDefinitions.h:36
armarx::starts_with
bool starts_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:47
memoryx::SimoxSceneImporter::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: SimoxSceneImporter.cpp:68
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:43
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
memoryx::EntityWrappers::SimoxObjectWrapperPtr
IceInternal::Handle< SimoxObjectWrapper > SimoxObjectWrapperPtr
Definition: SimoxObjectWrapper.h:300
memoryx::SimoxSceneImporter::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: SimoxSceneImporter.cpp:59
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:165
memoryx::MongoSerializer
Definition: MongoSerializer.h:40
SimoxObjectWrapper.h
ObjectInstance.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
SimoxSceneImporter.h
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
memoryx::ObjectClassPtr
IceInternal::Handle< ObjectClass > ObjectClassPtr
Definition: ObjectClass.h:35
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::ManagedIceObject::getCommunicator
Ice::CommunicatorPtr getCommunicator() const
Definition: ManagedIceObject.cpp:451
memoryx::EntityWrappers::SimoxObjectWrapper::GetAllFilenames
static void GetAllFilenames(SoNode *node, std::vector< std::string > &storeFilenames, const std::string &origFile="")
GetAllFilenames walks node and appends absolute paths of all found SoFile, SoImage,...
Definition: SimoxObjectWrapper.cpp:1010
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154