implemented dynamic lib loading for robotunit

source/RobotAPI/libraries/Controllers/RobotUnit.cpp

@@ -22,6 +22,10 @@
 
 #include "RobotUnit.h"
 
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <ArmarXCore/core/system/DynamicLibrary.h>
+
 armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
 {
     return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
@@ -311,3 +315,80 @@ bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const st
     return lvl0Controllers.end()               != lvl0Controllers.find(jointName) &&
            lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode);
 }
+
+
+bool armarx::RobotUnit::loadLibFromAbsolutePath(const std::string& path)
+{
+    if (loadedLibs.count(path) > 0)
+    {
+        return true;
+    }
+    DynamicLibraryPtr lib(new DynamicLibrary());
+    try
+    {
+        lib->load(path);
+    }
+    catch (...)
+    {
+        handleExceptions();
+        return false;
+    }
+
+    if (lib->isLibraryLoaded())
+    {
+        loadedLibs[path] = lib;
+    }
+    else
+    {
+        ARMARX_ERROR << "Could not load lib " + path + ": " + lib->getErrorMessage();
+        return false;
+    }
+
+
+    return true;
+}
+
+bool armarx::RobotUnit::loadLibFromPath(const std::string& path, const Ice::Current&)
+{
+    std::string absPath;
+    if (ArmarXDataPath::getAbsolutePath(path, absPath))
+    {
+        return loadLibFromAbsolutePath(absPath);
+    }
+    else
+    {
+        ARMARX_ERROR << "Could not find library " + path;
+        return false;
+    }
+}
+
+bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current&)
+{
+    CMakePackageFinder finder(package);
+    if (!finder.packageFound())
+    {
+        ARMARX_ERROR << "Could not find package '" << package << "'";
+        return false;
+    }
+
+    for (auto libDirPath : armarx::Split(finder.getLibraryPaths(), ";"))
+    {
+        boost::filesystem::path fullPath = libDirPath;
+        fullPath /= "lib" + name  + "." + DynamicLibrary::GetSharedLibraryFileExtension();
+        if (!boost::filesystem::exists(fullPath))
+        {
+            fullPath = libDirPath;
+            fullPath /= name;
+            if (!boost::filesystem::exists(fullPath))
+            {
+                continue;
+            }
+        }
+        if (loadLibFromAbsolutePath(fullPath.string()))
+        {
+            return true;
+        }
+    }
+    ARMARX_ERROR << "Could not find library " <<  name << " in package " << package;
+    return false;
+}

source/RobotAPI/libraries/Controllers/RobotUnit.h

@@ -47,6 +47,8 @@
 
 namespace armarx
 {
+    class DynamicLibrary;
+    typedef boost::shared_ptr<DynamicLibrary> DynamicLibraryPtr;
     /**
      * @class RobotUnitPropertyDefinitions
      * @brief
@@ -98,14 +100,8 @@ namespace armarx
         //loading and switching
         virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
         virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
-        virtual bool loadLibFromPath(const std::string& path   ,                         const Ice::Current& = GlobalIceCurrent) const override
-        {
-            throw "NYI";/*TODO mirko*/
-        }
-        virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) const override
-        {
-            throw "NYI";/*TODO mirko*/
-        }
+        virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
+        virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
 
         //units (ice)
         virtual           KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
@@ -161,8 +157,10 @@ namespace armarx
          * May not be accessed in rt.
          */
         std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
-
+        std::map<std::string, DynamicLibraryPtr> loadedLibs;
     private:
+        bool loadLibFromAbsolutePath(const std::string& path);
+
         JointNameToControlModeDictionary jointsUsedByLVL1Controler;
     };