Merge branch 'YarpOpenraveControlboard-env-parameter' into develop, #38

roboticslab-uc3m · Oct 31, 2017 · 8d0e4f7 · 8d0e4f7
2 parents 7b5c9c1 + 8906be8
commit 8d0e4f7
Show file tree

Hide file tree

Showing 2 changed files with 70 additions and 6 deletions.
diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp
@@ -2,23 +2,78 @@
 
 #include "YarpOpenraveControlboard.hpp"
 
+namespace roboticslab
+{
+
+// -------------------------------------------------------------------
+
+void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer)
+{
+    OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv,viewername);
+    BOOST_ASSERT(!!viewer);
+
+    // attach it to the environment:
+    penv->AddViewer(viewer);  // penv->AttachViewer(viewer);
+
+    // finally you call the viewer's infinite loop (this is why you need a separate thread):
+    bool showgui = true; // change to false to disable scene view
+    if(!_viewer) showgui = false;  // if viewer arg = 0
+    viewer->main(showgui);
+}
+
 // ------------------- DeviceDriver Related ------------------------------------
 
-bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) {
+bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) {
+
+    if ( ( config.check("env") ) && ( config.check("penv") ) )
+    {
+        CD_ERROR("Please do not use --env and --penv simultaneously. Bye!\n");
+        return false;
+    }
+
+    if ( config.check("env") )
+    {
+        CD_DEBUG("Found --env parameter.\n");
 
-    //CD_DEBUG("penv: %p\n",*((const OpenRAVE::EnvironmentBase**)(config.find("penv").asBlob())));
-    penv = *((OpenRAVE::EnvironmentBasePtr*)(config.find("penv").asBlob()));
+        // Initialize OpenRAVE-core
+        OpenRAVE::RaveInitialize(true);  // Start openrave core
+        penv = OpenRAVE::RaveCreateEnvironment();  // Create the main OpenRAVE environment, set the EnvironmentBasePtr
+        penv->StopSimulation();  // NEEDED??
+        boost::thread thviewer(boost::bind(SetViewer,penv,"qtcoin",1));
+        orThreads.add_thread(&thviewer);
+        yarp::os::Time::delay(0.4); // wait for the viewer to init, in [s]
+
+
+        // Actually load the scene
+        std::string envFull = config.find("env").asString();
+
+        if (! penv->Load(envFull.c_str()) ) {
+            CD_ERROR("Could not load '%s' environment.\n",envFull.c_str());
+            return false;
+        }
+        CD_SUCCESS("Loaded environment '%s'.\n",envFull.c_str());
+    }
+    else if ( config.check("penv") )
+    {
+        //CD_DEBUG("penv: %p\n",*((const OpenRAVE::EnvironmentBase**)(config.find("penv").asBlob())));
+        penv = *((OpenRAVE::EnvironmentBasePtr*)(config.find("penv").asBlob()));
+    }
+    else
+    {
+        CD_ERROR("Please use --env or --penv parameter. Bye!\n");
+        return false;
+    }
 
     int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt();
     if( robotIndex < 0 )  // a.k.a. -1 one line above
     {
-        CD_ERROR("Please review robotIndex.\n");
+        CD_ERROR("Please review robotIndex, currently '%d'.\n", robotIndex);
         return false;
     }
     int manipulatorIndex = config.check("manipulatorIndex",-1,"manipulatorIndex").asInt();
     if( manipulatorIndex < 0 )  // a.k.a. -1 one line above
     {
-        CD_ERROR("Please review manipulatorIndex.\n");
+        CD_ERROR("Please review manipulatorIndex, currently '%d'.\n", manipulatorIndex);
         return false;
     }
 
@@ -55,9 +110,12 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) {
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::YarpOpenraveControlboard::close() {
+bool YarpOpenraveControlboard::close() {
     printf("[YarpOpenraveControlboard] close()\n");
     return true;
 }
 
 // -----------------------------------------------------------------------------
+
+}  // namespace roboticslab
+
diff --git a/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp b/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp
@@ -23,6 +23,10 @@
 namespace roboticslab
 {
 
+
+// Specific for --env parameter
+void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer);
+
 /**
  * @ingroup TeoYarp
  * \defgroup YarpOpenraveControlboard
@@ -825,6 +829,8 @@ class YarpOpenraveControlboard : public yarp::dev::DeviceDriver, public yarp::de
                 std::vector<OpenRAVE::dReal> manipulatorTargets;
                 std::vector<OpenRAVE::RobotBase::JointPtr> vectorOfJointPtr;
 
+                // Specific for --env parameter
+                boost::thread_group orThreads;
         };
 
         }  // namespace roboticslab