Merge branch 'eurobin_nancy' into 'master'

euROBIN

See merge request !497

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp

@@ -185,7 +185,7 @@ namespace armarx
             {
                 if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
                 {
-                    ARMARX_INFO << deactivateSpam(10)
+                    ARMARX_INFO << deactivateSpam(60)
                                 << "Missing Iterations or iterations out of order! "
                                 << "This should not happen. " << VAROUT(_last_iteration_id) << ", "
                                 << VAROUT(step.iterationId);

source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp

 #include "KnownGraspProviderSegment.h"
 
+#include <SimoxUtility/algorithm/string/string_tools.h>
 #include <VirtualRobot/Grasping/GraspSet.h>
 #include <VirtualRobot/XML/ObjectIO.h>
 
@@ -63,7 +64,9 @@ namespace armarx::armem::grasping::segment
                     arondto::KnownGraspSet retGraspSet;
 
                     retGraspSet.name = graspSet->getName();
-                    retGraspSet.robot = graspSet->getRobotType();
+                    
+                    retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
+
                     retGraspSet.endeffector = graspSet->getEndEffector();
 
                     for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp

@@ -7,6 +7,7 @@
 #include <VirtualRobot/XML/RobotIO.h>
 
 #include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
@@ -32,37 +33,27 @@ namespace armarx::armem::articulated_object
                                                   const std::string& instanceName,
                                                   VirtualRobot::RobotIO::RobotDescription loadMode)
     {
-        const auto descriptions = queryDescriptions(timestamp, providerName);
+        ARMARX_VERBOSE << "Getting `" << typeName + "/" + instanceName << "`";
 
-        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+        const std::optional<ArticulatedObject> articulatedObjectDescription =
+            get(typeName + "/" + instanceName, timestamp, providerName);
+        ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
+            << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
 
-        const auto it = std::find_if(
-            descriptions.begin(),
-            descriptions.end(),
-            [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
-            { return desc.name == typeName; });
-
-        if (it == descriptions.end())
-        {
-            ARMARX_WARNING << "Description for articulate object with type <" << typeName
-                           << "> not (yet) available!";
-            return nullptr;
-        }
-
-        ARMARX_DEBUG << "Description for articulate object with type <" << typeName
-                     << "> available!";
-
-        auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+        auto obj = VirtualRobot::RobotIO::loadRobot(
+            articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
 
         if (not obj)
         {
-            ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
-                           << ">!";
+            ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
+                           << instanceName << "` from file `"
+                           << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
+
             return nullptr;
         }
 
         obj->setName(instanceName);
-        obj->setType(it->name);
+        obj->setType(typeName);
 
         return obj;
     }

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp

@@ -51,7 +51,8 @@ namespace armarx::armem::articulated_object
 
     bool
     ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
-                                                    const armem::Time& timestamp)
+                                                    const armem::Time& timestamp,
+                                                    const bool isStatic)
     {
 
         ARMARX_CHECK_NOT_NULL(articulatedObject);
@@ -59,6 +60,6 @@ namespace armarx::armem::articulated_object
         armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
             convert(*articulatedObject, Time::Now());
 
-        return store(armemArticulatedObject);
+        return store(armemArticulatedObject, isStatic);
     }
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h

@@ -14,8 +14,8 @@ namespace armarx::armem::articulated_object
     public:
         using Writer::Writer;
 
-        bool
-        storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
-                               const armem::Time& timestamp);
+        bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+                                    const armem::Time& timestamp,
+                                    bool isStatic = false);
     };
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp

@@ -171,6 +171,7 @@ namespace armarx::armem::articulated_object
 
         if (providerName.has_value()) // query single provider
         {
+            ARMARX_VERBOSE << "Single provider query";
             ARMARX_CHECK_NOT_EMPTY(providerName.value());
 
             // clang-format off
@@ -183,6 +184,8 @@ namespace armarx::armem::articulated_object
         }
         else // query all providers
         {
+            ARMARX_VERBOSE << "All provider query";
+
             // clang-format off
             qb
             .coreSegments().withName(objects::constants::CoreClassSegmentName)
@@ -192,7 +195,9 @@ namespace armarx::armem::articulated_object
             // clang-format on
         }
 
+        ARMARX_VERBOSE << "Before query";
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+        ARMARX_VERBOSE << "After query";
 
         ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
@@ -201,6 +206,8 @@ namespace armarx::armem::articulated_object
             return {};
         }
 
+        ARMARX_TRACE;
+        ARMARX_VERBOSE << "getRobotDescriptions";
         return getRobotDescriptions(qResult.memory);
     }
 
@@ -380,20 +387,18 @@ namespace armarx::armem::articulated_object
             memory.getCoreSegment(objects::constants::CoreClassSegmentName);
 
         std::vector<robot_state::description::RobotDescription> descriptions;
-        coreSegment.forEachEntity(
-            [&descriptions](const wm::Entity& entity)
+        coreSegment.forEachInstance(
+            [&descriptions](const wm::EntityInstance& instance)
             {
-                if (not entity.empty())
+                ARMARX_VERBOSE << "Converting ...";
+                if (const auto robotDescription = convertRobotDescription(instance))
                 {
-                    const auto robotDescription =
-                        convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
-                    if (robotDescription)
-                    {
-                        descriptions.push_back(*robotDescription);
-                    }
+                    descriptions.push_back(*robotDescription);
                 }
             });
 
+        ARMARX_VERBOSE << descriptions.size() << " descriptions";
+
         return descriptions;
     }
 

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp

@@ -15,11 +15,11 @@
 #include <RobotAPI/libraries/armem/core/operations.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
 
 #include "utils.h"
 
@@ -191,7 +191,7 @@ namespace armarx::armem::articulated_object
     }
 
     bool
-    Writer::storeInstance(const ArticulatedObject& obj) const
+    Writer::storeInstance(const ArticulatedObject& obj, const bool isStatic) const
     {
         std::lock_guard g{memoryWriterMutex};
 
@@ -218,7 +218,7 @@ namespace armarx::armem::articulated_object
         arondto::ObjectInstance objectInstance;
         toAron(objectInstance, obj.config);
 
-        const auto classId = storeOrGetClass(obj);
+        const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
         if (not classId)
         {
@@ -229,10 +229,17 @@ namespace armarx::armem::articulated_object
         // install memory link
         toAron(objectInstance.classID, *classId);
 
+        // set object instance id
+        const MemoryID memoryInstanceId = classId->withEntityName(entityName);
+
         armem::MemoryID id;
-        id.setEntityID(classId->getEntityID());
+        id.setEntityID(memoryInstanceId.getEntityID());
 
         armarx::ObjectID objectId(id.entityName);
+        ARMARX_INFO << "Object ID: " << objectId;
+
+        ARMARX_CHECK_NOT_EMPTY(objectId.instanceName())
+            << "An object instance name must be provided!";
 
         armarx::arondto::ObjectID cs;
         cs.className = objectId.className();
@@ -243,6 +250,8 @@ namespace armarx::armem::articulated_object
         objectInstance.pose.providerName = properties.providerName;
         objectInstance.pose.attachmentValid = false;
 
+        objectInstance.pose.isStatic = isStatic;
+
         update.instancesData = {objectInstance.toAron()};
         update.referencedTime = timestamp;
 
@@ -260,7 +269,7 @@ namespace armarx::armem::articulated_object
     }
 
     bool
-    Writer::store(const ArticulatedObject& obj) const
+    Writer::store(const ArticulatedObject& obj, const bool isStatic) const
     {
         const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
@@ -271,7 +280,7 @@ namespace armarx::armem::articulated_object
             return false;
         }
 
-        return storeInstance(obj);
+        return storeInstance(obj, isStatic);
     }
 
     // TODO this is a duplicate

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h

@@ -45,9 +45,9 @@ namespace armarx::armem::articulated_object
         void connect(armem::client::MemoryNameSystem& memoryNameSystem);
 
 
-        bool store(const ArticulatedObject& obj) const override;
+        bool store(const ArticulatedObject& obj, bool isStatic) const override;
 
-        bool storeInstance(const ArticulatedObject& obj) const;
+        bool storeInstance(const ArticulatedObject& obj, bool isStatic) const;
         std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
 
         // const std::string& getPropertyPrefix() const override;

source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h

@@ -30,7 +30,7 @@ namespace armarx::armem::articulated_object
     public:
         virtual ~WriterInterface() = default;
 
-        virtual bool store(const ArticulatedObject& obj) const = 0;
+        virtual bool store(const ArticulatedObject& obj, bool isStatic) const = 0;
     };
 
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp

@@ -3,6 +3,7 @@
 #include <Eigen/Geometry>
 
 #include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
 #include <SimoxUtility/math/pose.h>
 #include <SimoxUtility/math/rescale.h>
 
@@ -72,6 +73,9 @@ namespace armarx::armem::server::obj::familiar_object_instance
             auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
             auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
 
+            auto confidenceCmap = simox::color::cmaps::viridis();
+            confidenceCmap.set_vlimits(0, 1);
+
             for (const auto& familiarObject : familiarObjects)
             {
                 if (not isWithinTimeFrame(familiarObject))
@@ -120,7 +124,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
                         auto box = viz::Box(objectId.str());
                         box.pose(global_T_bb.matrix());
                         box.size(familiarObject.bounding_box.extents);
-                        box.color(simox::Color::gray(128, alpha));
+
+                        auto color = confidenceCmap.at(familiarObject.confidence);
+                        color.a = alpha;
+
+                        box.color(color);
                         layerBox.add(box);
                     }
                 }