ArmarXObjectsToMemory.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::ArmarXObjects::ArmarXObjectsImporter
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 
24 #include "ArmarXObjectsToMemory.h"
25 
26 #include <unordered_set>
27 
28 #include <SimoxUtility/json.h>
29 
36 
41 
45 
46 
47 namespace fs = std::filesystem;
48 
49 namespace memoryx
50 {
51 
53  {
54  }
55 
56  void
58  const std::vector<armarx::ObjectInfo>& objectInfos,
59  const GridFileManagerPtr& gridFileManager,
60  const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
61  const std::string& mongoDbName) const
62  {
63  int importCount = 0;
64 
65  for (const armarx::ObjectInfo& objectInfo : objectInfos)
66  {
67  const fs::path xmlFile = objectInfo.simoxXML().absolutePath;
68 
69  if (fs::is_regular_file(xmlFile))
70  {
71  const std::string className = objectInfo.idStr();
72 
73  ARMARX_INFO << "Importing " << objectInfo.id() << " with Simox XML " << xmlFile;
74 
76  newClass->setName(className);
77 
78  ARMARX_INFO << "\tStoring Simox XML file: " << xmlFile;
79  if (!dryRun)
80  {
82  newClass->addWrapper(
83  new memoryx::EntityWrappers::SimoxObjectWrapper(gridFileManager));
84  simoxWrapper->setAndStoreManipulationFile(xmlFile.string(), mongoDbName);
85  }
86 
87  memoryx::EntityBasePtr oldClass =
88  objectClassesSegment->getEntityByName(newClass->getName());
89  if (oldClass)
90  {
91  ARMARX_INFO << "\tUpdating existing entity " << oldClass->getName()
92  << "(ID: " << oldClass->getId() << ")";
93  if (!dryRun)
94  {
95  objectClassesSegment->updateEntity(oldClass->getId(), newClass);
96  }
97  }
98  else
99  {
100  ARMARX_INFO << "\tCreating new entity " << newClass->getName();
101  if (!dryRun)
102  {
103  objectClassesSegment->addEntity(newClass);
104  }
105  }
106 
107  importCount++;
108  }
109  }
110 
111  ARMARX_IMPORTANT << "Imported " << importCount << " object classes.";
112  }
113 
114  void
116  const std::filesystem::path& scenesDirectory,
117  const LongtermMemoryInterfacePrx& longtermMemory,
118  const WorkingMemoryInterfacePrx& workingMemory,
119  const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
120  const std::unordered_set<std::string>& scenes) const
121  {
122  ARMARX_TRACE;
123 
124  ARMARX_CHECK_NOT_NULL(longtermMemory);
125  ARMARX_CHECK_NOT_NULL(workingMemory);
126  ARMARX_CHECK_NOT_NULL(objectClassesSegment);
127 
128  memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesSegment =
129  workingMemory->getObjectInstancesSegment();
130  ARMARX_CHECK_NOT_NULL(objectInstancesSegment);
131 
132  int fileCount = 0;
133  fs::recursive_directory_iterator end_iter;
134  for (fs::recursive_directory_iterator dir_iter(scenesDirectory); dir_iter != end_iter;
135  ++dir_iter)
136  {
137  if (fs::is_regular_file(dir_iter->status()) &&
138  (dir_iter->path().extension() == ".json"))
139  {
140  const std::filesystem::path sceneJsonFile = dir_iter->path();
141  const std::string snapshotName = sceneJsonFile.stem();
142 
143  // Check enable-list (if set).
144  if (not scenes.empty() and scenes.count(snapshotName) == 0)
145  {
146  ARMARX_INFO << "Skipping snapshot " << snapshotName
147  << " as it has not been selected";
148  }
149  else
150  {
151  ARMARX_INFO << "Found snapshot: " << snapshotName;
152 
153  importSceneAsSnapshot(snapshotName,
154  sceneJsonFile,
155  longtermMemory,
156  workingMemory,
157  objectClassesSegment,
158  objectInstancesSegment);
159 
160  fileCount++;
161  }
162  }
163  }
164  ARMARX_IMPORTANT << "Found " << fileCount << " valid *.json files in the scenes directory '"
165  << scenesDirectory << "'";
166  }
167 
168  bool
170  const std::string& snapshotName,
171  const std::filesystem::path& jsonFile,
172  const LongtermMemoryInterfacePrx& longtermMemory,
173  const WorkingMemoryInterfacePrx& workingMemory,
174  const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
175  const memoryx::ObjectInstanceMemorySegmentBasePrx& objectInstancesSegment) const
176  {
177  ARMARX_TRACE;
178 
179  if (!dryRun)
180  {
181  ARMARX_TRACE;
182  workingMemory->clear();
183  }
184 
185  const nlohmann::json j = nlohmann::read_json(jsonFile);
187  scene = j;
188 
189  std::map<std::string, std::unordered_set<std::string>> instancesPerClass;
190  try
191  {
192  for (const auto& object : scene.objects)
193  {
195  object, objectClassesSegment, objectInstancesSegment, instancesPerClass);
196  }
197 
198  ARMARX_INFO << "Saving snapshot from working memory to longterm memory: "
199  << "'" << snapshotName << "'";
200  if (!dryRun)
201  {
202  ARMARX_TRACE;
203  longtermMemory->saveWorkingMemorySnapshot(snapshotName, workingMemory);
204  }
205 
206  return true;
207  }
208  catch (...)
209  {
211  ARMARX_WARNING << "Skipping scene " << snapshotName;
212  return false;
213  }
214  }
215 
216  bool
218  const armarx::objects::SceneObject& object,
219  const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
220  const ObjectInstanceMemorySegmentBasePrx& objectInstancesSegment,
221  std::map<std::string, std::unordered_set<std::string>>& instancesPerClass) const
222  {
223  ARMARX_TRACE;
224 
225  const std::string& className = object.className;
226  const Eigen::Vector3f& position = object.position;
227  const Eigen::Quaternionf& quat = object.orientation;
228 
229  // Get the object class
230  const auto obj = objectClassesSegment->getObjectClassByName(className);
231  if (armarx::isNullptr(obj))
232  {
233  ARMARX_WARNING << "Object class " << object.className << " for object "
234  << object.getObjectID() << " not found in class segment.";
235  return false;
236  }
237 
238  memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(obj);
239  ARMARX_CHECK_NOT_NULL(objectClass);
240 
241  // Add object instance
242  {
243  auto& instancesOfThisClass = instancesPerClass[className];
244 
245  auto getObjectName = [&]() -> std::string
246  {
247  // Instance name specified
248  if (not object.instanceName.empty())
249  {
250  if (instancesOfThisClass.count(object.instanceName) > 0)
251  {
252  ARMARX_WARNING << "Instance name " << object.instanceName
253  << " already in use.";
254  return "";
255  }
256  return object.instanceName;
257  }
258 
259  // No instance name specified => use template
260  for (int i = 0;; i++)
261  {
262  std::string objectName = className + "_" + std::to_string(i);
263  if (instancesOfThisClass.count(objectName) == 0)
264  {
265  return objectName;
266  }
267  }
268 
269  return "";
270  };
271 
272  const std::string objectName = getObjectName();
273  if (objectName.empty())
274  {
275  throw armarx::LocalException("No object name could be set!");
276  }
277 
278  instancesOfThisClass.insert(objectName);
279 
280  memoryx::ObjectInstancePtr newObject = new memoryx::ObjectInstance(objectName);
281 
282  NameList attributeNames = objectClass->getAttributeNames();
283  for (auto it = attributeNames.begin(); it != attributeNames.end(); ++it)
284  {
285  newObject->putAttribute(objectClass->getAttribute(*it));
286  }
287 
288  newObject->setClass(objectClass->getName(), 1.0f);
289  newObject->setExistenceCertainty(1.0f);
290 
291  armarx::FramedPositionPtr framedPosition =
292  new armarx::FramedPosition(position, armarx::GlobalFrame, "");
293  newObject->setPosition(framedPosition);
294 
295  armarx::FramedOrientationPtr newOrient =
296  new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
297  newObject->setOrientation(newOrient);
298 
299  std::string objectID = objectInstancesSegment->addEntity(newObject);
300  newObject->setId(objectID);
301 
302  ARMARX_INFO << "Added object of class '" << className << "' with ID '" << objectID
303  << "'.";
304 
305  return true;
306  }
307  }
308 
309 } // namespace memoryx
Pointer.h
armarx::isNullptr
bool isNullptr(const T &p)
Definition: Pointer.h:60
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
LocalException.h
armarx::objects::Scene::objects
std::vector< SceneObject > objects
Definition: Scene.h:57
memoryx::ArmarXObjectsToMemory::dryRun
bool dryRun
If true, don't actually change anything.
Definition: ArmarXObjectsToMemory.h:84
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
memoryx::ArmarXObjectsToMemory::importSceneAsSnapshot
bool importSceneAsSnapshot(const std::string &snapshotName, std::filesystem::path const &sceneJsonFile, memoryx::LongtermMemoryInterfacePrx const &longtermMemory, memoryx::WorkingMemoryInterfacePrx const &workingMemory, memoryx::PersistentObjectClassSegmentBasePrx const &objectClassesSegment, memoryx::ObjectInstanceMemorySegmentBasePrx const &objectInstancesSegment) const
Definition: ArmarXObjectsToMemory.cpp:169
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
trace.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
memoryx::ObjectInstance
Definition: ObjectInstance.h:48
ObjectClass.h
ArmarXObjectsToMemory.h
Scene.h
armarx::objects::SceneObject
Definition: Scene.h:37
IceInternal::Handle< ObjectClass >
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:165
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
json_conversions.h
FramedPose.h
memoryx::ObjectClass
Definition: ObjectClass.h:37
ObjectInfo.h
memoryx::ArmarXObjectsToMemory::importObjectToWorkingMemory
bool importObjectToWorkingMemory(const armarx::objects::SceneObject &object, memoryx::PersistentObjectClassSegmentBasePrx const &objectClassesSegment, memoryx::ObjectInstanceMemorySegmentBasePrx const &objectInstancesSegment, std::map< std::string, std::unordered_set< std::string >> &instancesPerClass) const
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::objects::Scene
Definition: Scene.h:55
ExpressionException.h
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
armarx::Quaternion< float, 0 >
memoryx::ArmarXObjectsToMemory::importObjectsToPriorKnowledge
void importObjectsToPriorKnowledge(std::vector< armarx::ObjectInfo > const &objectInfos, memoryx::GridFileManagerPtr const &gridFileManager, memoryx::PersistentObjectClassSegmentBasePrx const &objectClassesSegment, std::string const &mongoDbName) const
Definition: ArmarXObjectsToMemory.cpp:57
memoryx::ArmarXObjectsToMemory::ArmarXObjectsToMemory
ArmarXObjectsToMemory()
Definition: ArmarXObjectsToMemory.cpp:52
Logging.h
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
ArmarXDataPath.h
memoryx::ArmarXObjectsToMemory::importScenesAsSnapshots
void importScenesAsSnapshots(std::filesystem::path const &scenesDirectory, memoryx::LongtermMemoryInterfacePrx const &longtermMemory, memoryx::WorkingMemoryInterfacePrx const &workingMemory, memoryx::PersistentObjectClassSegmentBasePrx const &objectClassesSegment, const std::unordered_set< std::string > &scenes) const
Definition: ArmarXObjectsToMemory.cpp:115
armarx::ObjectInfo
Accessor for the object files.
Definition: ObjectInfo.h:36