object pose provider example: publishing articulated object

source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp

@@ -30,6 +30,7 @@
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
+#include <RobotAPI/interface/core/articulated_object/types.h>
 
 namespace armarx
 {
@@ -128,7 +129,7 @@ namespace armarx
                 update = requestedObjects.updateRequestedObjects(now);
             }
 
-            if (update.added.size() > 0 || update.removed.size() > 0)
+            if (!update.added.empty() || !update.removed.empty())
             {
                 ARMARX_INFO << "Added: " << update.added
                             << "Removed: " << update.removed;
@@ -173,6 +174,28 @@ namespace armarx
             ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
             objectPoseTopic->reportObjectPoses(getName(), poses);
 
+
+
+            const articulated_object::data::ArticulatedObject articulatedObject
+            {
+                .id = {
+                    .name = "dishwasher",
+                    .xml = "ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml",
+                    // .scaling = 1.0
+                },
+                .config = {
+                    .timestamp = TimeUtil::GetTime().toSecondsDouble(),
+                    .globalPose = new FramedPose(),
+                    .jointMap = {
+                        {"dishwasher_door_joint", 0.2},
+                        {"drawer_joint", 350}
+                    }
+                }
+            };
+
+            ARMARX_VERBOSE << "Reporting articulated object";
+            // TODO uncomment articulatedObjectTopic->reportArticulatedObjects(getName(), {articulatedObject});
+
             cycle.waitForCycleDuration();
         }
     }