GraspCandidateReader.cpp
Go to the documentation of this file.
1 #include "GraspCandidateReader.h"
2 
4 
5 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
10 
11 namespace armarx::armem
12 {
14  {
15  }
16 
17  void
19  {
20  // Wait for the memory to become available and add it as dependency.
21  ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
22  << "' ...";
23  try
24  {
25  memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
26  : memoryNameSystem.getReader(MemoryID(properties.memoryName));
27  ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
28  << properties.memoryName;
29  }
31  {
32  ARMARX_ERROR << e.what();
33  return;
34  }
35  }
36 
37  armarx::grasping::GraspCandidate
39  {
40  armarx::grasping::GraspCandidate candidate;
41 
42  grasping::arondto::GraspCandidate aronTransform;
43 
44  aronTransform.fromAron(instance.data());
45 
46  fromAron(aronTransform, candidate);
47 
48  return candidate;
49  }
50 
51  armarx::grasping::BimanualGraspCandidate
53  {
54  armarx::grasping::BimanualGraspCandidate candidate;
55 
56  grasping::arondto::BimanualGraspCandidate aronTransform;
57  aronTransform.fromAron(instance.data());
58 
59  fromAron(aronTransform, candidate);
60 
61  return candidate;
62  }
63 
64  grasping::GraspCandidatePtr
66  {
67  auto dict = queryGraspCandidateInstancesByID({id});
68  if (auto it = dict.find(id.str()); it != dict.end())
69  {
70  return it->second;
71  }
72  else
73  {
74  return nullptr;
75  }
76  }
77 
78  grasping::GraspCandidateDict
79  GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
80  {
82 
83  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
84 
86 
87  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
88 
89  if (!qResult.success)
90  {
91  // todo catch in provider
92  throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
93  }
94 
95  armarx::grasping::GraspCandidateDict candidates;
96  for (const MemoryID& id : ids)
97  {
98  if (const armem::wm::EntityInstance* instance = qResult.memory.findInstance(id))
99  {
100  candidates[id.str()] = new grasping::GraspCandidate(asGraspCandidate(*instance));
101  }
102  }
103 
104  return candidates;
105  }
106 
107  grasping::BimanualGraspCandidatePtr
109  {
111 
112  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
113 
114  qb.singleEntitySnapshot(id.getEntitySnapshotID());
115 
116  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
117 
118  if (!qResult.success)
119  {
120  // todo catch in provider
121  throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
122  }
123 
124 
125  armarx::grasping::BimanualGraspCandidatePtr candidate;
126 
128  visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const& instance)
129  {
130  if (instance.id() == id)
131  {
132  candidate =
133  new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
134  }
135  return true;
136  };
137 
138  visitor.applyTo(qResult.memory);
139 
140  return candidate;
141  }
142 
143  grasping::GraspCandidateDict
145  const std::string& entity) const
146  {
148 
149  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
150 
151  qb.coreSegments()
152  .withName(properties.graspCandidateMemoryName)
154  .withName(provider)
155  .entities()
156  .withName(entity)
157  .snapshots()
158  .latest();
159 
160  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
161 
162  return getGraspCandidatesFromResultSet(qResult);
163  }
164 
165  std::map<std::string, grasping::BimanualGraspCandidatePtr>
167  const std::string& entity) const
168  {
170 
171  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
172 
173  qb.coreSegments()
174  .withName(properties.bimanualGraspCandidateMemoryName)
176  .withName(provider)
177  .entities()
178  .withName(entity)
179  .snapshots()
180  .latest();
181 
182  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
183 
184  return getBimanualGraspCandidatesFromResultSet(qResult);
185  }
186 
187  grasping::GraspCandidateDict
188  GraspCandidateReader::queryLatestGraspCandidates(const std::string& provider) const
189  {
190 
192 
193  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
194 
195 
196  if (!provider.empty())
197  {
198  qb.coreSegments()
199  .withName(properties.graspCandidateMemoryName)
201  .withName(provider)
202  .entities()
203  .all()
204  .snapshots()
205  .latest();
206  }
207  else
208  {
209  qb.coreSegments()
210  .withName(properties.graspCandidateMemoryName)
212  .all()
213  .entities()
214  .all()
215  .snapshots()
216  .latest();
217  }
218 
219  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
220 
221  return getGraspCandidatesFromResultSet(qResult);
222  }
223 
224  std::map<std::string, grasping::BimanualGraspCandidatePtr>
226  {
228 
229  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
230 
231 
232  if (!provider.empty())
233  {
234  qb.coreSegments()
235  .withName(properties.bimanualGraspCandidateMemoryName)
237  .withName(provider)
238  .entities()
239  .all()
240  .snapshots()
241  .latest();
242  }
243  else
244  {
245  qb.coreSegments()
246  .withName(properties.bimanualGraspCandidateMemoryName)
248  .all()
249  .entities()
250  .all()
251  .snapshots()
252  .latest();
253  }
254 
255  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
256 
257  return getBimanualGraspCandidatesFromResultSet(qResult);
258  }
259 
260  void
262  {
263  ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
264 
265  const std::string prefix = propertyPrefix;
266 
267  def->optional(properties.graspCandidateMemoryName,
268  prefix + "GraspCandidateMemoryName",
269  "Name of the grasping memory core segment to use.");
270 
271  def->optional(properties.memoryName, prefix + "MemoryName");
272  }
273 
274  grasping::GraspCandidateDict
275  GraspCandidateReader::getGraspCandidatesFromResultSet(
276  const armem::client::QueryResult& qResult) const
277  {
278  if (!qResult.success)
279  {
280  throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
281  }
282 
283 
284  std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
285 
287  visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
288  {
289  candidates[instance.id().str()] =
290  new grasping::GraspCandidate(asGraspCandidate(instance));
291  return true;
292  };
293 
294  visitor.applyTo(qResult.memory);
295 
296 
297  return candidates;
298  }
299 
300  std::map<std::string, grasping::BimanualGraspCandidatePtr>
301  GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
302  const armem::client::QueryResult& qResult) const
303  {
304  if (!qResult.success)
305  {
306  throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
307  }
308 
309 
310  std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
311 
312  armem::wm::FunctionalVisitor visitor;
313  visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
314  {
315  candidates[instance.id().str()] =
316  new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
317  return true;
318  };
319 
320  visitor.applyTo(qResult.memory);
321 
322 
323  return candidates;
324  }
325 
326  grasping::GraspCandidateDict
328  const DateTime& timestamp) const
329  {
331 
332  ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
333 
334 
335  if (!provider.empty())
336  {
337  qb.coreSegments()
338  .withName(properties.graspCandidateMemoryName)
340  .withName(provider)
341  .entities()
342  .all()
343  .snapshots()
345  }
346  else
347  {
348  qb.coreSegments()
349  .withName(properties.graspCandidateMemoryName)
351  .all()
352  .entities()
353  .all()
354  .snapshots()
356  }
357 
358  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
359  return getGraspCandidatesFromResultSet(qResult);
360  }
361 
362 
363 } // namespace armarx::armem
armarx::armem::detail::SuccessHeader::success
bool success
Definition: SuccessHeader.h:20
armarx::armem::client::query::EntitySelector::all
EntitySelector & all() override
Definition: selectors.cpp:96
armarx::armem::GraspCandidateReader::queryLatestBimanualGraspCandidates
std::map< std::string, ::armarx::grasping::BimanualGraspCandidatePtr > queryLatestBimanualGraspCandidates(std::string const &provider="") const
Definition: GraspCandidateReader.cpp:225
armarx::armem::client::query::SnapshotSelector::latest
SnapshotSelector & latest()
Definition: selectors.cpp:28
armarx::armem::server::wm::EntityInstance
armem::wm::EntityInstance EntityInstance
Definition: forward_declarations.h:64
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::armem::GraspCandidateReader::GraspCandidateReader
GraspCandidateReader()
Definition: GraspCandidateReader.cpp:13
armarx::armem::client::QueryResult::memory
wm::Memory memory
The slice of the memory that matched the query.
Definition: Query.h:58
armarx::armem::client::query::SnapshotSelector::timeRange
SnapshotSelector & timeRange(Time min, Time max)
Definition: selectors.cpp:42
armarx::armem::client::query::Builder::singleEntitySnapshot
void singleEntitySnapshot(const MemoryID &snapshotID)
Definition: Builder.cpp:115
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::GraspCandidateReader::queryLatestBimanualGraspCandidateEntity
std::map< std::string, ::armarx::grasping::BimanualGraspCandidatePtr > queryLatestBimanualGraspCandidateEntity(std::string const &provider, std::string const &entity) const
Definition: GraspCandidateReader.cpp:166
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
armarx::armem::client::query::Builder::buildQueryInput
QueryInput buildQueryInput() const
Definition: Builder.cpp:11
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
armarx::armem::GraspCandidateReader::queryGraspCandidateInstanceByID
::armarx::grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const &id) const
Definition: GraspCandidateReader.cpp:65
GraspCandidateReader.h
armarx::armem
Definition: LegacyRobotStateMemoryAdapter.cpp:31
armarx::armem::client::query::Builder::multipleEntitySnapshots
void multipleEntitySnapshots(const std::vector< MemoryID > &snapshotIDs)
Definition: Builder.cpp:123
mns.h
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
use
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 to deal in the Software without including without limitation the rights to use
Definition: license.txt:39
armarx::armem::asGraspCandidate
armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance &instance)
Definition: GraspCandidateReader.cpp:38
armarx::armem::detail::SuccessHeader::errorMessage
std::string errorMessage
Definition: SuccessHeader.h:21
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::armem::client::query::Builder::coreSegments
CoreSegmentSelector & coreSegments()
Start specifying core segments.
Definition: Builder.cpp:38
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::armem::client::query::CoreSegmentSelector::withName
CoreSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:177
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::wm::FunctionalVisitor::instanceConstFn
std::function< bool(const EntityInstance &instance)> instanceConstFn
Definition: FunctionalVisitor.h:97
armarx::armem::wm::Visitor::applyTo
bool applyTo(Memory &memory)
Definition: Visitor.cpp:21
armarx::armem::GraspCandidateReader::queryGraspCandidateInstancesByID
::armarx::grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector< armem::MemoryID > const &ids) const
Definition: GraspCandidateReader.cpp:79
armarx::armem::GraspCandidateReader::queryGraspCandidatesNewerThan
::armarx::grasping::GraspCandidateDict queryGraspCandidatesNewerThan(std::string const &provider="", const armarx::DateTime &timestamp=armarx::DateTime::Now()) const
Definition: GraspCandidateReader.cpp:327
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
FunctionalVisitor.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::fromAron
void fromAron(const arondto::MemoryID &dto, MemoryID &bo)
Definition: aron_conversions.cpp:8
armarx::armem::client::query::EntitySelector::withName
EntitySelector & withName(const std::string &name) override
Definition: selectors.cpp:103
armarx::armem::client::query::ProviderSegmentSelector::withName
ProviderSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:140
armarx::armem::base::detail::GetFindInstanceMixin::findInstance
auto * findInstance(const MemoryID &instanceID)
Find an entity instance.
Definition: lookup_mixins.h:119
PropertyDefinitionContainer.h
armarx::armem::wm::FunctionalVisitor
A Visitor which can be parametrized by std::function instead of inheriting and overriding.
Definition: FunctionalVisitor.h:15
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::asBimanualGraspCandidate
armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance &instance)
Definition: GraspCandidateReader.cpp:52
armarx::armem::error::QueryFailed
Indicates that a query resulted in an Error.
Definition: ArMemError.h:190
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::armem::GraspCandidateReader::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: GraspCandidateReader.cpp:261
aron_conversions.h
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
armarx::armem::client::query::CoreSegmentSelector::providerSegments
ProviderSegmentSelector & providerSegments()
Start specifying provider segments.
Definition: selectors.cpp:160
armarx::armem::GraspCandidateReader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem, bool use=true)
Definition: GraspCandidateReader.cpp:18
util.h
armarx::armem::GraspCandidateReader::queryBimanualGraspCandidateInstanceByID
::armarx::grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const &id) const
Definition: GraspCandidateReader.cpp:108
armarx::armem::client::MemoryNameSystem::getReader
Reader getReader(const MemoryID &memoryID)
Get a reader to the given memory name.
Definition: MemoryNameSystem.cpp:177
armarx::armem::base::EntityInstanceBase::data
const DataT & data() const
Definition: EntityInstanceBase.h:129
armarx::armem::client::query::ProviderSegmentSelector::all
ProviderSegmentSelector & all() override
Definition: selectors.cpp:133
armarx::armem::GraspCandidateReader::queryLatestGraspCandidates
::armarx::grasping::GraspCandidateDict queryLatestGraspCandidates(std::string const &provider="") const
Definition: GraspCandidateReader.cpp:188
armarx::armem::GraspCandidateReader::queryLatestGraspCandidateEntity
::armarx::grasping::GraspCandidateDict queryLatestGraspCandidateEntity(std::string const &provider, std::string const &entity) const
Definition: GraspCandidateReader.cpp:144
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:29