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