From 57d0164e5d4c932439610f6e42aadf17437d48e9 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 31 Oct 2017 14:55:16 +0100 Subject: [PATCH 1/4] add some --env and --penv checks, #38 --- .../YarpOpenraveControlboard/DeviceDriverImpl.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index 0d0d5454..36a77451 100644 --- a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -6,6 +6,18 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) { + if ( ( ! config.check("env") ) && ( ! config.check("penv") ) ) + { + CD_ERROR("Please use --env or --penv parameter. Bye!\n"); + return false; + } + + if ( ( config.check("env") ) && ( config.check("penv") ) ) + { + CD_ERROR("Please do not use --env and --penv simultaneously. Bye!\n"); + return false; + } + //CD_DEBUG("penv: %p\n",*((const OpenRAVE::EnvironmentBase**)(config.find("penv").asBlob()))); penv = *((OpenRAVE::EnvironmentBasePtr*)(config.find("penv").asBlob())); From 9cf1b8b4a5775da0f726c7ee9d6a3b8c767095c2 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 31 Oct 2017 14:58:24 +0100 Subject: [PATCH 2/4] reorder code, 3 cases involving env and penv use, #38 --- .../DeviceDriverImpl.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index 36a77451..21d7d45a 100644 --- a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -6,20 +6,27 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) { - if ( ( ! config.check("env") ) && ( ! config.check("penv") ) ) + if ( ( config.check("env") ) && ( config.check("penv") ) ) { - CD_ERROR("Please use --env or --penv parameter. Bye!\n"); + CD_ERROR("Please do not use --env and --penv simultaneously. Bye!\n"); return false; } - if ( ( config.check("env") ) && ( config.check("penv") ) ) + if ( config.check("env") ) { - CD_ERROR("Please do not use --env and --penv simultaneously. Bye!\n"); + CD_DEBUG("Found env parameter. Not implemented yet. Bye!\n"); + return false; + } + 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; } - - //CD_DEBUG("penv: %p\n",*((const OpenRAVE::EnvironmentBase**)(config.find("penv").asBlob()))); - penv = *((OpenRAVE::EnvironmentBasePtr*)(config.find("penv").asBlob())); int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt(); if( robotIndex < 0 ) // a.k.a. -1 one line above From 1e19b3c5c6f366f2c403f1ff2fc6ca96cf133874 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 31 Oct 2017 15:27:32 +0100 Subject: [PATCH 3/4] decent implementation of --env --- .../DeviceDriverImpl.cpp | 57 +++++++++++++++++-- .../YarpOpenraveControlboard.hpp | 6 ++ 2 files changed, 57 insertions(+), 6 deletions(-) diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index 21d7d45a..be42afd5 100644 --- a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -2,9 +2,28 @@ #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") ) ) { @@ -14,8 +33,31 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) { if ( config.check("env") ) { - CD_DEBUG("Found env parameter. Not implemented yet. Bye!\n"); - return false; + CD_DEBUG("Found --env parameter.\n"); + + // 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()); + + // Attach a physics engine + /*if(physics=="ode"){ + penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode")); + penv->GetPhysicsEngine()->SetGravity(OpenRAVE::Vector(0,0,-9.8)); + }*/ } else if ( config.check("penv") ) { @@ -31,13 +73,13 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) { 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; } @@ -74,9 +116,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 index baacf648..696e17f5 100644 --- 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 manipulatorTargets; std::vector vectorOfJointPtr; + // Specific for --env parameter + boost::thread_group orThreads; }; } // namespace roboticslab From 8906be805b259adc43a0e67c2b2fbbf3f6d3a142 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 31 Oct 2017 15:33:40 +0100 Subject: [PATCH 4/4] remove physics code, opened issue for that --- yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index be42afd5..47967145 100644 --- a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -52,12 +52,6 @@ bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) { return false; } CD_SUCCESS("Loaded environment '%s'.\n",envFull.c_str()); - - // Attach a physics engine - /*if(physics=="ode"){ - penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode")); - penv->GetPhysicsEngine()->SetGravity(OpenRAVE::Vector(0,0,-9.8)); - }*/ } else if ( config.check("penv") ) {