ObjectReader.cpp
Go to the documentation of this file.
1 #include "ObjectReader.h"
2 
3 #include <mutex>
4 #include <optional>
5 
8 
15 #include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
17 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
21 
23 {
24  void
26  {
27  const std::string prefix = propertyPrefix;
28 
29  def->optional(p.memoryName, prefix + "MemoryName");
30  }
31 
32  void
34  {
35  // Wait for the memory to become available and add it as dependency.
36  ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
37  try
38  {
39  // simply wait until memory is ready. Do nothing with reader but get prx
40  auto r = memoryNameSystem.useReader(p.memoryName);
41 
42  // cast MemoryPrx to objPoseStoragePrx
43  this->objPoseStorage =
44  objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
45 
46  ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
47  << "'";
48  }
50  {
51  ARMARX_ERROR << e.what();
52  return;
53  }
54  }
55 
56  std::map<std::string, bool>
58  const armarx::core::time::Duration& until)
59  {
60  std::map<std::string, bool> ret;
61  auto providers = objPoseStorage->getAvailableProvidersInfo();
62  for (const auto& [k, p] : providers)
63  {
64  // TODO: check supported objects?
65  ret[k] = this->requestLocalization(instanceId, k, until);
66  }
67  return ret;
68  }
69 
70  bool
72  const std::string& provider,
73  const armarx::core::time::Duration& until)
74  {
75 
76  armarx::data::ObjectID requestObject;
77  armarx::toIce(requestObject, instanceId);
78 
79  armarx::objpose::observer::RequestObjectsInput req;
80  req.provider = provider;
81  req.request.objectIDs = {requestObject};
82  req.request.relativeTimeoutMS = until.toMilliSeconds();
83 
84  auto requestResult = objPoseStorage->requestObjects(req);
85 
86  if (requestResult.results.count(requestObject))
87  {
88  return requestResult.results.at(requestObject).result.success;
89  }
90  return false;
91  }
92 
93  std::optional<objpose::ObjectPose>
95  {
96  // TODO: Shall we throw an exception if no instance index is set?
97 
98  auto objectPoses = objPoseStorage->getObjectPoses();
99  for (const auto& pose : objectPoses)
100  {
101  ObjectID oid;
102  fromIce(pose.objectID, oid);
103  if (oid == instanceId)
104  {
106  fromIce(pose, oi);
107  return oi;
108  }
109  }
110  return std::nullopt;
111  }
112 
113  std::map<std::string, objpose::ObjectPose>
115  {
116  std::map<std::string, objpose::ObjectPose> ret;
117  auto objectPoses = objPoseStorage->getObjectPoses();
118  for (const auto& pose : objectPoses)
119  {
120  ObjectID oid;
121  fromIce(pose.objectID, oid);
122  fromIce(pose, ret[oid.str()]);
123  }
124  return ret;
125  }
126 
127  std::map<std::string, objpose::ObjectPose>
129  {
130  std::map<std::string, objpose::ObjectPose> ret;
131  auto objectPoses = objPoseStorage->getObjectPoses();
132  for (const auto& pose : objectPoses)
133  {
134  ObjectID oid;
135  fromIce(pose.objectID, oid);
136 
137  if (oid.equalClass(classId))
138  {
139  fromIce(pose, ret[oid.str()]);
140  }
141  }
142  return ret;
143  }
144 
145 } // namespace armarx::armem::obj::instance
armarx::armem::obj::instance::Reader::requestLocalization
std::map< std::string, bool > requestLocalization(const ObjectID &instanceId, const armarx::core::time::Duration &until)
Definition: ObjectReader.cpp:57
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
ice_conversions.h
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:11
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::attachment::ObjectID
armem::MemoryID ObjectID
Definition: types.h:79
armarx::armem::obj::instance::Reader::Properties::memoryName
std::string memoryName
Definition: ObjectReader.h:45
armarx::armem::obj::instance::Reader::queryLatestObjectInstance
std::optional< objpose::ObjectPose > queryLatestObjectInstance(const ObjectID &instanceId)
Definition: ObjectReader.cpp:94
armarx::armem::obj::instance::Reader::queryLatestObjectInstances
std::map< std::string, objpose::ObjectPose > queryLatestObjectInstances()
Definition: ObjectReader.cpp:114
armarx::toIce
void toIce(std::map< IceKeyT, IceValueT > &iceMap, const boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:15
error.h
armarx::armem::client::MemoryNameSystem::useReader
Reader useReader(const MemoryID &memoryID)
Use a memory server and get a reader for it.
Definition: MemoryNameSystem.cpp:184
armarx::armem::obj::instance::Reader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: ObjectReader.cpp:33
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
aron_conversions.h
armarx::armem::fromIce
void fromIce(const data::MemoryID &ice, MemoryID &id)
Definition: ice_conversions.cpp:31
memory_definitions.h
ObjectReader.h
armarx::ObjectID::equalClass
bool equalClass(const ObjectID &rhs) const
Indicates whether dataset and class name are equal.
Definition: ObjectID.cpp:70
aron_conversions.h
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::core::time::Duration
Represents a duration.
Definition: Duration.h:17
Time.h
Builder.h
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
armarx::core::time::Duration::toMilliSeconds
std::int64_t toMilliSeconds() const
Returns the amount of milliseconds.
Definition: Duration.cpp:69
util.h
Logging.h
robot_conversions.h
armarx::armem::obj::instance
Definition: ObjectReader.cpp:22
aron_conversions.h
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:36
armarx::armem::obj::instance::Reader::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: ObjectReader.cpp:25
armarx::ObjectID::str
std::string str() const
Return "dataset/className" or "dataset/className/instanceName".
Definition: ObjectID.cpp:55
PackagePath.h