Merge branch '99-rf' into develop

- #99
roboticslab-uc3m · Dec 4, 2019 · 3078c0d · 3078c0d
2 parents 5520f8b + d493448
commit 3078c0d
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 8 deletions.
diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp
@@ -14,6 +14,7 @@
 #include <yarp/os/Network.h>
 #include <yarp/os/Property.h>
 #include <yarp/os/Value.h>
+#include <yarp/os/ResourceFinder.h>
 
 #include <yarp/dev/PolyDriver.h>
 
@@ -59,26 +60,62 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase
         CD_DEBUG("[%s]\n", cmd.c_str());
         std::stringstream ss(cmd);
 
-        //-- Fill openStrings
+        //-- Fill openStrings and loadString
         std::vector<std::string> openStrings;
+        std::string envString("");
+
+        enum mode { none, open, env };
+        int currentMode = mode::none;
         while( ! ss.eof() )
         {
             std::string tmp;
             ss >> tmp;
+
             if(tmp == "open")
             {
                 std::string openString("open");
                 openStrings.push_back(openString);
+                currentMode = mode::open;
+            }
+            else if(tmp == "env")
+            {
+                currentMode = mode::env;
+            }
+            else
+            {
+                if(currentMode == mode::open)
+                {
+                    openStrings[openStrings.size()-1].append(" ");
+                    openStrings[openStrings.size()-1].append(tmp);
+                }
+                else if(currentMode == mode::env)
+                {
+                    envString = tmp;
+                }
+            }
+        }
+
+        CD_DEBUG("env: '%s'\n",envString.c_str());
+
+        if(envString!="")
+        {
+            if ( !!GetEnv()->Load(envString.c_str()) )
+            {
+                CD_SUCCESS("Loaded '%s' environment.\n",envString.c_str());
             }
             else
             {
-                if(openStrings.size() == 0)
+                CD_DEBUG("Could not load '%s' environment, attempting via yarp::os::ResourceFinder.\n",envString.c_str());
+
+                yarp::os::ResourceFinder rf = yarp::os::ResourceFinder::getResourceFinderSingleton();
+                std::string fullEnvString = rf.findFileByName(envString);
+
+                if ( !GetEnv()->Load(fullEnvString.c_str()) )
                 {
-                    CD_ERROR("args must start with open, sorry! Bye!\n");
-                    return 1;
+                    CD_ERROR("Could not load '%s' environment.\n",fullEnvString.c_str());
+                    return false;
                 }
-                openStrings[openStrings.size()-1].append(" ");
-                openStrings[openStrings.size()-1].append(tmp);
+                CD_SUCCESS("Loaded '%s' environment.\n",fullEnvString.c_str());
             }
         }
 

diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/README.md b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/README.md
@@ -26,10 +26,10 @@ set pos 1 45
 
 
 ### Example 2
-We can do funky commands like the following, where `open` acts as a delimiter:
+We can do funky commands like the following, with concatenated `env` and `open` commands:
 ```bash
 # Requires [teo-openrave-models](https://github.com/roboticslab-uc3m/teo-openrave-models)
-openrave /usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml --module OpenraveYarpPluginLoader "open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 2"
+openrave --module OpenraveYarpPluginLoader "load env openrave/teo/teo.robot.xml open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 2"
 ```
 
 ## Python Invocation