diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 0913ca80116f59b1fdb69f4fd105c7b0c72fb598..ef43ecebfd31e5a6e7e8db6ac1fa89acfb06e715 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -264,14 +264,28 @@
             propertyName="TextToSpeechTopicName"
             propertyIsOptional="true"
             propertyDefaultValue="TextToSpeech" />
-        <Proxy include="RobotAPI/interface/components/DSObstacleAvoidanceInterface.h"
-            humanName="Platform obstacle avoidance"
+        <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h"
+            humanName="Platform obstacle avoidance interface"
             typeName="ObstacleAvoidanceInterfacePrx"
             memberName="platformObstacleAvoidance"
             getterName="getPlatformObstacleAvoidance"
             propertyName="PlatformObstacleAvoidanceName"
             propertyIsOptional="true"
             propertyDefaultValue="PlatformObstacleAvoidance" />
+	<Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectorInterface.h"
+            humanName="Platform obstacle detection interface"
+            typeName="ObstacleDetectionInterfacePrx"
+            memberName="platformObstacleDetection"
+            getterName="getPlatformObstacleDetection"
+            propertyName="PlatformObstacleDetectionName"
+            propertyDefaultValue="PlatformObstacleAvoidance" />
+	<Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
+            humanName="Dynamic obstacle manager interface"
+            typeName="DynamicObstacleManagerInterfacePrx"
+            memberName="dynamicObstacleManager"
+            getterName="getDynamicObstacleManager"
+            propertyName="DynamicObstacleManagerName"
+            propertyDefaultValue="DynamicObstacleManager" />
         <Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
             humanName="Grasp Candidate Observer"
             typeName="grasping::GraspCandidateObserverInterfacePrx"
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
index 5278fcda731543f4cdb98c8a05e872067d214bef..ee891427aebe42fd391a64efee478ab2e9b260f0 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
@@ -59,7 +59,7 @@ namespace
 {
 
     std::shared_ptr<doa::Obstacle>
-    create_doa_obstacle(const armarx::dsobstacleavoidance::Obstacle& obstacle)
+    create_doa_obstacle(const armarx::obstacledetection::Obstacle& obstacle)
     {
         Eigen::Vector3d pos(obstacle.posX, obstacle.posY, obstacle.posZ);
         Eigen::Quaterniond ori{Eigen::AngleAxisd(obstacle.yaw, Eigen::Vector3d::UnitZ())};
@@ -87,11 +87,11 @@ namespace
 }
 
 
-namespace armarx::dsobstacleavoidance
+namespace armarx::obstacledetection
 {
 
     void
-    from_json(const nlohmann::json& j, Obstacle& obstacle)
+    from_json(const nlohmann::json& j, armarx::obstacledetection::Obstacle& obstacle)
     {
         j.at("name").get_to(obstacle.name);
         j.at("posX").get_to(obstacle.posX);
@@ -176,8 +176,8 @@ armarx::DSObstacleAvoidance::onInitComponent()
         std::ifstream ifs(scene_obstacles_path);
         nlohmann::json j = nlohmann::json::parse(ifs);
 
-        std::vector<dsobstacleavoidance::Obstacle> obstacles = j;
-        for (dsobstacleavoidance::Obstacle& obstacle : obstacles)
+        std::vector<obstacledetection::Obstacle> obstacles = j;
+        for (obstacledetection::Obstacle& obstacle : obstacles)
         {
             if (m_doa.cfg.only_2d)
             {
@@ -272,7 +272,7 @@ armarx::DSObstacleAvoidance::getConfig(const Ice::Current&)
 
 void
 armarx::DSObstacleAvoidance::updateObstacle(
-    const armarx::dsobstacleavoidance::Obstacle& obstacle,
+    const armarx::obstacledetection::Obstacle& obstacle,
     const Ice::Current&)
 {
     ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << ".";
@@ -313,18 +313,18 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co
 }
 
 
-std::vector<armarx::dsobstacleavoidance::Obstacle>
+std::vector<armarx::obstacledetection::Obstacle>
 armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
 {
     std::shared_lock l{m_doa.mutex};
 
-    std::vector<dsobstacleavoidance::Obstacle> obstacle_list;
+    std::vector<obstacledetection::Obstacle> obstacle_list;
 
     for (auto& [doa_name, doa_obstacle] : m_doa.env)
     {
         std::shared_ptr<doa::Ellipsoid> doa_ellipsoid =
             std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
-        dsobstacleavoidance::Obstacle obstacle;
+        obstacledetection::Obstacle obstacle;
 
         obstacle.name = doa_name;
         obstacle.posX = doa_ellipsoid->get_position()(0) * 1000;
@@ -409,7 +409,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
 
     // Make working copies of the updates and free them again.
     std::vector<buffer::update> update_log;
-    std::map<std::string, dsobstacleavoidance::Obstacle> obstacles;
+    std::map<std::string, obstacledetection::Obstacle> obstacles;
     {
         std::lock_guard l{m_buf.mutex};
 
@@ -461,7 +461,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
                     ARMARX_DEBUG << "Adding obstacle " << update.name << ".";
                 }
 
-                const dsobstacleavoidance::Obstacle& obstacle = obstacles.at(update.name);
+                const obstacledetection::Obstacle& obstacle = obstacles.at(update.name);
                 m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
             }
             break;
@@ -504,7 +504,7 @@ armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const
 
 void
 armarx::DSObstacleAvoidance::sanity_check_obstacle(
-    const armarx::dsobstacleavoidance::Obstacle& obstacle)
+    const armarx::obstacledetection::Obstacle& obstacle)
 const
 {
     ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`.";
@@ -553,7 +553,7 @@ armarx::DSObstacleAvoidance::run_visualization()
         color_m = Color::blue(255, 64);
     }
 
-    for (const dsobstacleavoidance::Obstacle& obstacle : getObstacles())
+    for (const obstacledetection::Obstacle& obstacle : getObstacles())
     {
         const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ;
         const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ;
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
index eb48fee2314c69b4d70103b1b133dd54960a6305..2f9035f45eb97f749f17e083ae8a150abf061ada 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
@@ -50,7 +50,7 @@
 
 // RobotAPI
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/interface/components/DSObstacleAvoidanceInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
 
 
 namespace armarx
@@ -79,11 +79,11 @@ namespace armarx
 
         void
         updateObstacle(
-            const dsobstacleavoidance::Obstacle& obstacle,
+            const obstacledetection::Obstacle& obstacle,
             const Ice::Current& = Ice::emptyCurrent)
         override;
 
-        std::vector<dsobstacleavoidance::Obstacle>
+        std::vector<obstacledetection::Obstacle>
         getObstacles(const Ice::Current& = Ice::emptyCurrent)
         override;
 
@@ -147,7 +147,7 @@ namespace armarx
     private:
 
         void
-        sanity_check_obstacle(const dsobstacleavoidance::Obstacle& obstacle)
+        sanity_check_obstacle(const obstacledetection::Obstacle& obstacle)
         const;
 
         void
@@ -205,7 +205,7 @@ namespace armarx
 
             mutable std::mutex mutex;
             std::vector<DSObstacleAvoidance::buffer::update> update_log;
-            std::map<std::string, dsobstacleavoidance::Obstacle> obstacles;
+            std::map<std::string, obstacledetection::Obstacle> obstacles;
             bool needs_env_update;
             IceUtil::Time last_env_update;
         };
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e39c54aecc85964c9c5f5b4ec0fe5d6ea37a20c3..ccc5409fc336854e175d0245f7a24a5b973f5bb7 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -231,6 +231,12 @@ namespace armarx
         }
     }
 
+    void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
+    {
+        usleep(m_min_value_for_accepting);
+    }
+
+
     void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
     {
         obstacledetection::Obstacle new_unmanaged_obstacle;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 371ccbc3e9099e5baed3ae8165cea453ba2059f9..1629b7efe3102dccf85521ac0fac6379e09225cc 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -72,6 +72,7 @@ namespace armarx
         void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
         void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
         void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
+        void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
 
     protected:
         void onInitComponent() override;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index a2587dc0c09e5329484dceb1838ea097e016283f..fa3c189dcd4a46055b33ef4566ede2f73a405317 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -4,7 +4,7 @@ armarx_component_set_name("ObjectPoseObserver")
 set(COMPONENT_LIBS
     ArmarXCore ArmarXCoreInterfaces
     ArmarXGuiComponentPlugins
-    RobotAPIComponentPlugins
+    RobotAPIArmarXObjects RobotAPIComponentPlugins
 
     ${PROJECT_NAME}Interfaces
 )
@@ -15,7 +15,6 @@ set(SOURCES
     plugins/ObjectPoseProviderPlugin.cpp
     plugins/ObjectPoseClientPlugin.cpp
 
-    ObjectFinder.cpp
     ice_conversions.cpp
     ObjectPose.cpp
 )
@@ -25,7 +24,6 @@ set(HEADERS
     plugins/ObjectPoseProviderPlugin.h
     plugins/ObjectPoseClientPlugin.h
 
-    ObjectFinder.h
     ice_conversions.h
     ObjectPose.h
 )
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 23484b0b377b7d2fad5b56c8aab7f87f613624a3..2fbb8fbfbee6943e4fa6d7cf1ac81fba83bb73e6 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -31,8 +31,8 @@
 #include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
-#include "ObjectFinder.h"
 #include "ObjectPose.h"
 
 #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index e40d29d709de816fedbd44e84d54f08961542eff..0085e76c8cca45298aad7c3cf11617c08c3e4ed8 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -3,8 +3,12 @@
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotConfig.h>
 
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
+#include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 
 
@@ -23,6 +27,25 @@ namespace armarx
         { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
     };
 
+
+    objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
+    {
+        objpose::AABB ice;
+        ice.center = new Vector3(aabb.center());
+        ice.extents = new Vector3(aabb.extents());
+        return ice;
+    }
+
+    objpose::Box objpose::toIce(const simox::OrientedBox<float>& oobb)
+    {
+        objpose::Box ice;
+        ice.position = new Vector3(oobb.center());
+        ice.orientation = new Quaternion(oobb.rotation().eval());
+        ice.extents = new Vector3(oobb.dimensions());
+        return ice;
+    }
+
 }
 
 
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
index e9a0da5413706e3e53eea0f4fb4fee267c5f8354..d71b34cae0b1e09f19ea32e37e5c5564aa70e2ea 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -7,6 +7,15 @@
 #include "ObjectPose.h"
 
 
+namespace simox
+{
+    // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+    struct AxisAlignedBoundingBox;
+    // #include <SimoxUtility/shapes/OrientedBox.h>
+    template<class FloatT> class OrientedBox;
+}
+
+
 namespace armarx::objpose
 {
 
@@ -14,4 +23,8 @@ namespace armarx::objpose
 
     extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
 
+
+    objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb);
+    objpose::Box toIce(const simox::OrientedBox<float>& oobb);
+
 }
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
index 02a1fc0d69dd2618b13664b3e52b77dfe4d42ed4..52f6af1c1df0c1fd925a7b5f59371a19c353015c 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -46,7 +46,7 @@
 
 // RobotAPI
 #include <RobotAPI/components/units/PlatformUnit.h>
-#include <RobotAPI/interface/components/ObstacleAvoidanceInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h>
 #include <RobotAPI/libraries/core/PIDController.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index 9552091c342285ccbfe1be5603ed105a1708e792..d21a3c55b7574a459f0be3e2577870cf7640f5e3 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -46,6 +46,8 @@ module armarx
 
         void
         remove_obstacle(string name);
+
+        void wait_unitl_obstacles_are_ready();
     };
 };
 
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5a3e68a75af25b715267a830dff6f726a250a93b
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ArmarXObjects
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ArmarXObjects.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h
new file mode 100644
index 0000000000000000000000000000000000000000..54315bcc8ebc727fdf55a6595a0683d6ccd1fea0
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h
@@ -0,0 +1,31 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ArmarXObjects
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "ObjectFinder.h"
+#include "ObjectInfo.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c819dd3e7e82293c031e5c6d19267cb9a2b1c533
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -0,0 +1,35 @@
+set(LIB_NAME       ${PROJECT_NAME}ArmarXObjects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+    ArmarXCoreInterfaces ArmarXCore
+)
+
+set(LIB_FILES
+    ArmarXObjects.cpp
+
+    ObjectFinder.cpp
+    ObjectInfo.cpp
+)
+set(LIB_HEADERS
+    ArmarXObjects.h
+
+    ObjectFinder.h
+    ObjectInfo.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    target_include_directories(ArmarXObjects PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
similarity index 61%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index eef7faa2231ea91333ed9b4da81ff402f74a1cdc..b7a74d925c6fc005cd506fff1f5a28558106c752 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,9 +1,6 @@
 #include "ObjectFinder.h"
 
 #include <SimoxUtility/filesystem/list_directory.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
-#include <SimoxUtility/shapes/OrientedBox.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -179,122 +176,5 @@ namespace armarx
         return packageName;
     }
 
-
-    ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
-                           const std::string& dataset, const std::string& name) :
-        _packageName(packageName), _packageDataDir(packageDataDir), _dataset(dataset), _name(name)
-    {
-    }
-
-    std::string ObjectInfo::package() const
-    {
-        return _packageName;
-    }
-
-    std::string ObjectInfo::dataset() const
-    {
-        return _dataset;
-    }
-
-    std::string ObjectInfo::name() const
-    {
-        return _name;
-    }
-
-    std::string ObjectInfo::id() const
-    {
-        return _dataset + "/" + _name;
-    }
-
-    ObjectInfo::path ObjectInfo::objectDirectory() const
-    {
-        return path(_packageName) / _dataset / _name;
-    }
-
-    PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
-    {
-        std::string extension = _extension;
-        if (extension.at(0) != '.')
-        {
-            extension = "." + extension;
-        }
-        std::string filename = _name + suffix + extension;
-
-        PackageFileLocation loc;
-        loc.package = _packageName;
-        loc.relativePath = objectDirectory() / filename;
-        loc.absolutePath = _packageDataDir / loc.relativePath;
-        return loc;
-    }
-
-    PackageFileLocation ObjectInfo::simoxXML() const
-    {
-        return file(".xml");
-    }
-
-    PackageFileLocation ObjectInfo::wavefrontObj() const
-    {
-        return file(".obj");
-    }
-
-    PackageFileLocation ObjectInfo::boundingBoxJson() const
-    {
-        return file(".json", "_bb");
-    }
-
-    simox::AxisAlignedBoundingBox ObjectInfo::aabb() const
-    {
-        nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
-        nlohmann::json jaabb = j.at("aabb");
-        auto min = jaabb.at("min").get<Eigen::Vector3f>();
-        auto max = jaabb.at("max").get<Eigen::Vector3f>();
-
-        return simox::AxisAlignedBoundingBox(min, max);
-    }
-
-    simox::OrientedBox<float> ObjectInfo::oobb() const
-    {
-        nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
-        nlohmann::json joobb = j.at("oobb");
-        auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
-        auto min = joobb.at("min").get<Eigen::Vector3f>();
-        auto extents = joobb.at("extents").get<Eigen::Vector3f>();
-
-        Eigen::Vector3f corner = ori * min;
-
-        simox::OrientedBox<float> oobb(corner,
-                                       ori.col(0) * extents(0),
-                                       ori.col(1) * extents(1),
-                                       ori.col(2) * extents(2));
-        return oobb;
-    }
-
-    bool ObjectInfo::checkPaths() const
-    {
-        namespace fs = std::filesystem;
-        bool result = true;
-
-        if (!fs::is_regular_file(simoxXML().absolutePath))
-        {
-            ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
-            result = false;
-        }
-        if (!fs::is_regular_file(wavefrontObj().absolutePath))
-        {
-            ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
-            result = false;
-        }
-
-        return result;
-    }
-
-}
-
-namespace armarx
-{
-    std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
-    {
-        return os << "'" << rhs.id() << "'";
-    }
 }
 
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7f3170c1954e810a077c88c0ea2f3aa1cc9f898
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <filesystem>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "ObjectInfo.h"
+
+
+namespace armarx
+{
+    /**
+     * @brief Used to find objects in the ArmarX objects repository [1].
+     *
+     * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
+     */
+    class ObjectFinder : Logging
+    {
+    public:
+        using path = std::filesystem::path;
+
+    public:
+
+        ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
+
+        std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
+        std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
+
+
+        std::vector<std::string> getDatasets() const;
+        std::vector<path> getDatasetDirectories() const;
+
+        std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
+        std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
+        std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
+
+
+    private:
+
+        void init() const;
+
+        path _rootDirAbs() const;
+        path _rootDirRel() const;
+
+
+    private:
+
+        /// Name of package containing the object models (ArmarXObjects by default).
+        mutable std::string packageName;
+
+        /// Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
+        mutable path packageDataDir;
+
+    };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..feaf2f203aca7445081f2f91e3d511a7b5bbdbd5
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -0,0 +1,146 @@
+#include "ObjectInfo.h"
+
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx
+{
+    namespace fs = std::filesystem;
+
+
+    ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
+                           const std::string& dataset, const std::string& name) :
+        _packageName(packageName), _packageDataDir(packageDataDir), _dataset(dataset), _name(name)
+    {
+    }
+
+    std::string ObjectInfo::package() const
+    {
+        return _packageName;
+    }
+
+    std::string ObjectInfo::dataset() const
+    {
+        return _dataset;
+    }
+
+    std::string ObjectInfo::name() const
+    {
+        return _name;
+    }
+
+    std::string ObjectInfo::id() const
+    {
+        return _dataset + "/" + _name;
+    }
+
+    ObjectInfo::path ObjectInfo::objectDirectory() const
+    {
+        return path(_packageName) / _dataset / _name;
+    }
+
+    PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
+    {
+        std::string extension = _extension;
+        if (extension.at(0) != '.')
+        {
+            extension = "." + extension;
+        }
+        std::string filename = _name + suffix + extension;
+
+        PackageFileLocation loc;
+        loc.package = _packageName;
+        loc.relativePath = objectDirectory() / filename;
+        loc.absolutePath = _packageDataDir / loc.relativePath;
+        return loc;
+    }
+
+    PackageFileLocation ObjectInfo::simoxXML() const
+    {
+        return file(".xml");
+    }
+
+    PackageFileLocation ObjectInfo::wavefrontObj() const
+    {
+        return file(".obj");
+    }
+
+    PackageFileLocation ObjectInfo::boundingBoxJson() const
+    {
+        return file(".json", "_bb");
+    }
+
+    simox::AxisAlignedBoundingBox ObjectInfo::aabb() const
+    {
+        nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+        nlohmann::json jaabb = j.at("aabb");
+
+        auto center = jaabb.at("center").get<Eigen::Vector3f>();
+        auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
+        auto min = jaabb.at("min").get<Eigen::Vector3f>();
+        auto max = jaabb.at("max").get<Eigen::Vector3f>();
+
+        simox::AxisAlignedBoundingBox aabb(min, max);
+
+        ARMARX_CHECK(aabb.center().isApprox(center)) << aabb.center().transpose() << "\n" << center.transpose();
+        ARMARX_CHECK(aabb.extents().isApprox(extents)) << aabb.extents().transpose() << "\n" << extents.transpose();
+        ARMARX_CHECK(aabb.min().isApprox(min)) << aabb.min().transpose() << "\n" << min.transpose();
+        ARMARX_CHECK(aabb.max().isApprox(max)) << aabb.max().transpose() << "\n" << max.transpose();
+
+        return aabb;
+    }
+
+    simox::OrientedBox<float> ObjectInfo::oobb() const
+    {
+        nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+        nlohmann::json joobb = j.at("oobb");
+        auto pos = joobb.at("pos").get<Eigen::Vector3f>();
+        auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
+        auto extents = joobb.at("extents").get<Eigen::Vector3f>();
+
+        Eigen::Vector3f corner = pos - ori * extents / 2;
+
+        simox::OrientedBox<float> oobb(corner,
+                                       ori.col(0) * extents(0),
+                                       ori.col(1) * extents(1),
+                                       ori.col(2) * extents(2));
+
+        ARMARX_CHECK(oobb.center().isApprox(pos)) << oobb.center().transpose() << "\n" << pos.transpose();
+        ARMARX_CHECK(oobb.rotation().isApprox(ori)) << oobb.rotation() << "\n" << ori;
+        ARMARX_CHECK(oobb.dimensions().isApprox(extents)) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+        return oobb;
+    }
+
+    bool ObjectInfo::checkPaths() const
+    {
+        namespace fs = std::filesystem;
+        bool result = true;
+
+        if (!fs::is_regular_file(simoxXML().absolutePath))
+        {
+            ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+            result = false;
+        }
+        if (!fs::is_regular_file(wavefrontObj().absolutePath))
+        {
+            ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+            result = false;
+        }
+
+        return result;
+    }
+
+}
+
+namespace armarx
+{
+    std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
+    {
+        return os << "'" << rhs.id() << "'";
+    }
+}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
similarity index 57%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
rename to source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 7bbaa7c2282029ed78399027154f392f84ccad8a..0fd1ef03216ba8fdbb620462306b9779baeabde9 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -1,66 +1,20 @@
 #pragma once
 
 #include <filesystem>
+#include <string>
 
-#include <ArmarXCore/core/logging/Logging.h>
 
 namespace simox
 {
+    // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
     struct AxisAlignedBoundingBox;
+    // #include <SimoxUtility/shapes/OrientedBox.h>
     template<class FloatT> class OrientedBox;
 }
 
 namespace armarx
 {
 
-    class ObjectInfo;
-
-
-    /**
-     * @brief Used to find objects in the ArmarX objects repository [1].
-     *
-     * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
-     */
-    class ObjectFinder : Logging
-    {
-    public:
-        using path = std::filesystem::path;
-
-    public:
-
-        ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
-
-        std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
-        std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
-
-
-        std::vector<std::string> getDatasets() const;
-        std::vector<path> getDatasetDirectories() const;
-
-        std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
-        std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
-        std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
-
-
-    private:
-
-        void init() const;
-
-        path _rootDirAbs() const;
-        path _rootDirRel() const;
-
-    private:
-
-
-        /// Name of package containing the object models (ArmarXObjects by default).
-        mutable std::string packageName;
-
-        /// Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
-        mutable path packageDataDir;
-
-    };
-
-
     struct PackageFileLocation
     {
         /// Name of the ArmarX package.
@@ -86,6 +40,9 @@ namespace armarx
         ObjectInfo(const std::string& packageName, const path& packageDataDir,
                    const std::string& dataset, const std::string& name);
 
+        virtual ~ObjectInfo() = default;
+
+
         std::string package() const;
 
         std::string dataset() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0af92b11dcd764b7ab376f07a5869cb1962e5f4f
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ArmarXObjects
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ArmarXObjects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArmarXObjects.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2ad25f7ea27f28908cf50d0c950ba4170487fc85
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+
+armarx_add_test(ArmarXObjectsTest ArmarXObjectsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 049d3fd2e11370c975a87557043dd5c40a253029..90a62cd3087bd6e2462865fb26a21c4140e4863c 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,4 +1,5 @@
 add_subdirectory(ArmarXEtherCAT)
+add_subdirectory(ArmarXObjects)
 add_subdirectory(ControllerUIUtility)
 add_subdirectory(core)
 add_subdirectory(DebugDrawerConfigWidget)
@@ -12,8 +13,9 @@ add_subdirectory(RobotStatechartHelpers)
 add_subdirectory(SimpleJsonLogger)
 add_subdirectory(SimpleTrajectory)
 add_subdirectory(widgets)
-add_subdirectory(natik)
-add_subdirectory(aron)
 
 add_subdirectory(diffik)
+add_subdirectory(natik)
+
 add_subdirectory(armem)
+add_subdirectory(aron)