added RobotPool class and test for reusing robots in e.g. statecharts

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -30,6 +30,7 @@ set(LIB_FILES
     OrientedPoint.cpp
     FramedOrientedPoint.cpp
     RobotStatechartContext.cpp
+    RobotPool.cpp
     checks/ConditionCheckMagnitudeChecks.cpp
     observerfilters/PoseAverageFilter.cpp
     observerfilters/PoseMedianOffsetFilter.cpp
@@ -61,6 +62,7 @@ set(LIB_HEADERS
     OrientedPoint.h
     FramedOrientedPoint.h
     RobotStatechartContext.h
+    RobotPool.h
     observerfilters/PoseMedianFilter.h
     observerfilters/PoseAverageFilter.h
     observerfilters/PoseMedianOffsetFilter.h

source/RobotAPI/libraries/core/RobotPool.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#include "RobotPool.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx
+{
+
+    RobotPool::RobotPool(VirtualRobot::RobotPtr robot) :
+        baseRobot(robot)
+    {
+
+    }
+
+    VirtualRobot::RobotPtr RobotPool::getRobot(int inflation)
+    {
+        ScopedLock lock(mutex);
+        for (auto& r : robots[inflation])
+        {
+            if (r.use_count() == 1)
+            {
+                return r;
+            }
+        }
+        auto newRobot = baseRobot->clone("", VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
+        newRobot->inflateCollisionModel(inflation);
+        ARMARX_CHECK_EQUAL(newRobot.use_count(), 1);
+        ARMARX_DEBUG << "created new robot clone n with inflation " << inflation;
+        robots[inflation].push_back(newRobot);
+        return newRobot;
+    }
+
+    size_t RobotPool::getPoolSize() const
+    {
+        ScopedLock lock(mutex);
+        size_t size = 0;
+        for (auto& pair : robots)
+        {
+            size += pair.second.size();
+        }
+        return size;
+    }
+
+    size_t RobotPool::getRobotsInUseCount() const
+    {
+        ScopedLock lock(mutex);
+        size_t count = 0;
+        for (auto& pair : robots)
+        {
+            for (auto& r : pair.second)
+            {
+                if (r.use_count() > 1)
+                {
+                    count++;
+                }
+            }
+        }
+        return count;
+    }
+
+    size_t RobotPool::clean()
+    {
+        ScopedLock lock(mutex);
+        size_t count = 0;
+        for (auto& pair : robots)
+        {
+            std::vector<VirtualRobot::RobotPtr> newList;
+            for (auto& r : pair.second)
+            {
+                if (r.use_count() > 1)
+                {
+                    newList.push_back(r);
+                }
+                else
+                    count++;
+            }
+            pair.second = newList;
+        }
+        return count;
+    }
+
+} // namespace armarx

source/RobotAPI/libraries/core/RobotPool.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+namespace armarx
+{
+
+    class RobotPool
+    {
+    public:
+        //    struct RobotWrapper
+        //    {
+        //        ScopedLockPtr lock;
+        //        VirtualRobot::RobotPtr robot;
+        //    };
+
+        RobotPool(VirtualRobot::RobotPtr robot);
+        /**
+         * @brief getRobot
+         * @param inflation inflation of collision model in mm
+         * @return
+         */
+        VirtualRobot::RobotPtr getRobot(int inflation = 0);
+        size_t getPoolSize() const;
+        size_t getRobotsInUseCount() const;
+        /**
+         * @brief Removes unused robots from the pool.
+         * @return number of cleaned up robots
+         */
+        size_t clean();
+    private:
+        std::map<int, std::vector<VirtualRobot::RobotPtr>> robots;
+        VirtualRobot::RobotPtr baseRobot;
+        mutable Mutex mutex;
+    };
+    typedef std::shared_ptr<RobotPool> RobotPoolPtr;
+} // namespace armarx
+
+

source/RobotAPI/libraries/core/test/RobotTest.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::RobotTest::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include "../RobotPool.h"
+
+using namespace armarx;
+
+
+BOOST_AUTO_TEST_CASE(RobotPoolTest)
+{
+    const std::string project = "RobotAPI";
+    armarx::CMakePackageFinder finder(project);
+
+    ARMARX_CHECK_EXPRESSION_W_HINT(finder.packageFound(), project);
+
+
+    armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+
+    std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+    ArmarXDataPath::getAbsolutePath(fn, fn);
+    std::string robotFilename = fn;
+    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eCollisionModel);
+    RobotPool pool(robot);
+    {
+        auto clone = pool.getRobot();
+        ARMARX_CHECK_EQUAL(pool.getPoolSize(), 1);
+        ARMARX_CHECK_EQUAL(pool.getRobotsInUseCount(), 1);
+        {
+            auto clone2 = pool.getRobot();
+            ARMARX_CHECK_EQUAL(pool.getPoolSize(), 2);
+            ARMARX_CHECK_EQUAL(pool.getRobotsInUseCount(), 2);
+        }
+        ARMARX_CHECK_EQUAL(pool.getPoolSize(), 2);
+        ARMARX_CHECK_EQUAL(pool.getRobotsInUseCount(), 1);
+        auto clone2 = pool.getRobot();
+        ARMARX_CHECK_EQUAL(pool.getPoolSize(), 2);
+        ARMARX_CHECK_EQUAL(pool.getRobotsInUseCount(), 2);
+    }
+    ARMARX_CHECK_EQUAL(pool.getRobotsInUseCount(), 0);
+    ARMARX_CHECK_EQUAL(pool.clean(), 2);
+    ARMARX_CHECK_EQUAL(pool.getPoolSize(), 0);
+
+}
+