armem/dev => master

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
     {