GraspCandidateWriter.cpp
Go to the documentation of this file.
1 #include "GraspCandidateWriter.h"
2 
4 
5 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
8 
9 namespace armarx::armem
10 {
11 
12  void
14  {
15  // Wait for the memory to become available and add it as dependency.
16  ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
17  << "' ...";
18  try
19  {
20  memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
21  ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '"
22  << properties.memoryName << "'";
23  }
25  {
26  ARMARX_ERROR << e.what();
27  return;
28  }
29  }
30 
31  void
33  {
34  ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
35 
36  const std::string prefix = propertyPrefix;
37 
38  def->optional(properties.graspMemoryName,
39  prefix + "GraspMemoryName",
40  "Name of the grasping memory core segment to use.");
41  def->optional(properties.bimanualGraspMemoryName,
42  prefix + "BimanualGraspMemoryName",
43  "Name of the bimanual grasping memory core segment to use.");
44 
45  def->optional(properties.memoryName, prefix + "MemoryName");
46  }
47 
48  bool
49  GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
50  const armem::Time& timestamp,
51  const std::string& provider)
52  {
53  armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
54  std::string objectName = "UnknownObject";
55  if (candidate.sourceInfo)
56  {
57  objectName = candidate.sourceInfo->referenceObjectName;
58  }
59 
60 
61  toAron(aronGraspCandidate, candidate);
62  auto dict = aronGraspCandidate.toAron();
63 
64  return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
65  }
66 
67  bool
69  const armarx::grasping::BimanualGraspCandidate& candidate,
70  const armem::Time& timestamp,
71  const std::string& provider)
72  {
73  armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
74 
75  std::string objectName = "UnknownObject";
76  if (candidate.sourceInfo)
77  {
78  objectName = candidate.sourceInfo->referenceObjectName;
79  }
80 
81  toAron(aronGraspCandidate, candidate);
82 
83  auto dict = aronGraspCandidate.toAron();
84 
85  return commitToMemory(
86  {dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
87  }
88 
89  bool
91  const armarx::grasping::GraspCandidateSeq& candidates,
92  const armem::Time& timestamp,
93  const std::string& provider)
94  {
95 
96  bool success = true;
97 
98  std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
99  armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
100 
101  for (const auto& candidate : candidates)
102  {
103  std::string objectName = "UnknownObject";
104  if (candidate->sourceInfo)
105  {
106  objectName = candidate->sourceInfo->referenceObjectName;
107  }
108  toAron(aronGraspCandidate, *candidate);
109 
110  updates[objectName].push_back(aronGraspCandidate.toAron());
111  }
112 
113  for (const auto& [key, dict] : updates)
114  {
115 
116  if (!commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
117  {
118  success = false;
119  }
120  }
121  return success;
122  }
123 
124  bool
126  const armarx::grasping::BimanualGraspCandidateSeq& candidates,
127  const armem::Time& timestamp,
128  const std::string& provider)
129  {
130  bool success = true;
131 
132  std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
133  armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
134 
135  for (const armarx::grasping::BimanualGraspCandidatePtr& candidate : candidates)
136  {
137  std::string objectName = "UnknownObject";
138  if (candidate->sourceInfo)
139  {
140  objectName = candidate->sourceInfo->referenceObjectName;
141  }
142  toAron(aronGraspCandidate, *candidate);
143  updates[objectName].push_back(aronGraspCandidate.toAron());
144  }
145 
146  for (const auto& [key, dict] : updates)
147  {
148 
149  if (!commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
150  {
151  success = false;
152  }
153  }
154  return success;
155  }
156 
157  bool
158  GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
159  const std::string& providerName,
160  const std::string& entityName,
161  const armem::Time& timestamp,
162  const std::string& coreMemoryName)
163  {
164  std::lock_guard g{memoryWriterMutex};
165 
166  const auto result = memoryWriter.addSegment(coreMemoryName, providerName);
167 
168  if (not result.success)
169  {
170  ARMARX_ERROR << result.errorMessage;
171 
172  // TODO(fabian.reister): message
173  return false;
174  }
175 
176  const auto providerId = armem::MemoryID(result.segmentID);
177  const auto entityID = providerId.withEntityName(entityName).withTimestamp(timestamp);
178 
180  update.entityID = entityID;
181  update.instancesData = instances;
182  update.referencedTime = timestamp;
183 
184  ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
185  armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
186 
187  ARMARX_DEBUG << updateResult;
188 
189  if (not updateResult.success)
190  {
191  ARMARX_ERROR << updateResult.errorMessage;
192  }
193 
194  return updateResult.success;
195  }
196 } // namespace armarx::armem
armarx::armem::client::Writer::addSegment
data::AddSegmentResult addSegment(const std::string &coreSegmentName, const std::string &providerSegmentName, bool clearWhenExists=false) const
Definition: Writer.cpp:16
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::GraspCandidateWriter::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: GraspCandidateWriter.cpp:32
armarx::armem::EntityUpdateResult
Result of an EntityUpdate.
Definition: Commit.h:72
armarx::armem::toAron
void toAron(arondto::MemoryID &dto, const MemoryID &bo)
Definition: aron_conversions.cpp:19
armarx::armem::client::MemoryNameSystem::useWriter
Writer useWriter(const MemoryID &memoryID)
Use a memory server and get a writer for it.
Definition: MemoryNameSystem.cpp:276
armarx::armem
Definition: LegacyRobotStateMemoryAdapter.cpp:31
mns.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:27
armarx::armem::client::Writer::commit
CommitResult commit(const Commit &commit) const
Writes a Commit to the memory.
Definition: Writer.cpp:59
armarx::armem::GraspCandidateWriter::commitBimanualGraspCandidate
bool commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate &candidate, const armem::Time &timestamp, const std::string &provider)
Definition: GraspCandidateWriter.cpp:68
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
GraspCandidateWriter.h
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
PropertyDefinitionContainer.h
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::armem::GraspCandidateWriter::commitBimanualGraspCandidateSeq
bool commitBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq &candidates, const armem::Time &timestamp, const std::string &provider)
Definition: GraspCandidateWriter.cpp:125
aron_conversions.h
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
armarx::armem::GraspCandidateWriter::commitGraspCandidateSeq
bool commitGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq &candidates, const armem::Time &timestamp, const std::string &provider)
Definition: GraspCandidateWriter.cpp:90
armarx::armem::GraspCandidateWriter::commitGraspCandidate
bool commitGraspCandidate(const armarx::grasping::GraspCandidate &candidate, const armem::Time &timestamp, const std::string &provider)
Definition: GraspCandidateWriter.cpp:49
armarx::status::success
@ success
armarx::armem::GraspCandidateWriter::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: GraspCandidateWriter.cpp:13
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:29