diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py index 6c7d16f8..7a545710 100644 --- a/examples/openrave-YarpPluginLoader.py +++ b/examples/openrave-YarpPluginLoader.py @@ -14,7 +14,7 @@ env.Load('/usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml') OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpPluginLoader.SendCommand('open --key1 value1 --key2 value2') + print OpenraveYarpPluginLoader.SendCommand('open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 --prefix /drl') while 1: pass diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 762a0ab9..e76b53b0 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -53,12 +53,9 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase argv[i] = 0; } - for(size_t i=0;iclose(); - delete yarpPlugins[i]; - yarpPlugins[i] = 0; - } + yarpPlugin->close(); + delete yarpPlugin; + yarpPlugin = 0; } virtual void Destroy() @@ -98,63 +95,78 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase argv.push_back(cstr); } - for(size_t i=0;i vectorOfRobotPtr; - GetEnv()->GetRobots(vectorOfRobotPtr); - - //-- For each robot - for(size_t robotPtrIdx=0;robotPtrIdxGetName().c_str()); - - //-- Get manipulators - std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[robotPtrIdx]->GetManipulators(); - - //-- For each manipulator - for(size_t manipulatorPtrIdx=0;manipulatorPtrIdxGetName().c_str() ); + std::string name; - //-- Formulate the manipulator port name - std::string manipulatorPortName; + if( options.check("prefix") ) // Note that not taken into account if using "name" + { + name += options.find("prefix").asString(); + } - manipulatorPortName += "/"; - manipulatorPortName += vectorOfManipulatorPtr[manipulatorPtrIdx]->GetName(); - RAVELOG_INFO( "* manipulatorPortName: %s\n",manipulatorPortName.c_str() ); + name += "/"; + int robotPtrIdx = options.find("robotIndex").asInt(); - //-- - yarp::dev::PolyDriver* robotDevice = new yarp::dev::PolyDriver; - yarp::os::Property options; - options.put("device","controlboardwrapper2"); //-- ports - options.put("subdevice","YarpOpenraveControlboard"); - options.put("name", manipulatorPortName ); + std::vector vectorOfRobotPtr; + GetEnv()->GetRobots(vectorOfRobotPtr); - yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr)); - options.put("penv",v); - options.put("robotIndex",static_cast(robotPtrIdx)); - options.put("manipulatorIndex",static_cast(manipulatorPtrIdx)); + if(robotPtrIdx >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size()); + return false; + } - robotDevice->open(options); + name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); - if( ! robotDevice->isValid() ) + if( options.check("manipulatorIndex") ) { - RAVELOG_INFO("Bad\n"); - return false; + int manipulatorPtrIdx = options.find("manipulatorIndex").asInt(); + + std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[ robotPtrIdx ]->GetManipulators(); + + if(manipulatorPtrIdx >= vectorOfManipulatorPtr.size()) + { + CD_ERROR("manipulatorPtrIdx %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorPtrIdx,vectorOfManipulatorPtr.size()); + return false; + } + + name += "/"; + name += vectorOfManipulatorPtr[ manipulatorPtrIdx ]->GetName(); } - yarpPlugins.push_back( robotDevice ); + + 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; } @@ -162,7 +174,7 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase std::vector argv; yarp::os::Network yarp; - std::vector< yarp::dev::PolyDriver* > yarpPlugins; + yarp::dev::PolyDriver* yarpPlugin; }; OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv)