ObjectFinder.cpp
Go to the documentation of this file.
1 #include "ObjectFinder.h"
2 
3 #include <SimoxUtility/algorithm/string.h>
4 #include <SimoxUtility/filesystem/list_directory.h>
5 #include <VirtualRobot/XML/ObjectIO.h>
6 
10 
11 namespace armarx
12 {
13  namespace fs = std::filesystem;
14 
15  ObjectFinder::ObjectFinder(const std::string& objectsPackageName,
16  const ObjectFinder::path& relObjectsDir) :
17  packageName(objectsPackageName), relObjectsDir(relObjectsDir)
18  {
19  Logging::setTag("ObjectFinder");
20  }
21 
22  void
23  ObjectFinder::setPath(const std::string& path)
24  {
25  packageName = path;
26  absPackageDataDir.clear();
27  }
28 
29  std::string
31  {
32  return packageName;
33  }
34 
35  void
36  ObjectFinder::init() const
37  {
38  if (absPackageDataDir.empty())
39  {
40  CMakePackageFinder packageFinder(packageName);
41  absPackageDataDir = packageFinder.getDataDir();
42  if (absPackageDataDir.empty())
43  {
44  ARMARX_WARNING << "Could not find package '" << packageName << "'.";
45  // throw LocalException() << "Could not find package '" << packageName << "'.";
46  }
47  else
48  {
49  ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
50 
51  // make sure this data path is available => e.g. for findArticulatedObjects
52  armarx::ArmarXDataPath::addDataPaths(std::vector<std::string>{absPackageDataDir});
53  }
54  }
55  }
56 
57  bool
58  ObjectFinder::isDatasetDirValid(const path& path) const
59  {
60  return std::filesystem::is_directory(path);
61  }
62 
63  std::optional<ObjectInfo>
64  ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
65  {
66  init();
67  if (!_ready())
68  {
69  return std::nullopt;
70  }
71  if (!dataset.empty())
72  {
73  return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
74  }
75  // Search for object in datasets.
76  const std::vector<std::string>& datasets = getDatasets();
77  for (const std::string& ds : datasets)
78  {
79  if (fs::is_directory(_rootDirAbs() / ds / name))
80  {
81  return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, ds, name);
82  }
83  }
84 
85  std::stringstream ss;
86  ss << "Did not find object '" << name << "' in any of these datasets:\n";
87  for (const auto& ds : datasets)
88  {
89  ss << "- " << ds << "\n";
90  }
91  ss << "Objects root directory: " << _rootDirAbs();
92  ARMARX_VERBOSE << ss.str();
93 
94  return std::nullopt;
95  }
96 
97  std::optional<ObjectInfo>
98  ObjectFinder::findObject(const std::string& nameOrID) const
99  {
100  return findObject(ObjectID(nameOrID));
101  }
102 
103  std::optional<ObjectInfo>
105  {
106  return findObject(id.dataset(), id.className());
107  }
108 
109  std::optional<ObjectInfo>
111  {
112  return findObject(obj.objectID);
113  }
114 
115  std::vector<std::string>
117  {
118  // init(); // Done by called methods.
119  std::vector<std::string> datasets;
120  for (const path& dir : getDatasetDirectories())
121  {
122  datasets.push_back(dir.filename());
123  }
124  return datasets;
125  }
126 
127  std::vector<ObjectFinder::path>
129  {
130  init();
131  if (!_ready())
132  {
133  return {};
134  }
135  const bool local = false;
136  std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
137  std::vector<path> datasetDirs;
138  for (const path& p : dirs)
139  {
140  if (isDatasetDirValid(p))
141  {
142  datasetDirs.push_back(p);
143  }
144  }
145  return datasetDirs;
146  }
147 
148  std::vector<ObjectInfo>
149  ObjectFinder::findAllObjects(bool checkPaths) const
150  {
151  init();
152  if (!_ready())
153  {
154  return {};
155  }
156  const bool local = true;
157  std::vector<ObjectInfo> objects;
158  for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
159  {
160  if (isDatasetDirValid(_rootDirAbs() / datasetDir))
161  {
162  std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
163  for (const auto& o : dataset)
164  {
165  objects.push_back(o);
166  }
167  }
168  }
169  return objects;
170  }
171 
172  std::vector<armem::articulated_object::ArticulatedObjectDescription>
174  {
175  init();
176  if (!_ready())
177  {
178  return {};
179  }
180 
181  const bool local = true;
182 
183  std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
184  for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
185  {
186  if (isDatasetDirValid(_rootDirAbs() / datasetDir))
187  {
188  const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
189  objects.insert(objects.end(), dataset.begin(), dataset.end());
190  }
191  }
192  return objects;
193  }
194 
195  std::map<std::string, std::vector<ObjectInfo>>
197  {
198  // init(); // Done by called methods.
199  std::map<std::string, std::vector<ObjectInfo>> objects;
200  for (const std::string& dataset : getDatasets())
201  {
202  objects[dataset] = findAllObjectsOfDataset(dataset, checkPaths);
203  }
204  return objects;
205  }
206 
207  std::vector<ObjectInfo>
208  ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
209  {
210  init();
211  if (!_ready())
212  {
213  return {};
214  }
215  path datasetDir = _rootDirAbs() / dataset;
216  if (!fs::is_directory(datasetDir))
217  {
218  ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
219  << datasetDir;
220  return {};
221  }
222 
223  std::vector<ObjectInfo> objects;
224  const bool local = true;
225  for (const path& dir : simox::fs::list_directory(datasetDir, local))
226  {
227  if (fs::is_directory(datasetDir / dir))
228  {
229  ObjectInfo object(
230  packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
231  object.setLogError(logObjectDiscoveryError);
232 
233  if (!checkPaths || object.checkPaths())
234  {
235  objects.push_back(object);
236  }
237  }
238  }
239  return objects;
240  }
241 
242  std::unordered_map<std::string,
243  std::vector<armem::articulated_object::ArticulatedObjectDescription>>
245  {
246  init();
247  if (!_ready())
248  {
249  return {};
250  }
251 
252  const bool local = true;
253 
254  std::unordered_map<std::string,
255  std::vector<armem::articulated_object::ArticulatedObjectDescription>>
256  datasets;
257  for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
258  {
259  if (isDatasetDirValid(_rootDirAbs() / datasetDir))
260  {
261  const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
262  datasets[datasetDir] = dataset;
263  }
264  }
265  return datasets;
266  }
267 
268  std::vector<armem::articulated_object::ArticulatedObjectDescription>
270  bool checkPaths) const
271  {
272  init();
273  if (!_ready())
274  {
275  return {};
276  }
277  path datasetDir = _rootDirAbs() / dataset;
278  if (!isDatasetDirValid(datasetDir))
279  {
280  ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
281  << datasetDir;
282  return {};
283  }
284 
285  std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
286  const bool local = true;
287  for (const path& dir : simox::fs::list_directory(datasetDir, local))
288  {
289  if (fs::is_directory(datasetDir / dir))
290  {
291  ObjectInfo object(
292  packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
293  std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
294  if (modelFile.has_value())
295  {
297  .name = object.idStr(),
298  .xml = {modelFile->package, modelFile->relativePath},
299  .visualization = {},
300  .info = {}
301  // .dataset = dataset
302  });
303  }
304  }
305  }
306  return objects;
307  }
308 
309  VirtualRobot::ManipulationObjectPtr
310  ObjectFinder::loadManipulationObject(const std::optional<ObjectInfo>& ts)
311  {
312  if (!ts)
313  {
314  return nullptr;
315  }
316  const auto data = ts->simoxXML();
318  std::string abs;
319  if (!ArmarXDataPath::SearchReadableFile(data.relativePath, abs))
320  {
321  return nullptr;
322  }
323  return VirtualRobot::ObjectIO::loadManipulationObject(abs);
324  }
325 
326  VirtualRobot::ManipulationObjectPtr
328  {
329  return loadManipulationObject(findObject(obj));
330  }
331 
332  VirtualRobot::ObstaclePtr
333  ObjectFinder::loadObstacle(const std::optional<ObjectInfo>& ts)
334  {
335  if (!ts)
336  {
337  return nullptr;
338  }
339  const auto data = ts->simoxXML();
341  std::string abs;
342  if (!ArmarXDataPath::SearchReadableFile(data.relativePath, abs))
343  {
344  return nullptr;
345  }
346  return VirtualRobot::ObjectIO::loadObstacle(abs);
347  }
348 
349  VirtualRobot::ObstaclePtr
351  {
352  return loadObstacle(findObject(obj));
353  }
354 
355  static std::vector<std::string>
356  _loadNames(
357  const ObjectFinder& finder,
358  const ObjectID& objectID,
359  const bool includeClassName,
360  const std::function<std::optional<std::vector<std::string>>(const ObjectInfo&)> loadNamesFn)
361  {
362  std::vector<std::string> names;
363  if (includeClassName)
364  {
365  names.push_back(simox::alg::to_lower(objectID.className()));
366  }
367  if (std::optional<ObjectInfo> info = finder.findObject(objectID))
368  {
369  if (std::optional<std::vector<std::string>> loadedNames = loadNamesFn(*info))
370  {
371  // Source: https://stackoverflow.com/a/201729
372  names.insert(names.end(), loadedNames->begin(), loadedNames->end());
373  }
374  }
375  return names;
376  }
377 
378  std::vector<std::string>
379  ObjectFinder::loadRecognizedNames(const ObjectID& objectID, bool includeClassName) const
380  {
381  return _loadNames(*this,
382  objectID,
383  includeClassName,
384  [](const ObjectInfo& info) { return info.loadRecognizedNames(); });
385  }
386 
387  std::vector<std::string>
388  ObjectFinder::loadSpokenNames(const ObjectID& objectID, bool includeClassName) const
389  {
390  return _loadNames(*this,
391  objectID,
392  includeClassName,
393  [](const ObjectInfo& info) { return info.loadSpokenNames(); });
394  }
395 
396  void
398  {
399  logObjectDiscoveryError = logEnabled;
400  }
401 
403  ObjectFinder::_rootDirAbs() const
404  {
405  return absPackageDataDir / packageName / relObjectsDir;
406  }
407 
409  ObjectFinder::_rootDirRel() const
410  {
411  return packageName;
412  }
413 
414  bool
415  ObjectFinder::_ready() const
416  {
417  return !absPackageDataDir.empty();
418  }
419 
420 } // namespace armarx
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::ObjectFinder::findAllArticulatedObjectsOfDataset
std::vector< armem::articulated_object::ArticulatedObjectDescription > findAllArticulatedObjectsOfDataset(const std::string &dataset, bool checkPaths) const
Definition: ObjectFinder.cpp:269
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:10
armarx::ObjectFinder::loadObstacle
static VirtualRobot::ObstaclePtr loadObstacle(const std::optional< ObjectInfo > &ts)
Definition: ObjectFinder.cpp:333
armarx::armem::robot_state::description::RobotDescription
Definition: types.h:44
armarx::ObjectFinder::findAllObjects
std::vector< ObjectInfo > findAllObjects(bool checkPaths=true) const
Definition: ObjectFinder.cpp:149
armarx::armem::attachment::ObjectID
armem::MemoryID ObjectID
Definition: types.h:79
armarx::ArmarXDataPath::FindPackageAndAddDataPath
static bool FindPackageAndAddDataPath(const std::string &packageName)
Search for the package and add its data path if it was found.
Definition: ArmarXDataPath.cpp:824
armarx::ObjectFinder::ObjectFinder
ObjectFinder(const std::string &objectsPackageName=DefaultObjectsPackageName, const path &relObjectsDir=DefaultObjectsDirectory)
Definition: ObjectFinder.cpp:15
armarx::ObjectFinder::path
std::filesystem::path path
Definition: ObjectFinder.h:25
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::ObjectFinder::loadManipulationObject
static VirtualRobot::ManipulationObjectPtr loadManipulationObject(const std::optional< ObjectInfo > &ts)
Definition: ObjectFinder.cpp:310
armarx::ObjectFinder::setLogObjectDiscoveryError
void setLogObjectDiscoveryError(bool logEnabled)
Definition: ObjectFinder.cpp:397
armarx::ObjectInfo::loadRecognizedNames
std::optional< std::vector< std::string > > loadRecognizedNames() const
Load names to use when matched when recognizing an object by name.
Definition: ObjectInfo.cpp:326
armarx::ObjectFinder
Used to find objects in the ArmarX objects repository [1] (formerly [2]).
Definition: ObjectFinder.h:22
armarx::ObjectFinder::setPath
void setPath(const std::string &path)
Definition: ObjectFinder.cpp:23
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
armarx::ObjectID::className
std::string className() const
Definition: ObjectID.h:30
armarx::ObjectFinder::findAllObjectsOfDataset
std::vector< ObjectInfo > findAllObjectsOfDataset(const std::string &dataset, bool checkPaths=true) const
Definition: ObjectFinder.cpp:208
armarx::ObjectFinder::findAllArticulatedObjectsByDataset
std::unordered_map< std::string, std::vector< armem::articulated_object::ArticulatedObjectDescription > > findAllArticulatedObjectsByDataset(bool checkPaths=true) const
Definition: ObjectFinder.cpp:244
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::ObjectFinder::loadSpokenNames
std::vector< std::string > loadSpokenNames(const ObjectID &objectID, bool includeClassName=false) const
Load names to use when verbalizing an object name.
Definition: ObjectFinder.cpp:388
armarx::ObjectFinder::loadRecognizedNames
std::vector< std::string > loadRecognizedNames(const ObjectID &objectID, bool includeClassName=false) const
Load names to use when matched when recognizing an object by name.
Definition: ObjectFinder.cpp:379
armarx::ObjectFinder::findAllObjectsByDataset
std::map< std::string, std::vector< ObjectInfo > > findAllObjectsByDataset(bool checkPaths=true) const
Definition: ObjectFinder.cpp:196
ExpressionException.h
armarx::ArmarXDataPath::SearchReadableFile
static bool SearchReadableFile(const std::string &querryFileName, std::string &resultFileName, bool verbose=true)
Definition: ArmarXDataPath.cpp:216
CMakePackageFinder.h
armarx::ObjectFinder::getDatasetDirectories
std::vector< path > getDatasetDirectories() const
Definition: ObjectFinder.cpp:128
armarx::objpose::ObjectPose::objectID
armarx::ObjectID objectID
The object ID, i.e. dataset, class name and instance name.
Definition: ObjectPose.h:56
armarx::armem::robot_state::description::RobotDescription::name
std::string name
Definition: types.h:48
armarx::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:13
armarx::ArmarXDataPath::addDataPaths
static void addDataPaths(const std::string &dataPathList)
Definition: ArmarXDataPath.cpp:554
armarx::ObjectInfo::loadSpokenNames
std::optional< std::vector< std::string > > loadSpokenNames() const
Load names to use when verbalizing an object name.
Definition: ObjectInfo.cpp:332
armarx::ObjectFinder::findObject
std::optional< ObjectInfo > findObject(const std::string &dataset, const std::string &name) const
Definition: ObjectFinder.cpp:64
armarx::ObjectFinder::findAllArticulatedObjects
std::vector< armem::articulated_object::ArticulatedObjectDescription > findAllArticulatedObjects(bool checkPaths) const
Definition: ObjectFinder.cpp:173
armarx::ObjectFinder::getDatasets
std::vector< std::string > getDatasets() const
Definition: ObjectFinder.cpp:116
armarx::Logging::setTag
void setTag(const LogTag &tag)
Definition: Logging.cpp:54
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::ObjectFinder::getPackageName
std::string getPackageName() const
Definition: ObjectFinder.cpp:30
ArmarXDataPath.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:33
armarx::ObjectInfo
Accessor for the object files.
Definition: ObjectInfo.h:36
ObjectFinder.h