28 #include <Eigen/Geometry>
30 #include <opencv2/imgcodecs.hpp>
31 #include <opencv2/imgproc.hpp>
32 #include <opencv2/opencv.hpp>
34 #include <SimoxUtility/color/cmaps.h>
35 #include <SimoxUtility/math/pose/pose.h>
40 #include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
62 defs->topic(debugObserver);
64 defs->optional(p.usedMemoryName,
"mem.UsedMemoryName",
"Name of the memory to use.");
65 defs->optional(p.commitFrequency,
67 "Frequency in which example data is commited. (max = 50Hz)");
75 return "ExampleMemoryClient";
86 p.commitFrequency =
std::min(p.commitFrequency, 50.f);
105 exampleProviderID = addProviderSegment();
107 exampleEntityID = exampleProviderID.
withEntityName(
"example_entity");
114 const std::vector<armem::MemoryID>& snapshotIDs)
117 <<
" was updated by " << snapshotIDs.size()
122 exampleEntityID,
this, &ExampleMemoryClient::processExampleEntityUpdate);
149 commitMultipleSnapshots(exampleEntityID, 3);
157 queryExactSnapshot(snapshotID);
166 commitExamplesWithIDs();
170 commitExamplesWithLinks();
178 commitExamplesWithUntypedData();
182 queryPredictionEngines();
185 CycleUtil c(
static_cast<int>(1000 / p.commitFrequency));
186 while (!task->isStopped())
188 commitSingleSnapshot(exampleEntityID);
190 c.waitForCycleDuration();
195 ExampleMemoryClient::addProviderSegment()
197 armem::data::AddSegmentInput
input;
198 input.coreSegmentName =
"ExampleModality";
199 input.providerSegmentName =
"FancyMethodModality";
211 ExampleMemoryClient::commitSingleSnapshot(
const armem::MemoryID& entityID)
213 std::default_random_engine gen(std::random_device{}());
214 std::uniform_int_distribution<int> distrib(-20, 20);
218 update.entityID = entityID;
221 double diff = (
update.referencedTime - runStarted).toMilliSecondsDouble() / 1000;
223 auto dict1 = std::make_shared<aron::data::Dict>();
224 auto dict2 = std::make_shared<aron::data::Dict>();
226 auto sin = std::make_shared<aron::data::Float>(std::sin(diff));
227 auto cos = std::make_shared<aron::data::Float>(std::cos(diff));
229 auto sqrt = std::make_shared<aron::data::Double>(
std::sqrt(diff));
230 auto lin = std::make_shared<aron::data::Long>(
static_cast<long>(diff * 1000));
231 auto rand = std::make_shared<aron::data::Int>(distrib(gen));
233 dict1->addElement(
"sin", sin);
234 dict1->addElement(
"cos", cos);
236 dict2->addElement(
"sqrt",
sqrt);
237 dict2->addElement(
"lin", lin);
238 dict2->addElement(
"rand", rand);
240 update.instancesData = {dict1, dict2};
245 if (!updateResult.success)
250 return updateResult.snapshotID;
254 ExampleMemoryClient::commitMultipleSnapshots(
const armem::MemoryID& entityID,
int num)
258 for (
int i = 0; i < num; ++i)
261 update.entityID = entityID;
263 for (
int j = 0; j < i; ++j)
265 update.instancesData.push_back(std::make_shared<aron::data::Dict>());
271 if (!commitResult.allSuccess())
280 ExampleMemoryClient::queryLatestSnapshot(
const armem::MemoryID& entityID)
283 <<
"\n- entityID: \t'" << entityID <<
"'";
285 armem::client::query::Builder builder;
286 builder.coreSegments()
295 armem::client::QueryResult qResult = memoryReader.
query(builder.buildQueryInput());
301 armem::wm::Memory& memory = qResult.memory;
306 const armem::wm::Entity* entity = memory.findEntity(entityID);
308 <<
"Entity " << entityID <<
" was not found in " <<
armem::print(memory);
315 <<
"\n- entity: \t" << entity->name() <<
"\n- snapshot: \t"
316 << snapshot.time() <<
"\n- #instances: \t" << snapshot.size();
319 tab.queryResult = std::move(memory);
329 ExampleMemoryClient::queryExactSnapshot(
const armem::MemoryID& snapshotID)
332 <<
"\n- snapshotID: \t'" << snapshotID <<
"'";
334 namespace qf = armem::client::query_fns;
335 armem::client::query::Builder qb;
336 qb.singleEntitySnapshot(snapshotID);
338 armem::client::QueryResult qResult = memoryReader.
query(qb.buildQueryInput());
343 armem::wm::Memory memory = std::move(qResult.memory);
348 <<
"\n- time: \t" << entitySnapshot.time()
349 <<
"\n- # instances: \t" << entitySnapshot.size();
353 memory.getEntity(snapshotID).getLatestSnapshot();
356 <<
"\n- time: \t" << entitySnapshot.time()
357 <<
"\n- # instances: \t" << entitySnapshot.size();
368 ExampleMemoryClient::commitExampleData()
375 if (!addSegmentResult.success)
383 if (!addSegmentResult.success)
397 update.referencedTime = time;
399 armem::example::ExampleData
data;
411 update.referencedTime = time;
413 armem::example::ExampleData
data;
414 data.the_bool =
true;
416 data.the_float = 21.5;
418 data.the_long = 424242;
419 data.the_string =
"fourty two";
420 data.the_float_list = {21, 42, 84};
421 data.the_int_list = {21, 42, 84};
422 data.the_string_list = simox::alg::multi_to_string(
data.the_int_list);
423 data.the_object_list.emplace_back();
425 data.the_float_dict = {
430 data.the_int_dict = {
436 data.the_position = {42, 24, 4224};
437 data.the_orientation = Eigen::AngleAxisf(1.57f, Eigen::Vector3f(1, 1, 1).normalized());
438 data.the_pose = simox::math::pose(
data.the_position,
data.the_orientation);
440 data.the_3x1_vector = {24, 42, 2442};
446 simox::ColorMap cmap = simox::color::cmaps::plasma();
448 cv::Mat& image =
data.the_rgb24_image;
449 image.create(10, 20, image.type());
450 cmap.set_vlimits(0,
float(image.cols + image.rows));
451 using Pixel = cv::Point3_<uint8_t>;
452 image.forEach<Pixel>(
453 [&cmap](Pixel& pixel,
const int index[]) ->
void
463 cv::imwrite(
"/tmp/the_rgb24_image.png", image);
466 cv::Mat& image =
data.the_depth32_image;
467 image.create(20, 10, image.type());
468 image.forEach<
float>(
469 [&image](
float& pixel,
const int index[]) ->
void
472 cmap.set_vlimits(0, 100);
473 cv::Mat rgb(image.rows, image.cols, CV_8UC3);
474 using Pixel = cv::Point3_<uint8_t>;
476 [&image, &cmap](Pixel& pixel,
const int index[]) ->
void
486 cv::imwrite(
"/tmp/the_depth32_image.png", image);
497 update.referencedTime = time;
499 armem::example::LinkedData
data;
500 data.yet_another_int = 42;
501 data.yet_another_string =
"Hi! I'm from another core segment!";
502 data.yet_another_object.element_int = 8349;
503 data.yet_another_object.element_float = -1e3;
504 data.yet_another_object.element_string =
"I'm a nested object in some linked data.";
512 theAnswerSnapshotID = commitResult.
results.at(1).snapshotID;
513 yetMoreDataSnapshotID = commitResult.
results.at(2).snapshotID;
522 ExampleMemoryClient::queryExampleData()
525 armem::client::query::Builder qb;
527 .withID(exampleProviderID)
529 .withID(exampleProviderID)
535 armem::client::QueryResult result = memoryReader.
query(qb.buildQueryInput());
538 tab.queryResult = std::move(result.memory);
548 ExampleMemoryClient::commitExamplesWithIDs()
557 update.referencedTime = time;
559 armem::example::ExampleData
data;
568 update.referencedTime = time;
570 armem::example::ExampleData
data;
582 armem::example::ExampleData
data;
591 update.referencedTime = time;
593 armem::example::ExampleData
data;
611 ARMARX_IMPORTANT <<
"Resolving multiple memory IDs via Memory Name System:";
613 std::vector<armem::MemoryID> ids;
620 std::map<armem::MemoryID, armem::wm::EntityInstance> instances =
624 std::stringstream ss;
625 for (
const auto& [
id, instance] : instances)
627 ss <<
"- Snapshot " <<
id <<
" "
628 <<
"\n--> Instance" << instance.id()
629 <<
" (# keys in data: " << instance.data()->childrenSize() <<
")"
637 ExampleMemoryClient::commitExamplesWithLinks()
647 update.referencedTime = time;
649 armem::example::ExampleData
data;
666 ARMARX_IMPORTANT <<
"Resolving multiple memory IDs via Memory Name System:";
668 std::vector<armem::MemoryID> ids;
675 std::map<armem::MemoryID, armem::wm::EntityInstance> instances =
679 std::stringstream ss;
680 for (
const auto& [
id, instance] : instances)
682 ss <<
"- Snapshot " <<
id <<
" "
683 <<
"\n--> Instance" << instance.id()
684 <<
" (# keys in data: " << instance.data()->childrenSize() <<
")"
692 ExampleMemoryClient::commitExampleImages()
700 update.referencedTime = time;
702 auto currentFolder = std::filesystem::current_path();
703 auto opencv_img = cv::imread(
704 (currentFolder /
"images" / (
std::to_string(imageCounter + 1) +
".jpg")).
string());
708 auto data = std::make_shared<aron::data::Dict>();
718 ExampleMemoryClient::commitExamplesWithUntypedData()
726 update.referencedTime = time;
728 armem::example::ExampleData
data;
733 aron->addElement(
"unexpectedString",
734 std::make_shared<aron::data::String>(
"unexpected value"));
737 std::make_shared<aron::data::Dict>(std::map<std::string, aron::data::VariantPtr>{
738 {
"key43", std::make_shared<aron::data::Int>(43)},
739 {
"keyABC", std::make_shared<aron::data::String>(
"ABC")},
741 update.instancesData = {aron};
752 ExampleMemoryClient::queryPredictionEngines()
754 const std::map<armem::MemoryID, std::vector<armem::PredictionEngine>> predictionEngines =
757 std::stringstream ss;
758 ss <<
"Prediction engines available in the server:" << std::endl;
759 for (
const auto& [
id, engines] : predictionEngines)
761 ss <<
" - " <<
id <<
": ";
764 ss << engine.engineID <<
", ";
773 ExampleMemoryClient::processExampleEntityUpdate(
const armem::MemoryID& subscriptionID,
774 const std::vector<armem::MemoryID>& snapshotIDs)
776 std::stringstream ss;
777 ss <<
"example_entity got updated: " << subscriptionID <<
"\n";
778 ss <<
"Updated snapshots: \n";
779 for (
const auto&
id : snapshotIDs)
781 ss <<
"- " <<
id <<
"\n";
803 if (tab.rebuild.exchange(
false))