articulated object localizer example: improvement / fix

source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp

@@ -32,10 +32,10 @@
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/XML/RobotIO.h>
 
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 
@@ -49,12 +49,15 @@
 namespace armarx::articulated_object
 {
     ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
-        articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem())),
-        articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem()))
+        articulatedObjectWriter(
+            new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem())),
+        articulatedObjectReader(
+            new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem()))
     {
     }
 
-    armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
+    armarx::PropertyDefinitionsPtr
+    ArticulatedObjectLocalizerExample::createPropertyDefinitions()
     {
         armarx::PropertyDefinitionsPtr defs =
             new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -63,6 +66,9 @@ namespace armarx::articulated_object
 
         defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
 
+        defs->optional(p.obj.dataset, "p.obj.dataset", "");
+        defs->optional(p.obj.className, "p.obj.class", "");
+
         // Reader will override some properties of writer.
         articulatedObjectWriter->registerPropertyDefinitions(defs);
         articulatedObjectReader->registerPropertyDefinitions(defs);
@@ -70,18 +76,21 @@ namespace armarx::articulated_object
         return defs;
     }
 
-    std::string ArticulatedObjectLocalizerExample::getDefaultName() const
+    std::string
+    ArticulatedObjectLocalizerExample::getDefaultName() const
     {
         return "ArticulatedObjectLocalizerExample";
     }
 
-    void ArticulatedObjectLocalizerExample::onInitComponent()
+    void
+    ArticulatedObjectLocalizerExample::onInitComponent()
     {
         // Reader overwrote property registered property of articulatedObjectWriter.
         articulatedObjectWriter->setProviderName(articulatedObjectReader->getProviderName());
     }
 
-    void ArticulatedObjectLocalizerExample::onConnectComponent()
+    void
+    ArticulatedObjectLocalizerExample::onConnectComponent()
     {
         articulatedObjectWriter->connect();
         articulatedObjectReader->connect();
@@ -90,33 +99,42 @@ namespace armarx::articulated_object
         start = armem::Time::Now();
 
         task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
-            this, &ArticulatedObjectLocalizerExample::run,
+            this,
+            &ArticulatedObjectLocalizerExample::run,
             static_cast<int>(1000.f / p.updateFrequency));
         task->start();
     }
 
-    void ArticulatedObjectLocalizerExample::onDisconnectComponent()
+    void
+    ArticulatedObjectLocalizerExample::onDisconnectComponent()
     {
         task->stop();
     }
 
-    void ArticulatedObjectLocalizerExample::onExitComponent()
+    void
+    ArticulatedObjectLocalizerExample::onExitComponent()
     {
     }
 
-    VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
+    VirtualRobot::RobotPtr
+    ArticulatedObjectLocalizerExample::createArticulatedObject()
     {
-        const std::string dishwasherName = "Kitchen/mobile-dishwasher";
+        const std::string dishwasherName = p.obj.dataset + "/" + p.obj.className;
 
         const auto descriptions = articulatedObjectReader->queryDescriptions(armem::Time::Now());
 
         ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
+        for(const auto& description: descriptions)
+        {
+            ARMARX_INFO << "- " << description.name;
+        }
+
         const auto it = std::find_if(
-                            descriptions.begin(),
-                            descriptions.end(),
-                            [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
-        { return desc.name == dishwasherName; });
+            descriptions.begin(),
+            descriptions.end(),
+            [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
+            { return desc.name == dishwasherName; });
 
         if (it == descriptions.end())
         {
@@ -133,21 +151,21 @@ namespace armarx::articulated_object
             return nullptr;
         }
 
-        obj->setName("MobileDishwasher0");
-        obj->setType(it->name);
+        obj->setName("0"); // aka instance name
+        obj->setType(it->name); // aka dataset/class name
 
         return obj;
     }
 
 
-
-    void ArticulatedObjectLocalizerExample::run()
+    void
+    ArticulatedObjectLocalizerExample::run()
     {
-        if (dishwasher == nullptr)
+        if (articulatedObject == nullptr)
         {
-            dishwasher = createDishwasher();
+            articulatedObject = createArticulatedObject();
 
-            if (dishwasher == nullptr) // still
+            if (articulatedObject == nullptr) // still
             {
                 return;
             }
@@ -156,21 +174,23 @@ namespace armarx::articulated_object
         ARMARX_DEBUG << "Reporting articulated objects";
 
         const armem::Time now = armem::Time::Now();
-        const float t = float((now - start).toSecondsDouble());
+        const float t = static_cast<float>((now - start).toSecondsDouble());
 
         // move joints at certain frequency
         const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
 
-        const std::map<std::string, float> jointValues
+        auto jointValues = articulatedObject->getJointValues();
+
+        for (auto& [name, jointValue] : jointValues)
         {
-            {"dishwasher_door_joint", M_PIf32 / 2 * k},
-            {"drawer_joint", 350 * k}
-        };
+            const auto node = articulatedObject->getRobotNode(name);
+            jointValue = node->unscaleJointValue(k, 0, 1);
+        }
 
-        dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
-        dishwasher->setJointValues(jointValues);
+        articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+        articulatedObject->setJointValues(jointValues);
 
-        articulatedObjectWriter->storeArticulatedObject(dishwasher, now);
+        articulatedObjectWriter->storeArticulatedObject(articulatedObject, now);
     }
 
 } // namespace armarx::articulated_object

source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h

@@ -10,6 +10,7 @@
 
 #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
 #include <RobotAPI/interface/armem/server/MemoryInterface.h>
@@ -53,8 +54,8 @@ namespace armarx::articulated_object
         void run();
 
     private:
-        VirtualRobot::RobotPtr createDishwasher();
-        std::shared_ptr<VirtualRobot::Robot> dishwasher;
+        VirtualRobot::RobotPtr createArticulatedObject();
+        std::shared_ptr<VirtualRobot::Robot> articulatedObject;
 
         /// Reference timestamp for object movement
         armem::Time start;
@@ -70,6 +71,12 @@ namespace armarx::articulated_object
         struct Properties
         {
             float updateFrequency{25.F};
+
+            struct
+            {
+                std::string dataset = "Kitchen";
+                std::string className = "mobile-dishwasher";
+            } obj;
         } p;
     };