3 #include <SimoxUtility/algorithm/get_map_keys_values.h>
10 #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
16 #include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
17 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
18 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
19 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
32 const std::string prefix = propertyPrefix;
34 def->optional(properties.memoryName, prefix +
"MemoryName");
36 def->optional(properties.coreInstanceSegmentName,
37 prefix +
"CoreSegment",
38 "Name of the memory core segment to use for object instances.");
39 def->optional(properties.coreClassSegmentName,
40 prefix +
"CoreSegment",
41 "Name of the memory core segment to use for object classes.");
45 properties.providerName, prefix +
"write.ProviderName",
"Name of this provider");
52 ARMARX_IMPORTANT <<
"Writer: Waiting for memory '" << properties.memoryName <<
"' ...";
55 memoryWriter = memoryNameSystem.
useWriter(properties.memoryName);
56 memoryReader = memoryNameSystem.
useReader(properties.memoryName);
57 ARMARX_IMPORTANT <<
"Writer: Connected to memory '" << properties.memoryName <<
"'";
65 const auto resultCoreClassSegment =
66 memoryWriter.
addSegment(properties.coreClassSegmentName, properties.providerName);
68 const auto resultCoreInstanceSegmentName =
69 memoryWriter.
addSegment(properties.coreInstanceSegmentName, properties.providerName);
77 memoryNameSystem.
subscribe(
id,
this, &Writer::updateKnownObjects);
83 arondto::RobotDescription aronArticulatedObjectDescription;
91 const std::vector<armem::MemoryID>& snapshotIDs)
98 Writer::updateKnownObjects()
100 knownObjects = queryDescriptions(
Time::Now());
102 ARMARX_INFO <<
"Known articulated objects " << simox::alg::get_keys(knownObjects);
105 std::optional<armem::MemoryID>
110 const auto objectId = knownObjects.find(obj.description.name);
113 if (objectId != knownObjects.end())
115 return objectId->second;
120 throw LocalException(
"articulated object class " + obj.description.name +
" not found");
123 if (properties.allowClassCreation)
131 std::optional<armem::MemoryID>
134 std::lock_guard g{memoryWriterMutex};
136 ARMARX_DEBUG <<
"Trying to create core segment + provider segment";
140 memoryWriter.
addSegment(properties.coreClassSegmentName, properties.providerName);
142 if (not result.success)
144 ARMARX_ERROR <<
"Creating core segment failed. Reason: " << result.errorMessage;
151 const auto entityID =
152 providerId.withEntityName(obj.
description.
name).withTimestamp(timestamp);
155 update.entityID = entityID;
157 arondto::RobotDescription aronArticulatedObjectDescription;
160 update.instancesData = {aronArticulatedObjectDescription.toAron()};
161 update.referencedTime = timestamp;
168 if (not updateResult.success)
177 return updateResult.snapshotID;
183 return properties.providerName;
189 this->properties.providerName = providerName;
195 std::lock_guard g{memoryWriterMutex};
202 ARMARX_DEBUG <<
"Storing articulated object instance '" << entityName <<
"' (provider '"
203 << properties.providerName <<
"')";
211 update.entityID = providerId.withEntityName(entityName);
217 arondto::ObjectInstance objectInstance;
220 const auto classId = storeOrGetClass(obj);
229 toAron(objectInstance.classID, *classId);
239 cs.dataset = objectId.
dataset();
241 objectInstance.pose.objectID = cs;
242 objectInstance.pose.providerName = properties.providerName;
243 objectInstance.pose.attachmentValid =
false;
245 update.instancesData = {objectInstance.toAron()};
246 update.referencedTime = timestamp;
253 if (not updateResult.success)
258 return updateResult.success;
264 const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
269 <<
"Known classes are " << simox::alg::get_keys(knownObjects);
277 std::optional<robot::RobotDescription>
282 .getCoreSegment(properties.coreClassSegmentName)
283 .getProviderSegment(properties.providerName);
297 std::unordered_map<std::string, armem::MemoryID>
300 const armem::wm::CoreSegment& coreSegment =
303 std::unordered_map<std::string, armem::MemoryID> descriptions;
304 coreSegment.forEachEntity(
305 [&descriptions](
const wm::Entity& entity)
309 ARMARX_WARNING <<
"No entity found";
316 const armem::MemoryID snapshotID(sn.id());
317 descriptions.insert({robotDescription->name, snapshotID});
325 std::unordered_map<std::string, armem::MemoryID>
326 Writer::queryDescriptions(
const armem::Time& timestamp)
329 armem::client::query::Builder qb;
333 .coreSegments().withName(properties.coreClassSegmentName)
334 .providerSegments().all()
336 .snapshots().latest();
339 const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
343 if (not qResult.success)
348 return getRobotDescriptions(qResult.memory);