ArmarXObjects: utility functions to convert ObjectPose to Simox types + filtering

source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt

@@ -30,6 +30,8 @@ set(LIB_FILES
     plugins/ObjectPoseProviderPlugin.cpp
     plugins/ObjectPoseClientPlugin.cpp
     plugins/RequestedObjects.cpp
+
+    util.cpp
 )
 set(LIB_HEADERS
     ArmarXObjects.h
@@ -54,6 +56,8 @@ set(LIB_HEADERS
     plugins/ObjectPoseProviderPlugin.h
     plugins/ObjectPoseClientPlugin.h
     plugins/RequestedObjects.h
+
+    util.h
 )
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

source/RobotAPI/libraries/ArmarXObjects/util.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "util.h"
+
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/Primitive.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/VisualizationNode.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+namespace armarx::objpose
+{
+
+    objpose::ObjectPoseSeq
+    filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlacklist)
+    {
+        const auto isBlacklisted = [&datasetBlacklist](const objpose::ObjectPose& objectPose)
+        {
+            const auto dataset = objectPose.objectID.dataset();
+
+            return std::find(datasetBlacklist.begin(), datasetBlacklist.end(), dataset) !=
+                   datasetBlacklist.end();
+        };
+
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+        return objects;
+    }
+
+
+    VirtualRobot::ManipulationObjectPtr
+    asManipulationObject(const objpose::ObjectPose& objectPose)
+    {
+        ObjectFinder finder;
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        if (auto obstacle = finder.loadManipulationObject(objectPose))
+        {
+            obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+            return obstacle;
+        }
+
+        ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+        return nullptr;
+    }
+
+
+    VirtualRobot::SceneObjectSetPtr
+    asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+    {
+        ObjectFinder finder;
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        for (const auto& objectPose : objectPoses)
+        {
+            if (auto obstacle = finder.loadManipulationObject(objectPose))
+            {
+                obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+                sceneObjects->addSceneObject(obstacle);
+            }
+        }
+
+        return sceneObjects;
+    }
+
+
+} // namespace armarx::objpose

source/RobotAPI/libraries/ArmarXObjects/util.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::objpose
+{
+    objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
+                                         const std::vector<std::string>& datasetBlacklist);
+
+    VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
+    VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+  
+} // namespace armarx::objpose