Merge branch 'issue-47-OpenraveYarpPluginLoader' into develop, for #47

roboticslab-uc3m · Dec 6, 2017 · 478142c · 478142c
2 parents b54adbd + 6b42dc7
commit 478142c
Show file tree

Hide file tree

Showing 22 changed files with 1,313 additions and 19 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -18,13 +18,16 @@ set(BUILD_SHARED_LIBS TRUE CACHE INTERNAL "hide this!")
 option(ENABLE_FakeControlboard "Enable/disable compilation of FakeControlboard" TRUE)
 option(ENABLE_YarpOpenraveControlboard "Enable/disable option YarpOpenraveControlboard" TRUE)
 option(ENABLE_YarpOpenraveControlboardCollision "Enable/disable option YarpOpenraveControlboardCollision" TRUE)
+option(ENABLE_YarpOpenraveGrabber "Enable/disable option YarpOpenraveGrabber" TRUE)
+option(ENABLE_YarpOpenraveRGBDSensor  "Enable/disable option YarpOpenraveRGBDSensor" TRUE)
 
 ### options: cpp libraries (openrave plugins)
 option(ENABLE_OpenraveYarpControlboard "Enable/disable option OpenraveYarpControlboard" TRUE)
 option(ENABLE_OpenraveYarpPaintSquares "Enable/disable option OpenraveYarpPaintSquares" TRUE)
 option(ENABLE_OpenraveWorldRpcResponder "Enable/disable option OpenraveWorldRpcResponder" TRUE)
 option(ENABLE_OpenraveYarpForce "Enable/disable option OpenraveYarpForce" TRUE)
 option(ENABLE_OpenraveYarpForceEstimator "Enable/disable option OpenraveYarpForceEstimator" TRUE)
+option(ENABLE_OpenraveYarpPluginLoader "Enable/disable option OpenraveYarpPluginLoader" TRUE)
 
 ### options: cpp programs
 option(ENABLE_teoSim "Choose if you want to compile (deprecated) teoSim" TRUE)

diff --git a/examples/openrave-YarpPluginLoader-controlboard.py b/examples/openrave-YarpPluginLoader-controlboard.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+
+import openravepy
+from openravepy import *
+
+try:
+    RaveInitialize()
+
+    if not RaveLoadPlugin('OpenraveYarpPluginLoader'):
+        raveLogError("Plugin not correctly loaded")
+
+    env=Environment()
+    env.SetViewer('qtcoin')
+    env.Load('/usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml')
+
+    OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader')
+    print OpenraveYarpPluginLoader.SendCommand('open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 --prefix /drl')
+
+    while 1:
+        pass
+
+finally:
+    RaveDestroy()
+
diff --git a/examples/openrave-YarpPluginLoader-grabber.py b/examples/openrave-YarpPluginLoader-grabber.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+
+import openravepy
+from openravepy import *
+
+try:
+    RaveInitialize()
+
+    if not RaveLoadPlugin('OpenraveYarpPluginLoader'):
+        raveLogError("Plugin not correctly loaded")
+
+    env=Environment()
+    env.SetViewer('qtcoin')
+    env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml')
+
+    OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader')
+    print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 1 --prefix /drl')
+
+    while 1:
+        pass
+
+finally:
+    RaveDestroy()
+
diff --git a/examples/openrave-YarpPluginLoader-rgbdsensor.py b/examples/openrave-YarpPluginLoader-rgbdsensor.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+
+import openravepy
+from openravepy import *
+
+try:
+    RaveInitialize()
+
+    if not RaveLoadPlugin('OpenraveYarpPluginLoader'):
+        raveLogError("Plugin not correctly loaded")
+
+    env=Environment()
+    env.SetViewer('qtcoin')
+    env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml')
+
+    OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader')
+    print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveRGBDSensor --robotIndex 0 --sensorIndex 0 --prefix /drl')
+
+    while 1:
+        pass
+
+finally:
+    RaveDestroy()
+
diff --git a/libraries/OpenravePlugins/CMakeLists.txt b/libraries/OpenravePlugins/CMakeLists.txt
@@ -9,6 +9,7 @@ add_subdirectory(OpenraveYarpPaintSquares)
 add_subdirectory(OpenraveWorldRpcResponder)
 add_subdirectory(OpenraveYarpForce)
 add_subdirectory(OpenraveYarpForceEstimator)
+add_subdirectory(OpenraveYarpPluginLoader)
 
 #ENDIF (ENABLE_openraveplugins)
 
diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/CMakeLists.txt b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/CMakeLists.txt
@@ -0,0 +1,53 @@
+# Generated by Juan G Victores 2013
+# Thanks Rosen Diankov for all OpenRAVE developments
+
+IF (ENABLE_OpenraveYarpPluginLoader)
+
+cmake_minimum_required (VERSION 2.6.0)
+
+set(KEYWORD "OpenraveYarpPluginLoader")
+
+project(${KEYWORD})
+
+set(CMAKE_PREFIX_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+find_package(OpenRAVE REQUIRED)
+find_package(YARP REQUIRED)
+
+if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX )
+  add_definitions("-fno-strict-aliasing -Wall -fPIC")
+endif( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX )
+
+find_package(Boost ${OpenRAVE_Boost_VERSION} EXACT COMPONENTS iostreams python thread)
+
+include_directories(${OpenRAVE_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} )
+if( Boost_INCLUDE_DIRS )
+  include_directories(${Boost_INCLUDE_DIRS})
+endif()
+
+link_directories(${OpenRAVE_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS})
+
+macro(build_openrave_plugin name)
+  add_library(${name} SHARED ${name}.cpp)
+  set_target_properties(${name} PROPERTIES COMPILE_FLAGS "${OpenRAVE_CXX_FLAGS}")
+  set_target_properties(${name} PROPERTIES LINK_FLAGS "${OpenRAVE_LINK_FLAGS}")
+  target_link_libraries(${name} ${YARP_LIBRARIES} ${OpenRAVE_LIBRARIES} )
+  # install(TARGETS ${name} DESTINATION lib )
+  # install(TARGETS ${name} DESTINATION ${OpenRAVE_LIBRARY_DIRS}/${OpenRAVE_LIBRARIES}-plugins)
+  execute_process(COMMAND openrave-config --prefix --plugins-dir
+                  OUTPUT_VARIABLE _openrave_config_cmd_output
+                  OUTPUT_STRIP_TRAILING_WHITESPACE)
+  string(REPLACE "\n" ";" _openrave_config_results ${_openrave_config_cmd_output})
+  list(LENGTH _openrave_config_results _len)
+  if(_len EQUAL 2)
+    list(GET _openrave_config_results 0 _openrave_install_prefix)
+    list(GET _openrave_config_results 1 _openrave_plugin_path)
+    file(RELATIVE_PATH _relative_path "${_openrave_install_prefix}" "${_openrave_plugin_path}")
+    install(TARGETS ${name} DESTINATION ${_relative_path})
+  else()
+    message(AUTHOR_WARNING "Unsuccessful call to 'openrave-config', the output was: ${_openrave_config_cmd_output}")
+  endif()
+endmacro(build_openrave_plugin)
+
+build_openrave_plugin(${KEYWORD})
+
+ENDIF (ENABLE_OpenraveYarpPluginLoader)
diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp
@@ -0,0 +1,207 @@
+/**
+ * thanks Rosen Diankov
+   Every plugin contains a bunch of openrave interfaces, the plugincpp plugin creates a simple OpenRAVE::ModuleBase interface named \b mymodule.
+   Inside programs, load the plugin using the RaveLoadPlugin, and then create the module the plugin offers using
+   \verbatim
+   m=RaveCreateModule(env,"mymodule");
+   \endverbatim
+   To test things through the command line, do:
+   \verbatim
+   openrave --loadplugin libplugincpp.so --module mymodule "my args"
+   \endverbatim
+   This will load liboplugincpp.so and startup module "mymodule". From plugincpp, notice that mymodule
+   supports some "commands". These are in-process string-based calls invoked through
+   interface->SendCommand function.
+   If you are using octave or matlab, then can communicate with openrave through tcp/ip, check out: http://openrave.programmingvision.com/wiki/index.php/OctaveMATLAB
+   Most openrave users use python to dynamically interact with openrave. For example:
+   \verbatim
+   openrave.py -i  --loadplugin libplugincpp.so data/lab1.env.xml
+   \endverbatim
+   drops into the python promp with the plugin loaded and a scene loaded. Then it is possible to execute the following python commands to create the interface and call a command:
+   \verbatim
+   m=RaveCreateModule(env,'mymodule')
+   env.Add(m,true,'my args')
+   m.SendCommand('numbodies')
+   \endverbatim
+   <b>Full Example Code:</b>
+ */
+#include <openrave/openrave.h>
+#include <openrave/plugin.h>
+#include <boost/bind.hpp>
+
+#include <yarp/os/all.h>
+#include <yarp/dev/all.h>
+
+#include "ColorDebug.hpp"
+
+class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase
+{
+public:
+    OpenraveYarpPluginLoader(OpenRAVE::EnvironmentBasePtr penv) : OpenRAVE::ModuleBase(penv)
+    {
+        __description = "OpenraveYarpPluginLoader plugin.";
+        RegisterCommand("open",boost::bind(&OpenraveYarpPluginLoader::Open, this,_1,_2),"opens port");
+    }
+
+    virtual ~OpenraveYarpPluginLoader()
+    {
+        //-- Note that we start on element 1, first elem was not via new!!
+        for(size_t i=1;i<argv.size();i++)
+        {
+            //CD_DEBUG("Deleting [%s]\n",argv[i]);
+            delete argv[i];
+            argv[i] = 0;
+        }
+
+        yarpPlugin->close();
+        delete yarpPlugin;
+        yarpPlugin = 0;
+    }
+
+    virtual void Destroy()
+    {
+        RAVELOG_INFO("module unloaded from environment\n");
+    }
+
+    /*int main(const string& cmd)
+    {
+        RAVELOG_INFO("module initialized cmd; %s\n", cmd.c_str());
+        return 0;
+    }*/
+
+    bool Open(std::ostream& sout, std::istream& sinput)
+    {
+        CD_INFO("Checking for yarp network...\n");
+        if ( ! yarp.checkNetwork() )
+        {
+            CD_ERROR("Found no yarp network (try running \"yarpserver &\"), bye!\n");
+            return false;
+        }
+        CD_SUCCESS("Found yarp network.\n");
+
+        //-- Given "std::istream& sinput", create equivalent to "int argc, char *argv[]"
+        //-- Note that char* != const char* given by std::string::c_str();
+        char* dummyProgramName = "dummyProgramName";
+        argv.push_back(dummyProgramName);
+
+        while(sinput)
+        {
+            std::string str;
+            sinput >> str;
+            if(str.length() == 0)  //-- Omits empty string that is usually at end via openrave.
+                continue;
+            char *cstr = new char[str.length() + 1];  // pushed to member argv to be deleted in ~.
+            strcpy(cstr, str.c_str());
+            argv.push_back(cstr);
+        }
+
+        //for(size_t i=0;i<argv.size();i++)
+        //    CD_DEBUG("argv[%d] is [%s]\n",i,argv[i]);
+
+        yarp::os::Property options;
+        options.fromCommand(argv.size(),argv.data());
+
+        CD_DEBUG("config: %s\n", options.toString().c_str());
+
+        //-- Get and put pointer to environment
+        CD_INFO("penv: %p\n",GetEnv().get());
+        OpenRAVE::EnvironmentBasePtr penv = GetEnv();
+        yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr));
+        options.put("penv",v);
+
+        if( ! options.check("name") )  // Enable bypass if "name" already exists
+        {
+            //-- If robotIndex (and then if manipulatorIndex), get and put name
+            if( options.check("robotIndex") )
+            {
+                std::string name;
+
+                if( options.check("prefix") )  // Note that not taken into account if using "name"
+                {
+                    name += options.find("prefix").asString();
+                }
+
+                name += "/";
+                int robotPtrIdx = options.find("robotIndex").asInt();
+
+                std::vector<OpenRAVE::RobotBasePtr> vectorOfRobotPtr;
+                GetEnv()->GetRobots(vectorOfRobotPtr);
+
+                if(robotPtrIdx >= vectorOfRobotPtr.size())
+                {
+                    CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size());
+                    return false;
+                }
+                else if (robotPtrIdx < 0)
+                {
+                    CD_ERROR("robotIndex %d < 0, not loading yarpPlugin.\n",robotPtrIdx);
+                    return false;
+                }
+
+                name += vectorOfRobotPtr[ robotPtrIdx ]->GetName();
+
+                if( options.check("manipulatorIndex") )
+                {
+                    int manipulatorPtrIdx = options.find("manipulatorIndex").asInt();
+
+                    std::vector<OpenRAVE::RobotBase::ManipulatorPtr> vectorOfManipulatorPtr = vectorOfRobotPtr[ robotPtrIdx ]->GetManipulators();
+
+                    if(manipulatorPtrIdx >= vectorOfManipulatorPtr.size())
+                    {
+                        CD_ERROR("manipulatorPtrIdx %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorPtrIdx,vectorOfManipulatorPtr.size());
+                        return false;
+                    }
+                    else if (manipulatorPtrIdx < 0)
+                    {
+                        CD_ERROR("manipulatorPtrIdx %d < 0, not loading yarpPlugin.\n",manipulatorPtrIdx);
+                        return false;
+                    }
+
+                    name += "/";
+                    name += vectorOfManipulatorPtr[ manipulatorPtrIdx ]->GetName();
+                }
+
+                options.put("name",name);
+            }
+        }
+
+        CD_DEBUG("post-config: %s\n", options.toString().c_str());
+
+        yarpPlugin = new yarp::dev::PolyDriver;
+        yarpPlugin->open(options);
+
+        if( ! yarpPlugin->isValid() )
+        {
+            CD_ERROR("yarpPlugin not valid.\n");
+            return false;
+        }
+        CD_SUCCESS("Valid yarpPlugin.\n");
+
+        return true;
+    }
+
+private:
+    std::vector<char *> argv;
+
+    yarp::os::Network yarp;
+    yarp::dev::PolyDriver* yarpPlugin;
+};
+
+OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv)
+{
+    if( type == OpenRAVE::PT_Module && interfacename == "openraveyarppluginloader")
+    {
+        return OpenRAVE::InterfaceBasePtr(new OpenraveYarpPluginLoader(penv));
+    }
+    return OpenRAVE::InterfaceBasePtr();
+}
+
+void GetPluginAttributesValidated(OpenRAVE::PLUGININFO& info)
+{
+    info.interfacenames[OpenRAVE::PT_Module].push_back("OpenraveYarpPluginLoader");
+}
+
+OPENRAVE_PLUGIN_API void DestroyPlugin()
+{
+    RAVELOG_INFO("destroying plugin\n");
+}
diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt
@@ -24,6 +24,8 @@ endif ()
     add_subdirectory(YarpOpenraveControlboard)
     add_subdirectory(YarpOpenraveControlboardCollision)
     add_subdirectory(FakeControlboard)
+    add_subdirectory(YarpOpenraveGrabber)
+    add_subdirectory(YarpOpenraveRGBDSensor)
 #  yarp_end_plugin_library(yarpplugins)
 
 #  IF (ENABLE_launchyarpPlugins)