begin OpenraveYarpPluginLoader for #47

begin OpenraveYarpPluginLoader for #47 as a copy of OpenraveYarpControlboard.
roboticslab-uc3m · Dec 3, 2017 · d3a72a4 · d3a72a4
1 parent b54adbd
commit d3a72a4
Show file tree

Hide file tree

Showing 4 changed files with 255 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -25,6 +25,7 @@ option(ENABLE_OpenraveYarpPaintSquares "Enable/disable option OpenraveYarpPaintS
 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/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,200 @@
+/**
+ * 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>
+
+using namespace std;
+using namespace OpenRAVE;
+
+//YARP_DECLARE_PLUGINS(yarpplugins)
+
+class OpenraveYarpPluginLoader : public ModuleBase
+{
+public:
+    OpenraveYarpPluginLoader(EnvironmentBasePtr penv) : ModuleBase(penv) {
+        //YARP_REGISTER_PLUGINS(yarpplugins);
+        __description = "OpenraveYarpPluginLoader plugin.";
+        RegisterCommand("open",boost::bind(&OpenraveYarpPluginLoader::Open, this,_1,_2),"opens port");
+    }
+
+    virtual ~OpenraveYarpPluginLoader() {
+        for(int i=0;i<robotDevices.size();i++)
+        {
+            robotDevices[i]->close();
+            delete robotDevices[i];
+        }
+    }
+
+    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(ostream& sout, istream& sinput)
+    {
+        bool collision = false;
+        bool alternativeRobotName = false;
+
+        vector<string> funcionArgs;
+        while(sinput)
+        {
+            string funcionArg;
+            sinput >> funcionArg;
+            funcionArgs.push_back(funcionArg);
+        }
+
+        if (funcionArgs.size() > 0)
+        {
+            if (funcionArgs[0] == "collision")
+            {
+                RAVELOG_INFO("Will open YarpOpenraveControlboardCollision");
+                collision = true;
+                if (funcionArgs.size() > 1)
+                {
+                    if( funcionArgs[1][0] == '/')
+                    {
+                        RAVELOG_INFO("Will use alternativeRobotName: %s",funcionArgs[1].c_str());
+                        alternativeRobotName = true;
+                    }
+                    else
+                    {
+                        RAVELOG_INFO("Will not use alternativeRobotName that does not begin with '/': %s",funcionArgs[1].c_str());
+                    }
+                }
+            }
+        }
+        else
+        {
+            RAVELOG_INFO("Will open YarpOpenraveControlboard");
+        }
+
+        if ( !yarp.checkNetwork() )
+        {
+            RAVELOG_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n");
+            return false;
+        }
+
+        RAVELOG_INFO("penv: %p\n",GetEnv().get());
+        OpenRAVE::EnvironmentBasePtr penv = GetEnv();
+
+        //-- Get robots
+        std::vector<OpenRAVE::RobotBasePtr> vectorOfRobotPtr;
+        GetEnv()->GetRobots(vectorOfRobotPtr);
+
+        //-- For each robot
+        for(size_t robotPtrIdx=0;robotPtrIdx<vectorOfRobotPtr.size();robotPtrIdx++)
+        {
+            RAVELOG_INFO( "Robots[%zu]: %s\n",robotPtrIdx,vectorOfRobotPtr[robotPtrIdx]->GetName().c_str());
+
+            //-- Get manipulators
+            std::vector<OpenRAVE::RobotBase::ManipulatorPtr> vectorOfManipulatorPtr = vectorOfRobotPtr[robotPtrIdx]->GetManipulators();
+
+            //-- For each manipulator
+            for(size_t manipulatorPtrIdx=0;manipulatorPtrIdx<vectorOfManipulatorPtr.size();manipulatorPtrIdx++)
+            {
+                RAVELOG_INFO( "* Manipulators[%zu]: %s\n",manipulatorPtrIdx,vectorOfManipulatorPtr[manipulatorPtrIdx]->GetName().c_str() );
+
+                //-- Formulate the manipulator port name
+                std::string manipulatorPortName;
+                if(alternativeRobotName)
+                {
+                    manipulatorPortName += funcionArgs[1];
+                }
+                else
+                {
+                    manipulatorPortName += "/";
+                    manipulatorPortName += vectorOfRobotPtr[robotPtrIdx]->GetName();
+                }
+                manipulatorPortName += "/";
+                manipulatorPortName += vectorOfManipulatorPtr[manipulatorPtrIdx]->GetName();
+                RAVELOG_INFO( "* manipulatorPortName: %s\n",manipulatorPortName.c_str() );
+
+                //--
+                yarp::dev::PolyDriver* robotDevice = new yarp::dev::PolyDriver;
+                yarp::os::Property options;
+                options.put("device","controlboardwrapper2");  //-- ports
+
+                if (collision)
+                {
+                    options.put("subdevice","YarpOpenraveControlboardCollision");
+                    std::string safe("/safe");
+                    options.put("name", safe+manipulatorPortName );
+                    options.put("remote", manipulatorPortName );
+                }
+                else
+                {
+                    options.put("subdevice","YarpOpenraveControlboard");
+                    options.put("name", manipulatorPortName );
+                }
+
+                yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr));
+                options.put("penv",v);
+
+                options.put("robotIndex",static_cast<int>(robotPtrIdx));
+                options.put("manipulatorIndex",static_cast<int>(manipulatorPtrIdx));
+
+                robotDevice->open(options);
+                if( ! robotDevice->isValid() )
+                {
+                    RAVELOG_INFO("Bad\n");
+                    return false;
+                }
+                robotDevices.push_back( robotDevice );
+            }
+        }
+        return true;
+    }
+
+private:
+    yarp::os::Network yarp;
+    std::vector< yarp::dev::PolyDriver* > robotDevices;
+};
+
+InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv) {
+    if( type == PT_Module && interfacename == "openraveyarpcontrolboard" ) {
+        return InterfaceBasePtr(new OpenraveYarpPluginLoader(penv));
+    }
+    return InterfaceBasePtr();
+}
+
+void GetPluginAttributesValidated(PLUGININFO& info) {
+    info.interfacenames[PT_Module].push_back("OpenraveYarpPluginLoader");
+}
+
+OPENRAVE_PLUGIN_API void DestroyPlugin() {
+    RAVELOG_INFO("destroying plugin\n");
+}