From 72bc5144f6b897086d86d90a75ff9aa2609de443 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 May 2021 12:20:36 +0200
Subject: [PATCH] articulated object class segment: loading objects via
ObjectFinder on startup (if desired)
---
.../articulated_object_class/Segment.cpp | 49 ++++++++++++++++++-
.../server/articulated_object_class/Segment.h | 7 +++
2 files changed, 55 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index b7e2aecf7..6ba2ae71a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -36,14 +36,24 @@ namespace armarx::armem::server::obj::articulated_object_class
{
defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
+ defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
+ "If true, load the objects from the objects package on startup.");
+
}
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ if (p.loadFromObjectsPackage)
+ {
+ loadByObjectFinder(p.objectsPackage);
+ }
}
void Segment::connect(viz::Client arviz)
@@ -51,6 +61,43 @@ namespace armarx::armem::server::obj::articulated_object_class
// this->visu = std::make_unique<Visu>(arviz, *this);
}
+ void Segment::loadByObjectFinder(const std::string& package)
+ {
+ ObjectFinder finder(package);
+
+ const auto knownArticulatedObjectDescriptions = finder.findAllArticulatedObjects(true);
+ ARMARX_INFO << "Found " << knownArticulatedObjectDescriptions.size() << " articulated objects";
+
+ loadObjectsIntoMemory(knownArticulatedObjectDescriptions, package);
+ }
+
+ void Segment::loadObjectsIntoMemory(const std::vector<armem::articulated_object::ArticulatedObjectDescription>& descriptions, const std::string& package)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(package);
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ // ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
+ // << objectFinder.getPackageName() << "' ...";
+ Commit commit;
+ for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(desc.name);
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription aronRobotDescription;
+ toAron(aronRobotDescription, desc);
+
+ update.instancesData = { aronRobotDescription.toAron()};
+ }
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " articulated object classes from '"
+ << package << "'.";
+ iceMemory.commit(commit);
+ }
+
+
std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
index 3c17b51f3..b6665dcdf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
@@ -36,6 +36,8 @@
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/types.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
namespace armarx::armem
{
namespace server
@@ -72,6 +74,8 @@ namespace armarx::armem::server::obj::articulated_object_class
private:
+ void loadByObjectFinder(const std::string& package);
+ void loadObjectsIntoMemory(const std::vector<armem::articulated_object::ArticulatedObjectDescription>& descriptions, const std::string& package);
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
@@ -81,6 +85,9 @@ namespace armarx::armem::server::obj::articulated_object_class
{
std::string coreClassSegmentName = "ArticulatedObjectClass";
int64_t maxHistorySize = -1;
+
+ std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
+ bool loadFromObjectsPackage = true;
};
Properties p;
--
GitLab