5 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
16 ARMARX_IMPORTANT <<
"GraspCandidateWriter: Waiting for memory '" << properties.memoryName
20 memoryWriter = memoryNameSystem.
useWriter(properties.memoryName);
22 << properties.memoryName <<
"'";
34 ARMARX_DEBUG <<
"GraspCandidateWriter: registerPropertyDefinitions";
36 const std::string prefix = propertyPrefix;
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.");
45 def->optional(properties.memoryName, prefix +
"MemoryName");
51 const std::string& provider)
53 armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
54 std::string objectName =
"UnknownObject";
55 if (candidate.sourceInfo)
57 objectName = candidate.sourceInfo->referenceObjectName;
61 toAron(aronGraspCandidate, candidate);
62 auto dict = aronGraspCandidate.toAron();
64 return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
69 const armarx::grasping::BimanualGraspCandidate& candidate,
71 const std::string& provider)
73 armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
75 std::string objectName =
"UnknownObject";
76 if (candidate.sourceInfo)
78 objectName = candidate.sourceInfo->referenceObjectName;
81 toAron(aronGraspCandidate, candidate);
83 auto dict = aronGraspCandidate.toAron();
85 return commitToMemory(
86 {dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
91 const armarx::grasping::GraspCandidateSeq& candidates,
93 const std::string& provider)
98 std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
99 armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
101 for (
const auto& candidate : candidates)
103 std::string objectName =
"UnknownObject";
104 if (candidate->sourceInfo)
106 objectName = candidate->sourceInfo->referenceObjectName;
108 toAron(aronGraspCandidate, *candidate);
110 updates[objectName].push_back(aronGraspCandidate.toAron());
113 for (
const auto& [key, dict] : updates)
116 if (!commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
126 const armarx::grasping::BimanualGraspCandidateSeq& candidates,
128 const std::string& provider)
132 std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
133 armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
135 for (
const armarx::grasping::BimanualGraspCandidatePtr& candidate : candidates)
137 std::string objectName =
"UnknownObject";
138 if (candidate->sourceInfo)
140 objectName = candidate->sourceInfo->referenceObjectName;
142 toAron(aronGraspCandidate, *candidate);
143 updates[objectName].push_back(aronGraspCandidate.toAron());
146 for (
const auto& [key, dict] : updates)
149 if (!commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
158 GraspCandidateWriter::commitToMemory(
const std::vector<armarx::aron::data::DictPtr>& instances,
159 const std::string& providerName,
160 const std::string& entityName,
162 const std::string& coreMemoryName)
164 std::lock_guard g{memoryWriterMutex};
166 const auto result = memoryWriter.
addSegment(coreMemoryName, providerName);
168 if (not result.success)
177 const auto entityID = providerId.withEntityName(entityName).withTimestamp(timestamp);
180 update.entityID = entityID;
181 update.instancesData = instances;
182 update.referencedTime = timestamp;
189 if (not updateResult.success)
194 return updateResult.success;