From d3a72a4c502d6274c27455f8a5a088a073cdae85 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:25:31 +0100 Subject: [PATCH 01/70] begin OpenraveYarpPluginLoader for #47 begin OpenraveYarpPluginLoader for #47 as a copy of OpenraveYarpControlboard. --- CMakeLists.txt | 1 + libraries/OpenravePlugins/CMakeLists.txt | 1 + .../OpenraveYarpPluginLoader/CMakeLists.txt | 53 +++++ .../OpenraveYarpPluginLoader.cpp | 200 ++++++++++++++++++ 4 files changed, 255 insertions(+) create mode 100644 libraries/OpenravePlugins/OpenraveYarpPluginLoader/CMakeLists.txt create mode 100644 libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f21a49f5..4678833f 100644 --- 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 index 795273cc..a65347b2 100644 --- 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 new file mode 100644 index 00000000..19671a35 --- /dev/null +++ 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 new file mode 100644 index 00000000..75b4683e --- /dev/null +++ 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 + Full Example Code: + */ +#include +#include +#include + +#include +#include + +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;iclose(); + 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 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 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() ); + + //-- 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(robotPtrIdx)); + options.put("manipulatorIndex",static_cast(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"); +} From 42f7392db717dd0f33ea16eb85b41695943a0d3a Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:38:01 +0100 Subject: [PATCH 02/70] remove ad-hoc code, improve indentation, refractor robotDevices->yarpPlugins --- .../OpenraveYarpPluginLoader.cpp | 78 ++++++------------- 1 file changed, 22 insertions(+), 56 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 75b4683e..6205fb49 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -40,17 +40,19 @@ using namespace OpenRAVE; class OpenraveYarpPluginLoader : public ModuleBase { public: - OpenraveYarpPluginLoader(EnvironmentBasePtr penv) : ModuleBase(penv) { + 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;iclose(); - delete robotDevices[i]; + yarpPlugins[i]->close(); + delete yarpPlugins[i]; } } @@ -66,9 +68,6 @@ class OpenraveYarpPluginLoader : public ModuleBase bool Open(ostream& sout, istream& sinput) { - bool collision = false; - bool alternativeRobotName = false; - vector funcionArgs; while(sinput) { @@ -81,28 +80,10 @@ class OpenraveYarpPluginLoader : public ModuleBase { 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() ) + if ( ! yarp.checkNetwork() ) { RAVELOG_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n"); return false; @@ -130,15 +111,7 @@ class OpenraveYarpPluginLoader : public ModuleBase //-- 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() ); @@ -147,33 +120,22 @@ class OpenraveYarpPluginLoader : public ModuleBase 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 ); - } + 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(robotPtrIdx)); options.put("manipulatorIndex",static_cast(manipulatorPtrIdx)); robotDevice->open(options); + if( ! robotDevice->isValid() ) { RAVELOG_INFO("Bad\n"); return false; } - robotDevices.push_back( robotDevice ); + yarpPlugins.push_back( robotDevice ); } } return true; @@ -181,20 +143,24 @@ class OpenraveYarpPluginLoader : public ModuleBase private: yarp::os::Network yarp; - std::vector< yarp::dev::PolyDriver* > robotDevices; + std::vector< yarp::dev::PolyDriver* > yarpPlugins; }; -InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv) { - if( type == PT_Module && interfacename == "openraveyarpcontrolboard" ) { +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) { +void GetPluginAttributesValidated(PLUGININFO& info) +{ info.interfacenames[PT_Module].push_back("OpenraveYarpPluginLoader"); } -OPENRAVE_PLUGIN_API void DestroyPlugin() { +OPENRAVE_PLUGIN_API void DestroyPlugin() +{ RAVELOG_INFO("destroying plugin\n"); } From 5e49b61fa3abbe8a4b72b56df3c3d35b08c4466e Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:39:03 +0100 Subject: [PATCH 03/70] remove very obsolete lines --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 6205fb49..9b79a9a5 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -35,14 +35,11 @@ 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"); } From a9f672638f73424c5a6b559d12ee13b3471e3a94 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:39:52 +0100 Subject: [PATCH 04/70] fix style --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 9b79a9a5..6a8eda1b 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -53,12 +53,13 @@ class OpenraveYarpPluginLoader : public ModuleBase } } - virtual void Destroy() { - + virtual void Destroy() + { RAVELOG_INFO("module unloaded from environment\n"); } - /*int main(const string& cmd) { + /*int main(const string& cmd) + { RAVELOG_INFO("module initialized cmd; %s\n", cmd.c_str()); return 0; }*/ From 5e9be57a45ac4e9bbb2ada45f6549bd0fabe4bf4 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:41:45 +0100 Subject: [PATCH 05/70] drop using std --- .../OpenraveYarpPluginLoader.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 6a8eda1b..48957070 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -32,7 +32,6 @@ #include #include -using namespace std; using namespace OpenRAVE; class OpenraveYarpPluginLoader : public ModuleBase @@ -64,23 +63,16 @@ class OpenraveYarpPluginLoader : public ModuleBase return 0; }*/ - bool Open(ostream& sout, istream& sinput) + bool Open(std::ostream& sout, std::istream& sinput) { - vector funcionArgs; + std::vector funcionArgs; while(sinput) { - string funcionArg; + std::string funcionArg; sinput >> funcionArg; funcionArgs.push_back(funcionArg); } - if (funcionArgs.size() > 0) - { - if (funcionArgs[0] == "collision") - { - } - } - if ( ! yarp.checkNetwork() ) { RAVELOG_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n"); From ed8671aedbe19b8d0a9a31ae2c3082fc95767b6f Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:44:15 +0100 Subject: [PATCH 06/70] drop using OpenRAVE --- .../OpenraveYarpPluginLoader.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 48957070..004f3571 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -32,12 +32,10 @@ #include #include -using namespace OpenRAVE; - -class OpenraveYarpPluginLoader : public ModuleBase +class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase { public: - OpenraveYarpPluginLoader(EnvironmentBasePtr penv) : ModuleBase(penv) + OpenraveYarpPluginLoader(OpenRAVE::EnvironmentBasePtr penv) : OpenRAVE::ModuleBase(penv) { __description = "OpenraveYarpPluginLoader plugin."; RegisterCommand("open",boost::bind(&OpenraveYarpPluginLoader::Open, this,_1,_2),"opens port"); @@ -136,18 +134,18 @@ class OpenraveYarpPluginLoader : public ModuleBase std::vector< yarp::dev::PolyDriver* > yarpPlugins; }; -InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv) +OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv) { - if( type == PT_Module && interfacename == "openraveyarpcontrolboard" ) + if( type == OpenRAVE::PT_Module && interfacename == "openraveyarpcontrolboard" ) { - return InterfaceBasePtr(new OpenraveYarpPluginLoader(penv)); + return OpenRAVE::InterfaceBasePtr(new OpenraveYarpPluginLoader(penv)); } - return InterfaceBasePtr(); + return OpenRAVE::InterfaceBasePtr(); } -void GetPluginAttributesValidated(PLUGININFO& info) +void GetPluginAttributesValidated(OpenRAVE::PLUGININFO& info) { - info.interfacenames[PT_Module].push_back("OpenraveYarpPluginLoader"); + info.interfacenames[OpenRAVE::PT_Module].push_back("OpenraveYarpPluginLoader"); } OPENRAVE_PLUGIN_API void DestroyPlugin() From 6a659bd971cea31e573b824fb5a8920c7f6411f6 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:45:49 +0100 Subject: [PATCH 07/70] replace vector -> Bottle for parsing --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 004f3571..60459056 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -63,12 +63,13 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase bool Open(std::ostream& sout, std::istream& sinput) { - std::vector funcionArgs; + yarp::os::Bottle funcionArgs; + while(sinput) { std::string funcionArg; sinput >> funcionArg; - funcionArgs.push_back(funcionArg); + funcionArgs.addString(funcionArg); } if ( ! yarp.checkNetwork() ) From d2f5d1713aed6db26b74535c985ff19d60d0c96c Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:46:36 +0100 Subject: [PATCH 08/70] move yarp.checkNetwork up --- .../OpenraveYarpPluginLoader.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 60459056..7b9737b7 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -63,6 +63,12 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase bool Open(std::ostream& sout, std::istream& sinput) { + if ( ! yarp.checkNetwork() ) + { + RAVELOG_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n"); + return false; + } + yarp::os::Bottle funcionArgs; while(sinput) @@ -72,11 +78,6 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase funcionArgs.addString(funcionArg); } - 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(); From 7f7f2451368ed6bd392b5d601e45a78da823de7f Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 12:50:45 +0100 Subject: [PATCH 09/70] add ColorDebug.hpp --- .../OpenraveYarpPluginLoader.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 7b9737b7..8c359a80 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -32,6 +32,8 @@ #include #include +#include "ColorDebug.hpp" + class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase { public: @@ -65,19 +67,20 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase { if ( ! yarp.checkNetwork() ) { - RAVELOG_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n"); + CD_ERROR("Found no yarp network (try running \"yarpserver &\"), bye!\n"); return false; } - yarp::os::Bottle funcionArgs; + yarp::os::Bottle config; while(sinput) { std::string funcionArg; sinput >> funcionArg; - funcionArgs.addString(funcionArg); + config.addString(funcionArg); } + CD_DEBUG("config: %s\n", config.toString().c_str()); RAVELOG_INFO("penv: %p\n",GetEnv().get()); OpenRAVE::EnvironmentBasePtr penv = GetEnv(); From ed19af1d5842526378884aa78a532b91dd855763 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 13:03:22 +0100 Subject: [PATCH 10/70] add openrave-YarpPluginLoader.py, not working yet, for #47 --- examples/openrave-YarpPluginLoader.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 examples/openrave-YarpPluginLoader.py diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py new file mode 100644 index 00000000..ccf43d66 --- /dev/null +++ b/examples/openrave-YarpPluginLoader.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import openravepy +from openravepy import * + +try: + RaveInitialize() + + if not RaveLoadPlugin('OpenraveYarpPluginLoader'): + raveLogError("Plugin not correctly loaded") + + env=Environment() + env.SetViewer('qtcoin') + env.Load('/usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml') + + OpenraveYarpControlboard = RaveCreateModule(env,'OpenraveYarpPluginLoader') + print OpenraveYarpControlboard.SendCommand('open') + + while 1: + pass + +finally: + RaveDestroy() + From b89e4a3e798c1e1c3a3ad300ad49197915e2c045 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 13:11:53 +0100 Subject: [PATCH 11/70] fix interfacename so OpenRAVE can find openraveyarppluginloader, #47 --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 8c359a80..e57956ae 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -141,7 +141,7 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv) { - if( type == OpenRAVE::PT_Module && interfacename == "openraveyarpcontrolboard" ) + if( type == OpenRAVE::PT_Module && interfacename == "openraveyarppluginloader") { return OpenRAVE::InterfaceBasePtr(new OpenraveYarpPluginLoader(penv)); } From 0c3bc02e031d48dd4fbb0674e2e09507cec781ea Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 17:10:19 +0100 Subject: [PATCH 12/70] add CD_INFO on yarp network check --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index e57956ae..a9e7134c 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -65,11 +65,13 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase bool Open(std::ostream& sout, std::istream& sinput) { + CD_INFO("Checking for yarp network...\n"); if ( ! yarp.checkNetwork() ) { CD_ERROR("Found no yarp network (try running \"yarpserver &\"), bye!\n"); return false; } + CD_SUCCESS("Found yarp network.\n"); yarp::os::Bottle config; From 0589735917cfa5127b4c869901e2f1838acaaad1 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 17:16:32 +0100 Subject: [PATCH 13/70] add parsable values --- examples/openrave-YarpPluginLoader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py index ccf43d66..3db7f353 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') OpenraveYarpControlboard = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpControlboard.SendCommand('open') + print OpenraveYarpControlboard.SendCommand('open --key1 value1 --key2 value2') while 1: pass From 09c09b5cb72aa14e8bf7d342de50579d27043d67 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 17:17:12 +0100 Subject: [PATCH 14/70] improve naming --- examples/openrave-YarpPluginLoader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py index 3db7f353..6c7d16f8 100644 --- a/examples/openrave-YarpPluginLoader.py +++ b/examples/openrave-YarpPluginLoader.py @@ -13,8 +13,8 @@ env.SetViewer('qtcoin') env.Load('/usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml') - OpenraveYarpControlboard = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpControlboard.SendCommand('open --key1 value1 --key2 value2') + OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') + print OpenraveYarpPluginLoader.SendCommand('open --key1 value1 --key2 value2') while 1: pass From 1976d14d9509bfb344491563b3ce59a2e1006c95 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 18:10:26 +0100 Subject: [PATCH 15/70] Property using input in argv format --- .../OpenraveYarpPluginLoader.cpp | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index a9e7134c..d4e6df7a 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -73,16 +73,36 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase } CD_SUCCESS("Found yarp network.\n"); - yarp::os::Bottle config; + std::vector argv; + char* dummyProgramName = "dummyProgramName"; + argv.push_back(dummyProgramName); while(sinput) { - std::string funcionArg; - sinput >> funcionArg; - config.addString(funcionArg); + std::string str; + sinput >> str; + if(str.length() == 0) + { + //CD_WARNING("Omit empty\n"); + continue; + } + char *cstr = new char[str.length() + 1]; + strcpy(cstr, str.c_str()); + argv.push_back(cstr); + CD_DEBUG("Here [%s]\n",cstr); + } + + for(int i=0;i Date: Sun, 3 Dec 2017 19:10:12 +0100 Subject: [PATCH 16/70] argv as part of class for destruction at end, style improvements --- .../OpenraveYarpPluginLoader.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index d4e6df7a..2b002503 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -50,6 +50,13 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase yarpPlugins[i]->close(); delete yarpPlugins[i]; } + + for(int i=0;i argv; char* dummyProgramName = "dummyProgramName"; argv.push_back(dummyProgramName); @@ -83,24 +89,21 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase sinput >> str; if(str.length() == 0) { - //CD_WARNING("Omit empty\n"); + //CD_WARNING("Omit empty string that is usually at end via openrave.\n"); continue; } - char *cstr = new char[str.length() + 1]; + char *cstr = new char[str.length() + 1]; // pushed to member argv to be deleted in ~. strcpy(cstr, str.c_str()); argv.push_back(cstr); - CD_DEBUG("Here [%s]\n",cstr); } - for(int i=0;i argv; + yarp::os::Network yarp; std::vector< yarp::dev::PolyDriver* > yarpPlugins; }; From 3211df59f0a553ff1ae895683e5d904572bc656f Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 19:29:02 +0100 Subject: [PATCH 17/70] avoid segfault --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 2b002503..e99ea457 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -51,7 +51,8 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase delete yarpPlugins[i]; } - for(int i=0;i Date: Sun, 3 Dec 2017 19:30:21 +0100 Subject: [PATCH 18/70] move freeing up --- .../OpenraveYarpPluginLoader.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index e99ea457..a602d9ba 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -45,12 +45,6 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase virtual ~OpenraveYarpPluginLoader() { - for(int i=0;iclose(); - delete yarpPlugins[i]; - } - //-- Note that we start on element 1, first elem was not via new!! for(int i=1;iclose(); + delete yarpPlugins[i]; + } } virtual void Destroy() From 2072cc3e6cdab1c8da3db8e21198ed1d7c077999 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 19:31:05 +0100 Subject: [PATCH 19/70] more explicit clean --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index a602d9ba..33c7376f 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -48,7 +48,7 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase //-- Note that we start on element 1, first elem was not via new!! for(int i=1;i Date: Sun, 3 Dec 2017 19:31:43 +0100 Subject: [PATCH 20/70] comment out comment on explicit clean --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 33c7376f..5b69c657 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -48,7 +48,7 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase //-- Note that we start on element 1, first elem was not via new!! for(int i=1;i Date: Sun, 3 Dec 2017 19:32:39 +0100 Subject: [PATCH 21/70] enable rest of plugin --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 5b69c657..1685cb99 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -106,8 +106,6 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase CD_DEBUG("config: %s\n", options.toString().c_str()); - return true; - RAVELOG_INFO("penv: %p\n",GetEnv().get()); OpenRAVE::EnvironmentBasePtr penv = GetEnv(); From 1aeaccbb4cd37571d4e3fe41a5345cae984349fd Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 19:37:08 +0100 Subject: [PATCH 22/70] improve code comments --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 1685cb99..e2e19af9 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -81,6 +81,8 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase } CD_SUCCESS("Found yarp network.\n"); + //-- Given "std::istream& sinput", create equivalent to "int argc, char *argv[]" + //-- Note that char* != const char* given by std::string::c_str(); char* dummyProgramName = "dummyProgramName"; argv.push_back(dummyProgramName); @@ -88,11 +90,8 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase { std::string str; sinput >> str; - if(str.length() == 0) - { - //CD_WARNING("Omit empty string that is usually at end via openrave.\n"); + if(str.length() == 0) //-- Omits empty string that is usually at end via openrave. continue; - } char *cstr = new char[str.length() + 1]; // pushed to member argv to be deleted in ~. strcpy(cstr, str.c_str()); argv.push_back(cstr); From 211f71c4f5b4fd51259d4aeec2782a6e15589ab3 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 19:39:09 +0100 Subject: [PATCH 23/70] int->size_t --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index e2e19af9..3f38fd4b 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -46,14 +46,14 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase virtual ~OpenraveYarpPluginLoader() { //-- Note that we start on element 1, first elem was not via new!! - for(int i=1;iclose(); delete yarpPlugins[i]; From d287befb694b67d25b6c991f9af6a7b3cfd36565 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 19:40:45 +0100 Subject: [PATCH 24/70] incorporate best practices --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 3f38fd4b..762a0ab9 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -57,6 +57,7 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase { yarpPlugins[i]->close(); delete yarpPlugins[i]; + yarpPlugins[i] = 0; } } From 8321473406e16e42583954881f68be375b5fd012 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 22:23:48 +0100 Subject: [PATCH 25/70] comment out debug lines --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 762a0ab9..7b4b30de 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -98,8 +98,8 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase argv.push_back(cstr); } - for(size_t i=0;i Date: Sun, 3 Dec 2017 22:40:05 +0100 Subject: [PATCH 26/70] leave single device implementation that only passes options and penv --- examples/openrave-YarpPluginLoader.py | 2 +- .../OpenraveYarpPluginLoader.cpp | 65 +++++-------------- 2 files changed, 16 insertions(+), 51 deletions(-) diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py index 6c7d16f8..e4aef5fe 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') while 1: pass diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 7b4b30de..6f8228f8 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() @@ -109,52 +106,20 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase RAVELOG_INFO("penv: %p\n",GetEnv().get()); OpenRAVE::EnvironmentBasePtr penv = GetEnv(); - //-- Get robots - std::vector vectorOfRobotPtr; - GetEnv()->GetRobots(vectorOfRobotPtr); + yarpPlugin = new yarp::dev::PolyDriver; + + yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr)); + options.put("penv",v); - //-- For each robot - for(size_t robotPtrIdx=0;robotPtrIdxopen(options); + + if( ! yarpPlugin->isValid() ) { - RAVELOG_INFO( "Robots[%zu]: %s\n",robotPtrIdx,vectorOfRobotPtr[robotPtrIdx]->GetName().c_str()); - - //-- Get manipulators - std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[robotPtrIdx]->GetManipulators(); - - //-- For each manipulator - for(size_t manipulatorPtrIdx=0;manipulatorPtrIdxGetName().c_str() ); - - //-- Formulate the manipulator port name - std::string manipulatorPortName; - - 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 - 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(robotPtrIdx)); - options.put("manipulatorIndex",static_cast(manipulatorPtrIdx)); - - robotDevice->open(options); - - if( ! robotDevice->isValid() ) - { - RAVELOG_INFO("Bad\n"); - return false; - } - yarpPlugins.push_back( robotDevice ); - } + CD_ERROR("Bad\n"); + return false; } + CD_SUCCESS("Good\n"); + return true; } @@ -162,7 +127,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) From b752e7d9772878d1af2ba6e1e13b57ab9eaf9657 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 22:42:13 +0100 Subject: [PATCH 27/70] move driver creation down --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 6f8228f8..e33d0b2a 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -106,11 +106,10 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase RAVELOG_INFO("penv: %p\n",GetEnv().get()); OpenRAVE::EnvironmentBasePtr penv = GetEnv(); - yarpPlugin = new yarp::dev::PolyDriver; - yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr)); options.put("penv",v); + yarpPlugin = new yarp::dev::PolyDriver; yarpPlugin->open(options); if( ! yarpPlugin->isValid() ) From 4c077b8d2634aa0e1f295739360fd105a1e12967 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 22:55:41 +0100 Subject: [PATCH 28/70] nice name getting --- .../OpenraveYarpPluginLoader.cpp | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index e33d0b2a..a7b7d236 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -103,12 +103,40 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase CD_DEBUG("config: %s\n", options.toString().c_str()); - RAVELOG_INFO("penv: %p\n",GetEnv().get()); + //-- Get and put pointer to environment + CD_INFO("penv: %p\n",GetEnv().get()); OpenRAVE::EnvironmentBasePtr penv = GetEnv(); - yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr)); options.put("penv",v); + //-- If robotIndex (and then if manipulatorIndex), get and put name + if( options.check("robotIndex") ) + { + std::string name("/"); + + int robotPtrIdx = options.find("robotIndex").asInt(); + + std::vector vectorOfRobotPtr; + GetEnv()->GetRobots(vectorOfRobotPtr); + + name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); + + if( options.check("manipulatorIndex") ) + { + int manipulatorPtrIdx = options.find("manipulatorIndex").asInt(); + + std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[robotPtrIdx]->GetManipulators(); + vectorOfRobotPtr[ robotPtrIdx ]->GetManipulators(); + + name += "/"; + name += vectorOfManipulatorPtr[ manipulatorPtrIdx ]->GetName(); + } + + options.put("name",name); + } + + CD_DEBUG("post-config: %s\n", options.toString().c_str()); + yarpPlugin = new yarp::dev::PolyDriver; yarpPlugin->open(options); From c8db78c6a11c87851937816ff6c7380a309aeae7 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 23:07:11 +0100 Subject: [PATCH 29/70] incorporate boundary checks for robotIndex and manipulatorIndex --- .../OpenraveYarpPluginLoader.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index a7b7d236..5b6e13a7 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -119,14 +119,25 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase std::vector vectorOfRobotPtr; GetEnv()->GetRobots(vectorOfRobotPtr); + if(robotPtrIdx >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size()); + return false; + } + name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); if( options.check("manipulatorIndex") ) { int manipulatorPtrIdx = options.find("manipulatorIndex").asInt(); - std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[robotPtrIdx]->GetManipulators(); - vectorOfRobotPtr[ robotPtrIdx ]->GetManipulators(); + 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(); @@ -142,10 +153,10 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase if( ! yarpPlugin->isValid() ) { - CD_ERROR("Bad\n"); + CD_ERROR("yarpPlugin not valid.\n"); return false; } - CD_SUCCESS("Good\n"); + CD_SUCCESS("Valid yarpPlugin.\n"); return true; } From 3c4544982fadc8e4d7b25f723f370a6598f42b6b Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 23:11:18 +0100 Subject: [PATCH 30/70] enable prefix option --- examples/openrave-YarpPluginLoader.py | 2 +- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader.py index e4aef5fe..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 --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0') + 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 5b6e13a7..3f930e42 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -112,8 +112,14 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase //-- If robotIndex (and then if manipulatorIndex), get and put name if( options.check("robotIndex") ) { - std::string name("/"); + std::string name; + if( options.check("prefix") ) + { + name += options.find("prefix").asString(); + } + + name += "/"; int robotPtrIdx = options.find("robotIndex").asInt(); std::vector vectorOfRobotPtr; From d0c91e1f90fc91d838c268e14bb9f6990de6ddb9 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 23:15:39 +0100 Subject: [PATCH 31/70] enable bypass if 'name' exists --- .../OpenraveYarpPluginLoader.cpp | 61 ++++++++++--------- 1 file changed, 32 insertions(+), 29 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index 3f930e42..e76b53b0 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -109,47 +109,50 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase yarp::os::Value v(&penv, sizeof(OpenRAVE::EnvironmentBasePtr)); options.put("penv",v); - //-- If robotIndex (and then if manipulatorIndex), get and put name - if( options.check("robotIndex") ) + if( ! options.check("name") ) // Enable bypass if "name" already exists { - std::string name; - - if( options.check("prefix") ) + //-- If robotIndex (and then if manipulatorIndex), get and put name + if( options.check("robotIndex") ) { - name += options.find("prefix").asString(); - } - - name += "/"; - int robotPtrIdx = options.find("robotIndex").asInt(); + std::string name; - std::vector vectorOfRobotPtr; - GetEnv()->GetRobots(vectorOfRobotPtr); + if( options.check("prefix") ) // Note that not taken into account if using "name" + { + name += options.find("prefix").asString(); + } - if(robotPtrIdx >= vectorOfRobotPtr.size()) - { - CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size()); - return false; - } + name += "/"; + int robotPtrIdx = options.find("robotIndex").asInt(); - name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); + std::vector vectorOfRobotPtr; + GetEnv()->GetRobots(vectorOfRobotPtr); - if( options.check("manipulatorIndex") ) - { - int manipulatorPtrIdx = options.find("manipulatorIndex").asInt(); + if(robotPtrIdx >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size()); + return false; + } - std::vector vectorOfManipulatorPtr = vectorOfRobotPtr[ robotPtrIdx ]->GetManipulators(); + name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); - if(manipulatorPtrIdx >= vectorOfManipulatorPtr.size()) + if( options.check("manipulatorIndex") ) { - CD_ERROR("manipulatorPtrIdx %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorPtrIdx,vectorOfManipulatorPtr.size()); - 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(); } - name += "/"; - name += vectorOfManipulatorPtr[ manipulatorPtrIdx ]->GetName(); + options.put("name",name); } - - options.put("name",name); } CD_DEBUG("post-config: %s\n", options.toString().c_str()); From 673ae9839c4a993b099d8c7b52a0c7c84eb4c335 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 23:52:45 +0100 Subject: [PATCH 32/70] mv openrave-YarpPluginLoader.py openrave-YarpPluginLoader-controlboard.py --- ...pPluginLoader.py => openrave-YarpPluginLoader-controlboard.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{openrave-YarpPluginLoader.py => openrave-YarpPluginLoader-controlboard.py} (100%) diff --git a/examples/openrave-YarpPluginLoader.py b/examples/openrave-YarpPluginLoader-controlboard.py similarity index 100% rename from examples/openrave-YarpPluginLoader.py rename to examples/openrave-YarpPluginLoader-controlboard.py From 496e44f721f7c181932f8c27dd9ca6dc7c11ccc9 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Sun, 3 Dec 2017 23:57:55 +0100 Subject: [PATCH 33/70] add openrave-YarpPluginLoader-grabber.py --- examples/openrave-YarpPluginLoader-grabber.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 examples/openrave-YarpPluginLoader-grabber.py diff --git a/examples/openrave-YarpPluginLoader-grabber.py b/examples/openrave-YarpPluginLoader-grabber.py new file mode 100644 index 00000000..245109fd --- /dev/null +++ b/examples/openrave-YarpPluginLoader-grabber.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import openravepy +from openravepy import * + +try: + RaveInitialize() + + if not RaveLoadPlugin('OpenraveYarpPluginLoader'): + raveLogError("Plugin not correctly loaded") + + env=Environment() + env.SetViewer('qtcoin') + env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') + + OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') + print OpenraveYarpPluginLoader.SendCommand('open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 --prefix /drl') + + while 1: + pass + +finally: + RaveDestroy() + From d94e8271f7213f30bb09829ec8541d4c99fe7c3b Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 00:25:56 +0100 Subject: [PATCH 34/70] begin YarpOpenraveGrabber --- CMakeLists.txt | 1 + libraries/YarpPlugins/CMakeLists.txt | 1 + .../YarpOpenraveGrabber/CMakeLists.txt | 50 ++ .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 257 ++++++ .../IFrameGrabberImageImpl.cpp | 40 + .../YarpPlugins/YarpOpenraveGrabber/README.md | 3 + .../YarpOpenraveGrabber.hpp | 835 ++++++++++++++++++ .../YarpOpenraveGrabber.ini | 7 + 8 files changed, 1194 insertions(+) create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/README.md create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp create mode 100644 libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini diff --git a/CMakeLists.txt b/CMakeLists.txt index 4678833f..451be7c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ set(BUILD_SHARED_LIBS TRUE CACHE INTERNAL "hide this!") option(ENABLE_FakeControlboard "Enable/disable compilation of FakeControlboard" TRUE) option(ENABLE_YarpOpenraveControlboard "Enable/disable option YarpOpenraveControlboard" TRUE) option(ENABLE_YarpOpenraveControlboardCollision "Enable/disable option YarpOpenraveControlboardCollision" TRUE) +option(ENABLE_YarpOpenraveGrabber "Enable/disable option YarpOpenraveGrabber" TRUE) ### options: cpp libraries (openrave plugins) option(ENABLE_OpenraveYarpControlboard "Enable/disable option OpenraveYarpControlboard" TRUE) diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt index a6cd606d..b3298c1f 100644 --- a/libraries/YarpPlugins/CMakeLists.txt +++ b/libraries/YarpPlugins/CMakeLists.txt @@ -24,6 +24,7 @@ endif () add_subdirectory(YarpOpenraveControlboard) add_subdirectory(YarpOpenraveControlboardCollision) add_subdirectory(FakeControlboard) + add_subdirectory(YarpOpenraveGrabber) # yarp_end_plugin_library(yarpplugins) # IF (ENABLE_launchyarpPlugins) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt new file mode 100644 index 00000000..e3996a70 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt @@ -0,0 +1,50 @@ +# Copyright: (C) 2013 Universidad Carlos III de Madrid +# Author: Juan G. Victores + +yarp_prepare_plugin(YarpOpenraveGrabber + CATEGORY device + TYPE roboticslab::YarpOpenraveGrabber + INCLUDE YarpOpenraveGrabber.hpp + WRAPPER grabberwrapper2) + +IF (NOT SKIP_YarpOpenraveGrabber) + +SET(CMAKE_MODULE_PATH ${TEO_MODULE_PATH} ${CMAKE_MODULE_PATH}) +FIND_PACKAGE(YARP REQUIRED) +FIND_PACKAGE(OpenRAVE REQUIRED) +find_package(Boost COMPONENTS iostreams python thread system) + +if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX ) + add_definitions("-fno-strict-aliasing -Wall") +endif( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX ) + +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR} ${OpenRAVE_INCLUDE_DIRS}) # kdl name depends on version +if( Boost_INCLUDE_DIRS ) + include_directories(${Boost_INCLUDE_DIRS}) +endif() + +LINK_DIRECTORIES(${OPENRAVE_YARP_PLUGINS_LINK_DIRS}) + +YARP_ADD_PLUGIN(YarpOpenraveGrabber YarpOpenraveGrabber.hpp DeviceDriverImpl.cpp IFrameGrabberImageImpl.cpp) +add_dependencies(YarpOpenraveGrabber COLOR_DEBUG) +set_target_properties(${KEYWORD} PROPERTIES COMPILE_FLAGS "${OpenRAVE_CXXFLAGS}") +set_target_properties(${KEYWORD} PROPERTIES LINK_FLAGS "${OpenRAVE_LINK_FLAGS}") +TARGET_LINK_LIBRARIES(YarpOpenraveGrabber ${OpenRAVE_LIBRARIES} ${OpenRAVE_CORE_LIBRARIES} ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${YARP_LIBRARIES}) + +# Exporting dependencies for TEOConfig.cmake quite manually for now... +set(TEO_INCLUDE_DIRS ${TEO_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} CACHE INTERNAL "appended libraries") +set(TEO_LIBRARIES ${TEO_LIBRARIES} TeoYarp YarpOpenraveGrabber CACHE INTERNAL "appended libraries") + + #install(TARGETS YarpOpenraveGrabber DESTINATION lib) + yarp_install(TARGETS YarpOpenraveGrabber + EXPORT YARP + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) + + yarp_install(FILES YarpOpenraveGrabber.ini + COMPONENT runtime + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + +ENDIF (NOT SKIP_YarpOpenraveGrabber) + diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp new file mode 100644 index 00000000..78059250 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -0,0 +1,257 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveGrabber.hpp" + +#include // Thanks: https://notfaq.wordpress.com/2007/08/04/cc-convert-string-to-upperlower-case/ + +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 YarpOpenraveGrabber::open(yarp::os::Searchable& config) { + + CD_DEBUG("config: %s\n",config.toString().c_str()); + + double genRefSpeed = config.check("genRefSpeed",DEFAULT_GEN_REF_SPEED,"general ref speed").asDouble(); + + 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"); + + // Initialize OpenRAVE-core + OpenRAVE::RaveInitialize(true); // Start openrave core + penv = OpenRAVE::RaveCreateEnvironment(); // Create the main OpenRAVE environment, set the EnvironmentBasePtr + penv->StopSimulation(); // NEEDED?? + + if ( config.check("view") ) + { + boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin",1)); + openraveThreads.add_thread(&openraveViewerThread); + 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, 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, currently '%d'.\n", manipulatorIndex); + return false; + } + + if( config.check("orPlugin") ) + { + CD_DEBUG("Found --orPlugin parameter.\n"); + + std::string orPluginAndModuleName = config.find("orPlugin").asString(); + if ( ! OpenRAVE::RaveLoadPlugin(orPluginAndModuleName) ) + { + CD_ERROR("Could not load plugin '%s'\n",orPluginAndModuleName.c_str()); + return false; + } + OpenRAVE::ModuleBasePtr pModule = OpenRAVE::RaveCreateModule(penv,orPluginAndModuleName); // create the module + penv->Add(pModule,true); // load the module, calls main and also enables good destroy. + std::stringstream cmdin,cmdout; + cmdin << "open"; + // RAVELOG_INFO("%s\n",cmdin.str().c_str()); + if( ! pModule->SendCommand(cmdout,cmdin) ) + { + CD_ERROR("Bad send 'open' command.\n"); + } + CD_SUCCESS("Sent 'open' command.\n"); + } + + if( config.check("orPlugins") ) + { + CD_DEBUG("Found --orPlugins parameter.\n"); + + if( ! config.find("orPlugins").isList() ) + { + CD_ERROR("orPlugins usage from CLI: (read code) ->. Bye!\n"); //--orPlugins "(plugin1 (module module1) (commands \"open port\"))" + return false; + } + yarp::os::Bottle orPlugins = config.findGroup("orPlugins"); + CD_DEBUG("orPlugins: %s\n",orPlugins.toString().c_str()); + for(int i=1; i. Bye!\n"); //--orPlugins "(plugin1 (module module1) (commands \"open port\"))" + return false; + } + yarp::os::Bottle* orPlugin = orPlugins.get(i).asList(); + CD_DEBUG("orPlugin[%d]: %s\n",i,orPlugin->toString().c_str()); + std::string orPluginName = orPlugin->get(0).asString(); + CD_DEBUG("* orPlugin[%d]: plugin: %s\n",i,orPluginName.c_str()); + std::string orModuleName = orPlugin->find("module").asString(); + CD_DEBUG("* orPlugin[%d]: module: %s\n",i,orModuleName.c_str()); + if( orPlugin->check("commands") ) + { + CD_DEBUG("* orPlugin[%d]: commands: %s\n",i,orPlugin->find("commands").asString().c_str()); + } + + //-- Load plugin (docs say will reload if already loaded) + if ( ! OpenRAVE::RaveLoadPlugin(orPluginName) ) + { + CD_ERROR("Could not load plugin '%s'\n",orPluginName.c_str()); + return false; + } + + //-- Load module from plugin + OpenRAVE::ModuleBasePtr pModule = OpenRAVE::RaveCreateModule(penv,orModuleName); // create the module + penv->Add(pModule,true); // load the module, calls main and also enables good destroy. + //-- Send command if exist + if( orPlugin->check("commands") ) + { + std::stringstream cmdin,cmdout; + cmdin << orPlugin->find("commands").asString(); + // RAVELOG_INFO("%s\n",cmdin.str().c_str()); + if( ! pModule->SendCommand(cmdout,cmdin) ) + { + CD_ERROR("Bad send '%s' command.\n",cmdin.str().c_str()); + } + CD_SUCCESS("Sent '%s' command.\n",cmdin.str().c_str()); + } + } + } + + std::vector vectorOfRobotPtr; + penv->GetRobots(vectorOfRobotPtr); + probot = vectorOfRobotPtr[robotIndex]; + robotName = probot->GetName(); + + std::vector vectorOfManipulatorPtr = probot->GetManipulators(); + manipulatorIDs = vectorOfManipulatorPtr[manipulatorIndex]->GetArmIndices(); + + axes = manipulatorIDs.size(); + manipulatorTargetRads.resize( axes, 0.0 ); + controlModes.resize( axes, VOCAB_CM_POSITION ); + refSpeeds.resize( axes, genRefSpeed ); + + for(size_t i=0; iGetJointFromDOFIndex(manipulatorIDs[i]); + vectorOfJointPtr.push_back(jointPtr); + CD_DEBUG("Get JointPtr for manipulatorIDs[%d]: %d\n",i,manipulatorIDs[i]); + } + + //-- Create the controller, make sure to lock environment! + { + OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment + + std::vector activeDOFIndices = probot->GetActiveDOFIndices(); + //--- Console output robot active DOF + //for(size_t i=0; iGetController(); + CD_DEBUG("pcontrol: %p, %s\n",pcontrol.get(),pcontrol->GetXMLId().c_str()); + //-- Doing case insensitive check because defaults to IdealController but idealcontroller exists + std::string controllerName( pcontrol->GetXMLId() ); + std::transform(controllerName.begin(), controllerName.end(), controllerName.begin(), ::tolower); + if( controllerName == "idealcontroller" ) + { + CD_INFO("Detected idealcontroller, switch to genericmulticontroller.\n"); + pcontrol = OpenRAVE::RaveCreateMultiController(penv); + probot->SetController(pcontrol,activeDOFIndices,1); // idealcontroller -> genericmulticontroller + } + else if( controllerName == "genericmulticontroller") + { + CD_INFO("Detected genericmulticontroller, which will be used.\n"); + } + else + { + CD_ERROR("Non-treated controller case. Bye!\n"); + return false; + } + CD_DEBUG("pcontrol: %p, %s\n",pcontrol.get(),pcontrol->GetXMLId().c_str()); + + //-- Safe to assume we have a multicontroller, store for usage. + multi = boost::dynamic_pointer_cast< OpenRAVE::MultiControllerBase >(pcontrol); + + for(size_t i=0; i tmpIndices; + tmpIndices.push_back( manipulatorIDs[i] ); + CD_DEBUG("Attach individual controller for manipulatorIDs[%d]: %d\n",i,tmpIndices[0]); + multi->AttachController(pindivcontrol, tmpIndices, 0); + pcontrols.push_back(pindivcontrol); + } + + penv->StopSimulation(); + penv->StartSimulation(0.01); + + //-- Console output of the robot ConfigurationSpecification + //OpenRAVE::ConfigurationSpecification activeConfigurationSpecification = probot->GetActiveConfigurationSpecification(); + //for (size_t i = 0; i < activeConfigurationSpecification._vgroups.size(); i++) + //{ + // CD_DEBUG("%d, %s, %s\n",i,activeConfigurationSpecification._vgroups[i].name.c_str(), activeConfigurationSpecification._vgroups[i].interpolation.c_str()); + //} + + } + + return true; +} + +// ----------------------------------------------------------------------------- + +bool YarpOpenraveGrabber::close() { + printf("[YarpOpenraveGrabber] close()\n"); + return true; +} + +// ----------------------------------------------------------------------------- + +} // namespace roboticslab + diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp new file mode 100644 index 00000000..072d7a88 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -0,0 +1,40 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveGrabber.hpp" + +// ------------------ IVelocity Related ---------------------------------------- + +bool roboticslab::YarpOpenraveGrabber::velocityMove(int j, double sp) { // velExposed = sp; + + //-- Check if we are in position mode. + if( controlModes[j] != VOCAB_CM_VELOCITY ) + { + CD_ERROR("Will not velocityMove as not in velocityMode\n"); + return false; + } + + std::stringstream sout,ss; ss << "setvelocity "; + for(size_t i = 0; i < manipulatorIDs.size(); ++i) { + ss << sp << " "; + } + + if( ! pcontrols[j]->SendCommand(sout,ss) ) { + CD_ERROR("Failed to send velocity command\n"); + return false; + } + + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::YarpOpenraveGrabber::velocityMove(const double *sp) { + CD_INFO("\n"); + bool ok = true; + for(unsigned int i=0;i +#include + +#include + +#include + +#include +#include +#include +#include + +#include "ColorDebug.hpp" + +#define DEFAULT_GEN_REF_SPEED 7.5 // Exposed. + +namespace roboticslab +{ + + +// Specific for --env parameter +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer); + +/** + * @ingroup TeoYarp + * \defgroup YarpOpenraveGrabber + * + * @brief Contains teo::YarpOpenraveGrabber. + * + * @section YarpOpenraveGrabber_install Installation + * + * The plugin is compiled when ENABLE_TeoYarp_YarpOpenraveGrabber is activated (not default). For further + * installation steps refer to your own system installation guidelines. + */ + +/** + * @ingroup YarpOpenraveGrabber + * @brief Implements the YARP_dev IPositionControl, IVelocityControl, IEncodersTimed, etc. + * interface class member functions. + */ +// Note: IEncodersTimed inherits from IEncoders +// Note: IPositionControl2 inherits from IPositionControl +// Note: IVelocityControl2 inherits from IVelocityControl +// Note: IControlLimits2 inherits from IControlLimits +// Note: IControlMode2 inherits from IControlMode +class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IPositionControl2, public yarp::dev::IVelocityControl2, public yarp::dev::IEncodersTimed, + public yarp::dev::IControlLimits2, public yarp::dev::IControlMode2, public yarp::dev::ITorqueControl { +public: + + // Set the Thread Rate in the class constructor + YarpOpenraveGrabber() {} + + // ------- IPositionControl declarations. Implementation in IPositionImpl.cpp ------- + + /** + * Get the number of controlled axes. This command asks the number of controlled + * axes for the current physical interface. + * @param ax pointer to storage + * @return true/false. + */ + virtual bool getAxes(int *ax); + + /** Set new reference point for a single axis. + * @param j joint number + * @param ref specifies the new ref point + * @return true/false on success/failure + */ + virtual bool positionMove(int j, double ref); + + /** Set new reference point for all axes. + * @param refs array, new reference points. + * @return true/false on success/failure + */ + virtual bool positionMove(const double *refs); + + /** Set relative position. The command is relative to the + * current position of the axis. + * @param j joint axis number + * @param delta relative command + * @return true/false on success/failure + */ + virtual bool relativeMove(int j, double delta); + + /** Set relative position, all joints. + * @param deltas pointer to the relative commands + * @return true/false on success/failure + */ + virtual bool relativeMove(const double *deltas); + + /** Check if the current trajectory is terminated. Non blocking. + * @return true if the trajectory is terminated, false otherwise + */ + virtual bool checkMotionDone(int j, bool *flag); + + /** Check if the current trajectory is terminated. Non blocking. + * @return true if the trajectory is terminated, false otherwise + */ + virtual bool checkMotionDone(bool *flag); + + /** Set reference speed for a joint, this is the speed used during the + * interpolation of the trajectory. + * @param j joint number + * @param sp speed value + * @return true/false upon success/failure + */ + virtual bool setRefSpeed(int j, double sp); + + /** Set reference speed on all joints. These values are used during the + * interpolation of the trajectory. + * @param spds pointer to the array of speed values. + * @return true/false upon success/failure + */ + virtual bool setRefSpeeds(const double *spds); + + /** Set reference acceleration for a joint. This value is used during the + * trajectory generation. + * @param j joint number + * @param acc acceleration value + * @return true/false upon success/failure + */ + virtual bool setRefAcceleration(int j, double acc); + + /** Set reference acceleration on all joints. This is the valure that is + * used during the generation of the trajectory. + * @param accs pointer to the array of acceleration values + * @return true/false upon success/failure + */ + virtual bool setRefAccelerations(const double *accs); + + /** Get reference speed for a joint. Returns the speed used to + * generate the trajectory profile. + * @param j joint number + * @param ref pointer to storage for the return value + * @return true/false on success or failure + */ + virtual bool getRefSpeed(int j, double *ref); + + /** Get reference speed of all joints. These are the values used during the + * interpolation of the trajectory. + * @param spds pointer to the array that will store the speed values. + */ + virtual bool getRefSpeeds(double *spds); + + /** Get reference acceleration for a joint. Returns the acceleration used to + * generate the trajectory profile. + * @param j joint number + * @param acc pointer to storage for the return value + * @return true/false on success/failure + */ + virtual bool getRefAcceleration(int j, double *acc); + + /** Get reference acceleration of all joints. These are the values used during the + * interpolation of the trajectory. + * @param accs pointer to the array that will store the acceleration values. + * @return true/false on success or failure + */ + virtual bool getRefAccelerations(double *accs); + + /** Stop motion, single joint + * @param j joint number + * @return true/false on success/failure + */ + virtual bool stop(int j); + + /** Stop motion, multiple joints + * @return true/false on success/failure + */ + virtual bool stop(); + + // ------- IPositionControl2 declarations. Implementation in IPosition2Impl.cpp ------- + + /** Set new reference point for a subset of joints. + * @param joints pointer to the array of joint numbers + * @param refs pointer to the array specifing the new reference points + * @return true/false on success/failure + */ + virtual bool positionMove(const int n_joint, const int *joints, const double *refs); + + /** Set relative position for a subset of joints. + * @param joints pointer to the array of joint numbers + * @param deltas pointer to the array of relative commands + * @return true/false on success/failure + */ + virtual bool relativeMove(const int n_joint, const int *joints, const double *deltas); + + /** Check if the current trajectory is terminated. Non blocking. + * @param joints pointer to the array of joint numbers + * @param flags pointer to return value (logical "and" of all set of joints) + * @return true/false if network communication went well. + */ + virtual bool checkMotionDone(const int n_joint, const int *joints, bool *flags); + + /** Set reference speed on all joints. These values are used during the + * interpolation of the trajectory. + * @param joints pointer to the array of joint numbers + * @param spds pointer to the array with speed values. + * @return true/false upon success/failure + */ + virtual bool setRefSpeeds(const int n_joint, const int *joints, const double *spds); + + /** Set reference acceleration on all joints. This is the valure that is + * used during the generation of the trajectory. + * @param joints pointer to the array of joint numbers + * @param accs pointer to the array with acceleration values + * @return true/false upon success/failure + */ + virtual bool setRefAccelerations(const int n_joint, const int *joints, const double *accs); + + /** Get reference speed of all joints. These are the values used during the + * interpolation of the trajectory. + * @param joints pointer to the array of joint numbers + * @param spds pointer to the array that will store the speed values. + * @return true/false upon success/failure + */ + virtual bool getRefSpeeds(const int n_joint, const int *joints, double *spds); + + /** Get reference acceleration for a joint. Returns the acceleration used to + * generate the trajectory profile. + * @param joints pointer to the array of joint numbers + * @param accs pointer to the array that will store the acceleration values + * @return true/false on success/failure + */ + virtual bool getRefAccelerations(const int n_joint, const int *joints, double *accs); + + /** Stop motion for subset of joints + * @param joints pointer to the array of joint numbers + * @return true/false on success/failure + */ + virtual bool stop(const int n_joint, const int *joints); + + /** Get the last position reference for the specified axis. + * This is the dual of PositionMove and shall return only values sent using + * IPositionControl interface. + * If other interfaces like IPositionDirect are implemented by the device, this call + * must ignore their values, i.e. this call must never return a reference sent using + * IPositionDirect::SetPosition + * @param ref last reference sent using PositionMove functions + * @return true/false on success/failure + */ + virtual bool getTargetPosition(const int joint, double *ref); + + /** Get the last position reference for all axes. + * This is the dual of PositionMove and shall return only values sent using + * IPositionControl interface. + * If other interfaces like IPositionDirect are implemented by the device, this call + * must ignore their values, i.e. this call must never return a reference sent using + * IPositionDirect::SetPosition + * @param ref last reference sent using PositionMove functions + * @return true/false on success/failure + */ + virtual bool getTargetPositions(double *refs); + + /** Get the last position reference for the specified group of axes. + * This is the dual of PositionMove and shall return only values sent using + * IPositionControl interface. + * If other interfaces like IPositionDirect are implemented by the device, this call + * must ignore their values, i.e. this call must never return a reference sent using + * IPositionDirect::SetPosition + * @param ref last reference sent using PositionMove functions + * @return true/false on success/failure + */ + virtual bool getTargetPositions(const int n_joint, const int *joints, double *refs); + + // ---------- IEncodersTimed Declarations. Implementation in IEncoderImpl.cpp ---------- + + /** + * Reset encoder, single joint. Set the encoder value to zero + * @param j encoder number + * @return true/false + */ + virtual bool resetEncoder(int j); + + /** + * Reset encoders. Set the encoders value to zero + * @return true/false + */ + virtual bool resetEncoders(); + + /** + * Set the value of the encoder for a given joint. + * @param j encoder number + * @param val new value + * @return true/false + */ + virtual bool setEncoder(int j, double val); + + /** + * Set the value of all encoders. + * @param vals pointer to the new values + * @return true/false + */ + virtual bool setEncoders(const double *vals); + + /** + * Read the value of an encoder. + * @param j encoder number + * @param v pointer to storage for the return value + * @return true/false, upon success/failure (you knew it, uh?) + */ + virtual bool getEncoder(int j, double *v); + + /** + * Read the position of all axes. + * @param encs pointer to the array that will contain the output + * @return true/false on success/failure + */ + virtual bool getEncoders(double *encs); + + /** + * Read the istantaneous speed of an axis. + * @param j axis number + * @param sp pointer to storage for the output + * @return true if successful, false ... otherwise. + */ + virtual bool getEncoderSpeed(int j, double *sp); + + /** + * Read the instantaneous speed of all axes. + * @param spds pointer to storage for the output values + * @return guess what? (true/false on success or failure). + */ + virtual bool getEncoderSpeeds(double *spds); + + /** + * Read the instantaneous acceleration of an axis. + * @param j axis number + * @param spds pointer to the array that will contain the output + */ + virtual bool getEncoderAcceleration(int j, double *spds); + + /** + * Read the instantaneous acceleration of all axes. + * @param accs pointer to the array that will contain the output + * @return true if all goes well, false if anything bad happens. + */ + virtual bool getEncoderAccelerations(double *accs); + + /** + * Read the instantaneous acceleration of all axes. + * \param encs pointer to the array that will contain the output + * \param time pointer to the array that will contain individual timestamps + * \return true if all goes well, false if anything bad happens. + */ + virtual bool getEncodersTimed(double *encs, double *time); + + /** + * Read the instantaneous acceleration of all axes. + * \param j axis index + * \param encs encoder value (pointer to) + * \param time corresponding timestamp (pointer to) + * \return true if all goes well, false if anything bad happens. + */ + virtual bool getEncoderTimed(int j, double *encs, double *time); + + // --------- IVelocityControl Declarations. Implementation in IVelocityImpl.cpp --------- + + /** + * Start motion at a given speed, single joint. + * @param j joint number + * @param sp speed value + * @return bool/false upone success/failure + */ + virtual bool velocityMove(int j, double sp); + + /** + * Start motion at a given speed, multiple joints. + * @param sp pointer to the array containing the new speed values + * @return true/false upon success/failure + */ + virtual bool velocityMove(const double *sp); + + // --------- IVelocityControl2 Declarations. Implementation in IVelocity2Impl.cpp --------- + + /** Start motion at a given speed for a subset of joints. + * @param n_joint how many joints this command is referring to + * @param joints of joints controlled. The size of this array is n_joints + * @param spds pointer to the array containing the new speed values, one value for each joint, the size of the array is n_joints. + * The first value will be the new reference fot the joint joints[0]. + * for example: + * n_joint 3 + * joints 0 2 4 + * spds 10 30 40 + * @return true/false on success/failure + */ + virtual bool velocityMove(const int n_joint, const int *joints, const double *spds); + + /** Get the last reference speed set by velocityMove for single joint. + * @param j joint number + * @param vel returns the requested reference. + * @return true/false on success/failure + */ + virtual bool getRefVelocity(const int joint, double *vel); + + /** Get the last reference speed set by velocityMove for all joints. + * @param vels pointer to the array containing the new speed values, one value for each joint + * @return true/false on success/failure + */ + virtual bool getRefVelocities(double *vels); + + /** Get the last reference speed set by velocityMove for a group of joints. + * @param n_joint how many joints this command is referring to + * @param joints of joints controlled. The size of this array is n_joints + * @param vels pointer to the array containing the requested values, one value for each joint. + * The size of the array is n_joints. + * @return true/false on success/failure + */ + virtual bool getRefVelocities(const int n_joint, const int *joints, double *vels); + + /** Set new velocity pid value for a joint + * @param j joint number + * @param pid new pid value + * @return true/false on success/failure + */ + virtual bool setVelPid(int j, const yarp::dev::Pid &pid); + + /** Set new velocity pid value on multiple joints + * @param pids pointer to a vector of pids + * @return true/false upon success/failure + */ + virtual bool setVelPids(const yarp::dev::Pid *pids); + + /** Get current velocity pid value for a specific joint. + * @param j joint number + * @param pid pointer to storage for the return value. + * @return success/failure + */ + virtual bool getVelPid(int j, yarp::dev::Pid *pid); + + /** Get current velocity pid value for a specific subset of joints. + * @param pids vector that will store the values of the pids. + * @return success/failure + */ + virtual bool getVelPids(yarp::dev::Pid *pids); + + // --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp --------- + + /** + * Set the software limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number (why am I telling you this) + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setLimits(int axis, double min, double max); + + /** Get the software limits for a particular axis. + * @param axis joint number (again... why am I telling you this) + * @param pointer to store the value of the lower limit + * @param pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getLimits(int axis, double *min, double *max); + + /** + * Set the software speed limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setVelLimits(int axis, double min, double max); + + /** + * Get the software speed limits for a particular axis. + * @param axis joint number + * @param min pointer to store the value of the lower limit + * @param max pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getVelLimits(int axis, double *min, double *max); + + // --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp --------- + + /** + * Set position mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setPositionMode(int j); + + /** + * Set velocity mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setVelocityMode(int j); + + /** + * Set torque mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setTorqueMode(int j); + + /** + * Set impedance position mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setImpedancePositionMode(int j); + + /** + * Set impedance velocity mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setImpedanceVelocityMode(int j); + + /** + * Set open loop mode, single axis. + * @param j: joint number + * @return: true/false success failure. + */ + virtual bool setOpenLoopMode(int j); + + /** + * Get the current control mode. + * @param j: joint number + * @param mode: a vocab of the current control mode for joint j. + * @return: true/false success failure. + */ + virtual bool getControlMode(int j, int *mode); + + /** + * Get the current control mode (multiple joints). + * @param modes: a vector containing vocabs for the current control modes of the joints. + * @return: true/false success failure. + */ + virtual bool getControlModes(int *modes); + + // --------- IControlMode2 Declarations. Implementation in IControlMode2Impl.cpp --------- + + /** + * Get the current control mode for a subset of axes. + * @param n_joints how many joints this command is referring to + * @param joints list of joint numbers, the size of this array is n_joints + * @param modes array containing the new controlmodes, one value for each joint, the size is n_joints. + * The first value will be the new reference fot the joint joints[0]. + * for example: + * n_joint 3 + * joints 0 2 4 + * modes VOCAB_CM_POSITION VOCAB_CM_VELOCITY VOCAB_CM_POSITION + * @return true/false success failure. + */ + virtual bool getControlModes(const int n_joint, const int *joints, int *modes); + + /** + * Set the current control mode. + * @param j: joint number + * @param mode: a vocab of the desired control mode for joint j. + * @return true if the new controlMode was successfully set, false if the message was not received or + * the joint was unable to switch to the desired controlMode + * (e.g. the joint is on a fault condition or the desired mode is not implemented). */ + virtual bool setControlMode(const int j, const int mode); + + /** + * Set the current control mode for a subset of axes. + * @param n_joints how many joints this command is referring to + * @param joints list of joint numbers, the size of this array is n_joints + * @param modes array containing the new controlmodes, one value for each joint, the size is n_joints. + * The first value will be the new reference fot the joint joints[0]. + * for example: + * n_joint 3 + * joints 0 2 4 + * modes VOCAB_CM_POSITION VOCAB_CM_VELOCITY VOCAB_CM_POSITION + * @return true if the new controlMode was successfully set, false if the message was not received or + * the joint was unable to switch to the desired controlMode + * (e.g. the joint is on a fault condition or the desired mode is not implemented). + */ + virtual bool setControlModes(const int n_joint, const int *joints, int *modes); + + /** + * Set the current control mode (multiple joints). + * @param modes: a vector containing vocabs for the desired control modes of the joints. + * @return true if the new controlMode was successfully set, false if the message was not received or + * the joint was unable to switch to the desired controlMode + * (e.g. the joint is on a fault condition or the desired mode is not implemented). + */ + virtual bool setControlModes(int *modes); + // -------- ITorqueControl declarations. Implementation in ITorqueImpl.cpp -------- + + + /** Get the reference value of the torque for all joints. + * This is NOT the feedback (see getTorques instead). + * @param t pointer to the array of torque values + * @return true/false on success/failure + */ + virtual bool getRefTorques(double *t); + + /** Get the reference value of the torque for a given joint. + * This is NOT the feedback (see getTorque instead). + * @param j joint number + * @param t the returned reference torque of joint j + * @return true/false on success/failure + */ + virtual bool getRefTorque(int j, double *t); + + /** Set the reference value of the torque for all joints. + * @param t pointer to the array of torque values + * @return true/false on success/failure + */ + virtual bool setRefTorques(const double *t); + + /** Set the reference value of the torque for a given joint. + * @param j joint number + * @param t new value + * @return true/false on success/failure + */ + virtual bool setRefTorque(int j, double t); + + /** Set the back-efm compensation gain for a given joint. + * @param j joint number + * @param bemf the returned bemf gain of joint j + * @return true/false on success/failure + */ + virtual bool getBemfParam(int j, double *bemf); + + /** Set the back-efm compensation gain for a given joint. + * @param j joint number + * @param bemf new value + * @return true/false on success/failure + */ + virtual bool setBemfParam(int j, double bemf); + + /** Set new pid value for a joint axis. + * @param j joint number + * @param pid new pid value + * @return true/false on success/failure + */ + virtual bool setTorquePid(int j, const yarp::dev::Pid &pid); + + /** Get the value of the torque on a given joint (this is the + * feedback if you have a torque sensor). + * @param j joint number + * @param t pointer to the result value + * @return true/false on success/failure + */ + virtual bool getTorque(int j, double *t); + + /** Get the value of the torque for all joints (this is + * the feedback if you have torque sensors). + * @param t pointer to the array that will store the output + * @return true/false on success/failure + */ + virtual bool getTorques(double *t); + + /** Get the full scale of the torque sensor of a given joint + * @param j joint number + * @param min minimum torque of the joint j + * @param max maximum torque of the joint j + * @return true/false on success/failure + */ + virtual bool getTorqueRange(int j, double *min, double *max); + + /** Get the full scale of the torque sensors of all joints + * @param min pointer to the array that will store minimum torques of the joints + * @param max pointer to the array that will store maximum torques of the joints + * @return true/false on success/failure + */ + virtual bool getTorqueRanges(double *min, double *max); + + /** Set new pid value on multiple axes. + * @param pids pointer to a vector of pids + * @return true/false upon success/failure + */ + virtual bool setTorquePids(const yarp::dev::Pid *pids); + + /** Set the torque error limit for the controller on a specific joint + * @param j joint number + * @param limit limit value + * @return true/false on success/failure + */ + virtual bool setTorqueErrorLimit(int j, double limit); + + /** Get the torque error limit for the controller on all joints. + * @param limits pointer to the vector with the new limits + * @return true/false on success/failure + */ + virtual bool setTorqueErrorLimits(const double *limits); + + /** Get the current torque error for a joint. + * @param j joint number + * @param err pointer to the storage for the return value + * @return true/false on success failure + */ + virtual bool getTorqueError(int j, double *err); + + /** Get the torque error of all joints. + * @param errs pointer to the vector that will store the errors + * @return true/false on success/failure + */ + virtual bool getTorqueErrors(double *errs); + + /** Get the output of the controller (e.g. pwm value) + * @param j joint number + * @param out pointer to storage for return value + * @return true/false on success/failure + */ + virtual bool getTorquePidOutput(int j, double *out); + + /** Get the output of the controllers (e.g. pwm value) + * @param outs pointer to the vector that will store the output values + * @return true/false on success/failure + */ + virtual bool getTorquePidOutputs(double *outs); + + /** Get current pid value for a specific joint. + * @param j joint number + * @param pid pointer to storage for the return value. + * @return true/false on success/failure + */ + virtual bool getTorquePid(int j, yarp::dev::Pid *pid); + + /** Get current pid value for a specific joint. + * @param pids vector that will store the values of the pids. + * @return true/false on success/failure + */ + virtual bool getTorquePids(yarp::dev::Pid *pids); + + /** Get the torque error limit for the controller on a specific joint + * @param j joint number + * @param limit pointer to the result value + * @return true/false on success/failure + */ + virtual bool getTorqueErrorLimit(int j, double *limit); + + /** Get the torque error limit for all controllers + * @param limits pointer to the array that will store the output + * @return true/false on success/failure + */ + virtual bool getTorqueErrorLimits(double *limits); + + /** Reset the controller of a given joint, usually sets the + * current position of the joint as the reference value for the PID, and resets + * the integrator. + * @param j joint number + * @return true/false on success/failure + */ + virtual bool resetTorquePid(int j); + + /** Disable the pid computation for a joint + * @param j joint number + * @return true/false on success/failure + */ + virtual bool disableTorquePid(int j); + + /** Enable the pid computation for a joint + * @param j joint number + * @return true/false on success/failure + */ + virtual bool enableTorquePid(int j); + + /** Set offset value for a given pid + * @param j joint number + * @param v the new value + * @return true/false on success/failure + */ + virtual bool setTorqueOffset(int j, double v); + + // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- + + /** + * Open the DeviceDriver. + * @param config is a list of parameters for the device. + * Which parameters are effective for your device can vary. + * See \ref dev_examples "device invocation examples". + * If there is no example for your device, + * you can run the "yarpdev" program with the verbose flag + * set to probe what parameters the device is checking. + * If that fails too, + * you'll need to read the source code (please nag one of the + * yarp developers to add documentation for your device). + * @return true/false upon success/failure + */ + virtual bool open(yarp::os::Searchable& config); + + /** + * Close the DeviceDriver. + * @return true/false on success/failure. + */ + virtual bool close(); + + // -------- RateThread declarations. Implementation in RateThreadImpl.cpp -------- + + /** + * Initialization method. The thread executes this function + * when it starts and before "run". This is a good place to + * perform initialization tasks that need to be done by the + * thread itself (device drivers initialization, memory + * allocation etc). If the function returns false the thread + * quits and never calls "run". The return value of threadInit() + * is notified to the class and passed as a parameter + * to afterStart(). Note that afterStart() is called by the + * same thread that is executing the "start" method. + */ + bool threadInit(); + + /** + * Loop function. This is the thread itself. + */ + void run(); + + // ------------------------------- Private ------------------------------------- + +private: + + // General Joint Motion Controller parameters // + unsigned int axes; + std::vector< int > controlModes; + std::vector refSpeeds; // Exposed. + + //OpenRAVE// + OpenRAVE::EnvironmentBasePtr penv; + OpenRAVE::RobotBasePtr probot; + std::string robotName; + OpenRAVE::MultiControllerBasePtr multi; + std::vector< OpenRAVE::ControllerBasePtr > pcontrols; + std::vector< int > manipulatorIDs; + std::vector manipulatorTargetRads; + std::vector vectorOfJointPtr; + + // Specific for --env parameter with --view + boost::thread_group openraveThreads; +}; + +} // namespace roboticslab + +#endif // __YARP_OPENRAVE_CONTROLBOARD_HPP__ diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini new file mode 100644 index 00000000..877041a0 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini @@ -0,0 +1,7 @@ +[plugin YarpOpenraveGrabber] +type device +name YarpOpenraveGrabber +library YarpOpenraveGrabber +part YarpOpenraveGrabber +code "YarpOpenraveGrabber" +wrapper grabberwrapper2 From 6cb96e064462e0abb1a3c75ce2d0adab2bf8fe40 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 00:40:29 +0100 Subject: [PATCH 35/70] YarpOpenraveGrabber loads --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 66 -- .../IFrameGrabberImageImpl.cpp | 42 +- .../YarpOpenraveGrabber.hpp | 762 +----------------- .../YarpOpenraveGrabber.ini | 2 +- 4 files changed, 38 insertions(+), 834 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 78059250..1def935b 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -168,77 +168,11 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { robotName = probot->GetName(); std::vector vectorOfManipulatorPtr = probot->GetManipulators(); - manipulatorIDs = vectorOfManipulatorPtr[manipulatorIndex]->GetArmIndices(); - - axes = manipulatorIDs.size(); - manipulatorTargetRads.resize( axes, 0.0 ); - controlModes.resize( axes, VOCAB_CM_POSITION ); - refSpeeds.resize( axes, genRefSpeed ); - - for(size_t i=0; iGetJointFromDOFIndex(manipulatorIDs[i]); - vectorOfJointPtr.push_back(jointPtr); - CD_DEBUG("Get JointPtr for manipulatorIDs[%d]: %d\n",i,manipulatorIDs[i]); - } //-- Create the controller, make sure to lock environment! { OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment - std::vector activeDOFIndices = probot->GetActiveDOFIndices(); - //--- Console output robot active DOF - //for(size_t i=0; iGetController(); - CD_DEBUG("pcontrol: %p, %s\n",pcontrol.get(),pcontrol->GetXMLId().c_str()); - //-- Doing case insensitive check because defaults to IdealController but idealcontroller exists - std::string controllerName( pcontrol->GetXMLId() ); - std::transform(controllerName.begin(), controllerName.end(), controllerName.begin(), ::tolower); - if( controllerName == "idealcontroller" ) - { - CD_INFO("Detected idealcontroller, switch to genericmulticontroller.\n"); - pcontrol = OpenRAVE::RaveCreateMultiController(penv); - probot->SetController(pcontrol,activeDOFIndices,1); // idealcontroller -> genericmulticontroller - } - else if( controllerName == "genericmulticontroller") - { - CD_INFO("Detected genericmulticontroller, which will be used.\n"); - } - else - { - CD_ERROR("Non-treated controller case. Bye!\n"); - return false; - } - CD_DEBUG("pcontrol: %p, %s\n",pcontrol.get(),pcontrol->GetXMLId().c_str()); - - //-- Safe to assume we have a multicontroller, store for usage. - multi = boost::dynamic_pointer_cast< OpenRAVE::MultiControllerBase >(pcontrol); - - for(size_t i=0; i tmpIndices; - tmpIndices.push_back( manipulatorIDs[i] ); - CD_DEBUG("Attach individual controller for manipulatorIDs[%d]: %d\n",i,tmpIndices[0]); - multi->AttachController(pindivcontrol, tmpIndices, 0); - pcontrols.push_back(pindivcontrol); - } - - penv->StopSimulation(); - penv->StartSimulation(0.01); - - //-- Console output of the robot ConfigurationSpecification - //OpenRAVE::ConfigurationSpecification activeConfigurationSpecification = probot->GetActiveConfigurationSpecification(); - //for (size_t i = 0; i < activeConfigurationSpecification._vgroups.size(); i++) - //{ - // CD_DEBUG("%d, %s, %s\n",i,activeConfigurationSpecification._vgroups[i].name.c_str(), activeConfigurationSpecification._vgroups[i].interpolation.c_str()); - //} - } return true; diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index 072d7a88..956dd37c 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -2,39 +2,33 @@ #include "YarpOpenraveGrabber.hpp" -// ------------------ IVelocity Related ---------------------------------------- +namespace roboticslab +{ -bool roboticslab::YarpOpenraveGrabber::velocityMove(int j, double sp) { // velExposed = sp; +// ------------------ IFrameGrabberImage Related ---------------------------------------- - //-- Check if we are in position mode. - if( controlModes[j] != VOCAB_CM_VELOCITY ) - { - CD_ERROR("Will not velocityMove as not in velocityMode\n"); - return false; - } +bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& image) +{ - std::stringstream sout,ss; ss << "setvelocity "; - for(size_t i = 0; i < manipulatorIDs.size(); ++i) { - ss << sp << " "; - } + return true; +} - if( ! pcontrols[j]->SendCommand(sout,ss) ) { - CD_ERROR("Failed to send velocity command\n"); - return false; - } +// ---------------------------------------------------------------------------- - return true; +int YarpOpenraveGrabber::height() const +{ + + return 0; } -// ----------------------------------------------------------------------------- +// ---------------------------------------------------------------------------- + +int YarpOpenraveGrabber::width() const +{ -bool roboticslab::YarpOpenraveGrabber::velocityMove(const double *sp) { - CD_INFO("\n"); - bool ok = true; - for(unsigned int i=0;i& image); /** - * Loop function. This is the thread itself. + * Return the height of each frame. + * @return image height */ - void run(); + virtual int height() const; - // ------------------------------- Private ------------------------------------- + /** + * Return the width of each frame. + * @return image width + */ + virtual int width() const; private: - // General Joint Motion Controller parameters // - unsigned int axes; - std::vector< int > controlModes; - std::vector refSpeeds; // Exposed. + // General Grabber parameters // //OpenRAVE// OpenRAVE::EnvironmentBasePtr penv; OpenRAVE::RobotBasePtr probot; std::string robotName; - OpenRAVE::MultiControllerBasePtr multi; - std::vector< OpenRAVE::ControllerBasePtr > pcontrols; - std::vector< int > manipulatorIDs; - std::vector manipulatorTargetRads; - std::vector vectorOfJointPtr; // Specific for --env parameter with --view boost::thread_group openraveThreads; diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini index 877041a0..f8cd0352 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini @@ -4,4 +4,4 @@ name YarpOpenraveGrabber library YarpOpenraveGrabber part YarpOpenraveGrabber code "YarpOpenraveGrabber" -wrapper grabberwrapper2 +wrapper grabber From 63edce46a1bebc8e3c0425a0064dbe5a56103099 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:21:34 +0100 Subject: [PATCH 36/70] simplify viewer definition --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 8 +++----- .../YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 1def935b..1b31e3dc 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -9,7 +9,7 @@ namespace roboticslab // ------------------------------------------------------------------- -void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer) +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername) { OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv,viewername); BOOST_ASSERT(!!viewer); @@ -18,9 +18,7 @@ void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, 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); + viewer->main(true); } // ------------------- DeviceDriver Related ------------------------------------ @@ -48,7 +46,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { if ( config.check("view") ) { - boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin",1)); + boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin")); openraveThreads.add_thread(&openraveViewerThread); yarp::os::Time::delay(0.4); // wait for the viewer to init, in [s] } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index 4f4f9c75..41ae1ec2 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -24,7 +24,7 @@ namespace roboticslab // Specific for --env parameter -void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer); +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername); /** * @ingroup TeoYarp From 5c586a9a52452419d81eceeff915d29d3e4eaf92 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:22:56 +0100 Subject: [PATCH 37/70] remove not used here --- libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 1b31e3dc..89f897dd 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -2,8 +2,6 @@ #include "YarpOpenraveGrabber.hpp" -#include // Thanks: https://notfaq.wordpress.com/2007/08/04/cc-convert-string-to-upperlower-case/ - namespace roboticslab { From 71030124a0b780d3801137709e347ed2eb2cc8f1 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:30:02 +0100 Subject: [PATCH 38/70] also check below 0 --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index e76b53b0..b03fa67c 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -132,6 +132,11 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotPtrIdx,vectorOfRobotPtr.size()); return false; } + else if (robotPtrIdx < 0) + { + CD_ERROR("robotIndex %d < 0, not loading yarpPlugin.\n",robotPtrIdx); + return false; + } name += vectorOfRobotPtr[ robotPtrIdx ]->GetName(); From edbe860c0c2c7dcfde068df664bbf9dfd104900c Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:31:08 +0100 Subject: [PATCH 39/70] also check below 0 --- .../OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp index b03fa67c..9175cb63 100644 --- a/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -151,6 +151,11 @@ class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase CD_ERROR("manipulatorPtrIdx %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorPtrIdx,vectorOfManipulatorPtr.size()); return false; } + else if (manipulatorPtrIdx < 0) + { + CD_ERROR("manipulatorPtrIdx %d < 0, not loading yarpPlugin.\n",manipulatorPtrIdx); + return false; + } name += "/"; name += vectorOfManipulatorPtr[ manipulatorPtrIdx ]->GetName(); From 4b8ff85794e12a1d1368524c609eca2c98fa980a Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:38:21 +0100 Subject: [PATCH 40/70] better index checks in YarpOpenraveGrabber --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 37 ++++++++++++------- 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 89f897dd..566709cf 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -25,7 +25,8 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { CD_DEBUG("config: %s\n",config.toString().c_str()); - double genRefSpeed = config.check("genRefSpeed",DEFAULT_GEN_REF_SPEED,"general ref speed").asDouble(); + int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt(); + int manipulatorIndex = config.check("manipulatorIndex",-1,"manipulatorIndex").asInt(); if ( ( config.check("env") ) && ( config.check("penv") ) ) { @@ -69,19 +70,6 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { 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, 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, currently '%d'.\n", manipulatorIndex); - return false; - } - if( config.check("orPlugin") ) { CD_DEBUG("Found --orPlugin parameter.\n"); @@ -160,10 +148,31 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { std::vector vectorOfRobotPtr; penv->GetRobots(vectorOfRobotPtr); + if(robotIndex >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotIndex,vectorOfRobotPtr.size()); + return false; + } + else if (robotIndex < 0) + { + CD_ERROR("robotIndex %d < 0, not loading yarpPlugin.\n",robotIndex); + return false; + } + probot = vectorOfRobotPtr[robotIndex]; robotName = probot->GetName(); std::vector vectorOfManipulatorPtr = probot->GetManipulators(); + if(manipulatorIndex >= vectorOfManipulatorPtr.size()) + { + CD_ERROR("manipulatorIndex %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorIndex,vectorOfManipulatorPtr.size()); + return false; + } + else if (manipulatorIndex < 0) + { + CD_ERROR("manipulatorIndex %d < 0, not loading yarpPlugin.\n",manipulatorIndex); + return false; + } //-- Create the controller, make sure to lock environment! { From 40d111f73b60514877850a2ef39cc00990d8b6d1 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 09:45:33 +0100 Subject: [PATCH 41/70] add cameraSensorDataPtr --- .../YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index 41ae1ec2..b8edf333 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -102,6 +102,8 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF OpenRAVE::RobotBasePtr probot; std::string robotName; + boost::shared_ptr cameraSensorDataPtr; + // Specific for --env parameter with --view boost::thread_group openraveThreads; }; From a2f999cfade171a9dc3b93e56649e5eecec7a9ae Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 16:00:40 +0100 Subject: [PATCH 42/70] add GetSensorData(cameraSensorDataPtr) --- .../IFrameGrabberImageImpl.cpp | 15 +++++++++++++++ .../YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 1 + 2 files changed, 16 insertions(+) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index 956dd37c..f71835c8 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -9,6 +9,21 @@ namespace roboticslab bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& image) { + sensorPtrForCameras->GetSensorData(cameraSensorDataPtr); + + //std::vector currentFrame = pcamerasensordata->vimagedata; + //printf("Vector size: %d\n",currentFrame.size()); // i.e. 480 * 640 * 3 = 921600; + //yarp::sig::ImageOf& i_imagen = ptrVectorOfRgbPortPtr->at(camIter)->prepare(); + //i_imagen.resize(ptrVectorOfCameraWidth->at(camIter),ptrVectorOfCameraHeight->at(camIter)); // Tamaño de la pantalla + yarp::sig::PixelRgb p; + for (int i_x = 0; i_x < image.width(); ++i_x) { + for (int i_y = 0; i_y < image.height(); ++i_y) { + p.r = cameraSensorDataPtr->vimagedata[3*(i_x+(i_y*image.width()))]; + p.g = cameraSensorDataPtr->vimagedata[1+3*(i_x+(i_y*image.width()))]; + p.b = cameraSensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; + image.safePixel(i_x,i_y) = p; + } + } return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index b8edf333..ef93d68c 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -102,6 +102,7 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF OpenRAVE::RobotBasePtr probot; std::string robotName; + OpenRAVE::SensorBasePtr sensorPtrForCameras; boost::shared_ptr cameraSensorDataPtr; // Specific for --env parameter with --view From a143ae209ce467afb383ef1297f82ee63d4b4fc5 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 16:02:10 +0100 Subject: [PATCH 43/70] astyle --- .../YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index f71835c8..7eb1378c 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -16,8 +16,10 @@ bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& imag //yarp::sig::ImageOf& i_imagen = ptrVectorOfRgbPortPtr->at(camIter)->prepare(); //i_imagen.resize(ptrVectorOfCameraWidth->at(camIter),ptrVectorOfCameraHeight->at(camIter)); // Tamaño de la pantalla yarp::sig::PixelRgb p; - for (int i_x = 0; i_x < image.width(); ++i_x) { - for (int i_y = 0; i_y < image.height(); ++i_y) { + for (int i_x = 0; i_x < image.width(); ++i_x) + { + for (int i_y = 0; i_y < image.height(); ++i_y) + { p.r = cameraSensorDataPtr->vimagedata[3*(i_x+(i_y*image.width()))]; p.g = cameraSensorDataPtr->vimagedata[1+3*(i_x+(i_y*image.width()))]; p.b = cameraSensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; From 618fda8671fee342c64c4a21105e76c512a2937b Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 16:24:14 +0100 Subject: [PATCH 44/70] start working with GetAttachedSensors --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 566709cf..4f69a5d8 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -26,7 +26,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { CD_DEBUG("config: %s\n",config.toString().c_str()); int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt(); - int manipulatorIndex = config.check("manipulatorIndex",-1,"manipulatorIndex").asInt(); + int sensorIndex = config.check("sensorIndex",-1,"sensorIndex").asInt(); if ( ( config.check("env") ) && ( config.check("penv") ) ) { @@ -162,15 +162,15 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { probot = vectorOfRobotPtr[robotIndex]; robotName = probot->GetName(); - std::vector vectorOfManipulatorPtr = probot->GetManipulators(); - if(manipulatorIndex >= vectorOfManipulatorPtr.size()) + std::vector vectorOfSensorPtr = vectorOfRobotPtr.at(robotIndex)->GetAttachedSensors(); + if(sensorIndex >= vectorOfSensorPtr.size()) { - CD_ERROR("manipulatorIndex %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorIndex,vectorOfManipulatorPtr.size()); + CD_ERROR("sensorIndex %d >= vectorOfSensorPtr.size() %d, not loading yarpPlugin.\n",sensorIndex,vectorOfSensorPtr.size()); return false; } - else if (manipulatorIndex < 0) + else if (sensorIndex < 0) { - CD_ERROR("manipulatorIndex %d < 0, not loading yarpPlugin.\n",manipulatorIndex); + CD_ERROR("sensorIndex %d < 0, not loading yarpPlugin.\n",sensorIndex); return false; } From c5e64d1920f51617fbec47b0bbc8eb6fcb5c6f40 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 16:52:19 +0100 Subject: [PATCH 45/70] no controller here --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 4f69a5d8..41722805 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -174,12 +174,6 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { return false; } - //-- Create the controller, make sure to lock environment! - { - OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment - - } - return true; } From 45dec74c520c34a83f56f35107fe22380f54b69b Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:13:39 +0100 Subject: [PATCH 46/70] load new YarpOpenraveGrabber --- examples/openrave-YarpPluginLoader-grabber.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/openrave-YarpPluginLoader-grabber.py b/examples/openrave-YarpPluginLoader-grabber.py index 245109fd..8322b867 100644 --- a/examples/openrave-YarpPluginLoader-grabber.py +++ b/examples/openrave-YarpPluginLoader-grabber.py @@ -14,7 +14,7 @@ env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpPluginLoader.SendCommand('open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 --prefix /drl') + print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 0 --prefix /drl') while 1: pass From 19731e94b1314d315d73cac66879cdc65e434fd0 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:15:03 +0100 Subject: [PATCH 47/70] point to existing ecro sensor --- examples/openrave-YarpPluginLoader-grabber.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/openrave-YarpPluginLoader-grabber.py b/examples/openrave-YarpPluginLoader-grabber.py index 8322b867..47e1c839 100644 --- a/examples/openrave-YarpPluginLoader-grabber.py +++ b/examples/openrave-YarpPluginLoader-grabber.py @@ -14,7 +14,7 @@ env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 0 --prefix /drl') + print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 1 --prefix /drl') while 1: pass From 503b8a593adf0b8905d7fbdfebe759de29f1f3cf Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:23:38 +0100 Subject: [PATCH 48/70] implemented most functionalities, such as height and width --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 43 +++++++++++++++++++ .../IFrameGrabberImageImpl.cpp | 8 ++-- .../YarpOpenraveGrabber.hpp | 1 + 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 41722805..e8ff63ba 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -174,6 +174,49 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { return false; } + OpenRAVE::SensorBasePtr psensorbase = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + + std::string tipo = psensorbase->GetName(); + + printf("Sensor %d name: %s\n",sensorIndex,tipo.c_str()); + + // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); + + if ( ! psensorbase->Supports(OpenRAVE::SensorBase::ST_Camera) ) + { + CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); + } + + // Activate the camera + psensorbase->Configure(OpenRAVE::SensorBase::CC_PowerOn); + + // Show the camera image in a separate window + // pcamerasensorbase->Configure(SensorBase::CC_RenderDataOn); + // Get some camera parameter info + boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(psensorbase->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); + _width = pcamerageomdata->width; + _height = pcamerageomdata->height; + + cameraSensorDataPtr = boost::dynamic_pointer_cast(psensorbase->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + + /* + // Get a pointer to access the camera data stream + vectorOfCameraSensorDataPtr.push_back(boost::dynamic_pointer_cast(psensorbase->CreateSensorData(OpenRAVE::SensorBase::ST_Camera))); + vectorOfSensorPtrForCameras.push_back(psensorbase); // "save" + yarp::os::BufferedPort >* tmpPort = new yarp::os::BufferedPort >; + yarp::os::ConstString tmpName("/"); + yarp::os::ConstString cameraSensorString(psensorbase->GetName()); + size_t pos = cameraSensorString.find("imageMap"); + if ( pos != std::string::npos) { + tmpName += cameraSensorString.substr (0,pos-1); + tmpName += "/imageMap:o"; + } else { + tmpName += cameraSensorString.c_str(); + tmpName += "/img:o"; + } + */ + return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index 7eb1378c..c03e8c89 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -34,16 +34,16 @@ bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& imag int YarpOpenraveGrabber::height() const { - - return 0; + CD_DEBUG("%d\n", _height); + return _height; } // ---------------------------------------------------------------------------- int YarpOpenraveGrabber::width() const { - - return 0; + CD_DEBUG("%d\n", _width); + return _width; } // ---------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index ef93d68c..a4603d4e 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -96,6 +96,7 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF private: // General Grabber parameters // + int _height, _width; //OpenRAVE// OpenRAVE::EnvironmentBasePtr penv; From 84b46cc0c3bb1436a87642fe9e6ca2e4c8ccb559 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:37:05 +0100 Subject: [PATCH 49/70] remove and update commented code --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index e8ff63ba..e25ca666 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -191,7 +191,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { psensorbase->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window - // pcamerasensorbase->Configure(SensorBase::CC_RenderDataOn); + //psensorbase->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(psensorbase->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); @@ -200,23 +200,6 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { cameraSensorDataPtr = boost::dynamic_pointer_cast(psensorbase->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); - /* - // Get a pointer to access the camera data stream - vectorOfCameraSensorDataPtr.push_back(boost::dynamic_pointer_cast(psensorbase->CreateSensorData(OpenRAVE::SensorBase::ST_Camera))); - vectorOfSensorPtrForCameras.push_back(psensorbase); // "save" - yarp::os::BufferedPort >* tmpPort = new yarp::os::BufferedPort >; - yarp::os::ConstString tmpName("/"); - yarp::os::ConstString cameraSensorString(psensorbase->GetName()); - size_t pos = cameraSensorString.find("imageMap"); - if ( pos != std::string::npos) { - tmpName += cameraSensorString.substr (0,pos-1); - tmpName += "/imageMap:o"; - } else { - tmpName += cameraSensorString.c_str(); - tmpName += "/img:o"; - } - */ - return true; } From 631e5b6882124e243fcf7732e0822bb3750cae06 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:50:51 +0100 Subject: [PATCH 50/70] grabberwrapper2 -> grabber, though was more important in .ini --- libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt index e3996a70..0ca1d92c 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt @@ -5,7 +5,7 @@ yarp_prepare_plugin(YarpOpenraveGrabber CATEGORY device TYPE roboticslab::YarpOpenraveGrabber INCLUDE YarpOpenraveGrabber.hpp - WRAPPER grabberwrapper2) + WRAPPER grabber) IF (NOT SKIP_YarpOpenraveGrabber) From 0715f40479e3109a652f1ff59d351a78d1d6bedb Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 18:57:22 +0100 Subject: [PATCH 51/70] Better checks on robotIndex and manipulatorIndex, for #39 --- .../DeviceDriverImpl.cpp | 37 ++++++++++++------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index e3c41629..3c379c26 100644 --- a/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -29,6 +29,8 @@ bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) { CD_DEBUG("config: %s\n",config.toString().c_str()); + int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt(); + int manipulatorIndex = config.check("manipulatorIndex",-1,"manipulatorIndex").asInt(); double genRefSpeed = config.check("genRefSpeed",DEFAULT_GEN_REF_SPEED,"general ref speed").asDouble(); if ( ( config.check("env") ) && ( config.check("penv") ) ) @@ -73,19 +75,6 @@ bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) { 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, 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, currently '%d'.\n", manipulatorIndex); - return false; - } - if( config.check("orPlugin") ) { CD_DEBUG("Found --orPlugin parameter.\n"); @@ -164,10 +153,32 @@ bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) { std::vector vectorOfRobotPtr; penv->GetRobots(vectorOfRobotPtr); + if(robotIndex >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotIndex,vectorOfRobotPtr.size()); + return false; + } + else if (robotIndex < 0) + { + CD_ERROR("robotIndex %d < 0, not loading yarpPlugin.\n",robotIndex); + return false; + } + probot = vectorOfRobotPtr[robotIndex]; robotName = probot->GetName(); std::vector vectorOfManipulatorPtr = probot->GetManipulators(); + if(manipulatorIndex >= vectorOfManipulatorPtr.size()) + { + CD_ERROR("manipulatorIndex %d >= vectorOfManipulatorPtr.size() %d, not loading yarpPlugin.\n",manipulatorIndex,vectorOfManipulatorPtr.size()); + return false; + } + else if (manipulatorIndex < 0) + { + CD_ERROR("manipulatorIndex %d < 0, not loading yarpPlugin.\n",manipulatorIndex); + return false; + } + manipulatorIDs = vectorOfManipulatorPtr[manipulatorIndex]->GetArmIndices(); axes = manipulatorIDs.size(); From 582c121677bda757d99baac29bdcf960faf95262 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 4 Dec 2017 23:32:04 +0100 Subject: [PATCH 52/70] fix bugs, remove redundant --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 14 +++++++------- .../YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 8 +++----- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index e25ca666..9f01fb11 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -174,31 +174,31 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { return false; } - OpenRAVE::SensorBasePtr psensorbase = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + sensorPtrForCameras = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); - std::string tipo = psensorbase->GetName(); + std::string tipo = sensorPtrForCameras->GetName(); printf("Sensor %d name: %s\n",sensorIndex,tipo.c_str()); // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); - if ( ! psensorbase->Supports(OpenRAVE::SensorBase::ST_Camera) ) + if ( ! sensorPtrForCameras->Supports(OpenRAVE::SensorBase::ST_Camera) ) { CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); } // Activate the camera - psensorbase->Configure(OpenRAVE::SensorBase::CC_PowerOn); + sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window - //psensorbase->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(psensorbase->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorPtrForCameras->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); _width = pcamerageomdata->width; _height = pcamerageomdata->height; - cameraSensorDataPtr = boost::dynamic_pointer_cast(psensorbase->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorPtrForCameras->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index a4603d4e..5277251d 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -1,7 +1,7 @@ // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- -#ifndef __YARP_OPENRAVE_CONTROLBOARD_HPP__ -#define __YARP_OPENRAVE_CONTROLBOARD_HPP__ +#ifndef __YARP_OPENRAVE_GRABBER_HPP__ +#define __YARP_OPENRAVE_GRABBER_HPP__ #include #include @@ -17,8 +17,6 @@ #include "ColorDebug.hpp" -#define DEFAULT_GEN_REF_SPEED 7.5 // Exposed. - namespace roboticslab { @@ -112,4 +110,4 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF } // namespace roboticslab -#endif // __YARP_OPENRAVE_CONTROLBOARD_HPP__ +#endif // __YARP_OPENRAVE_GRABBER_HPP__ From 3fc7da2faaeaf469b7087e4a1d3ecdfc4a8e1eb8 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 00:05:16 +0100 Subject: [PATCH 53/70] comment out renderdata --- libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 9f01fb11..fd1aadd9 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -191,7 +191,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window - sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorPtrForCameras->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); From 5646f14a2e5689e6593c0cca78cb65f1badbb8bf Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 15:20:43 +0100 Subject: [PATCH 54/70] begin YarpOpenraveRGBDSensor as a copy of YarpOpenraveGrabber --- CMakeLists.txt | 1 + libraries/YarpPlugins/CMakeLists.txt | 1 + .../YarpOpenraveRGBDSensor/CMakeLists.txt | 50 ++++ .../DeviceDriverImpl.cpp | 216 ++++++++++++++++++ .../IFrameGrabberImageImpl.cpp | 51 +++++ .../YarpOpenraveRGBDSensor/README.md | 3 + .../YarpOpenraveRGBDSensor.hpp | 113 +++++++++ .../YarpOpenraveRGBDSensor.ini | 7 + 8 files changed, 442 insertions(+) create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/README.md create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp create mode 100644 libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.ini diff --git a/CMakeLists.txt b/CMakeLists.txt index 451be7c0..d8f78670 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ option(ENABLE_FakeControlboard "Enable/disable compilation of FakeControlboard" option(ENABLE_YarpOpenraveControlboard "Enable/disable option YarpOpenraveControlboard" TRUE) option(ENABLE_YarpOpenraveControlboardCollision "Enable/disable option YarpOpenraveControlboardCollision" TRUE) option(ENABLE_YarpOpenraveGrabber "Enable/disable option YarpOpenraveGrabber" TRUE) +option(ENABLE_YarpOpenraveRGBDSensor "Enable/disable option YarpOpenraveRGBDSensor" TRUE) ### options: cpp libraries (openrave plugins) option(ENABLE_OpenraveYarpControlboard "Enable/disable option OpenraveYarpControlboard" TRUE) diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt index b3298c1f..e57206d6 100644 --- a/libraries/YarpPlugins/CMakeLists.txt +++ b/libraries/YarpPlugins/CMakeLists.txt @@ -25,6 +25,7 @@ endif () add_subdirectory(YarpOpenraveControlboardCollision) add_subdirectory(FakeControlboard) add_subdirectory(YarpOpenraveGrabber) + add_subdirectory(YarpOpenraveRGBDSensor) # yarp_end_plugin_library(yarpplugins) # IF (ENABLE_launchyarpPlugins) diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt new file mode 100644 index 00000000..f5570f7f --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt @@ -0,0 +1,50 @@ +# Copyright: (C) 2013 Universidad Carlos III de Madrid +# Author: Juan G. Victores + +yarp_prepare_plugin(YarpOpenraveRGBDSensor + CATEGORY device + TYPE roboticslab::YarpOpenraveRGBDSensor + INCLUDE YarpOpenraveRGBDSensor.hpp + WRAPPER grabber) + +IF (NOT SKIP_YarpOpenraveRGBDSensor) + +SET(CMAKE_MODULE_PATH ${TEO_MODULE_PATH} ${CMAKE_MODULE_PATH}) +FIND_PACKAGE(YARP REQUIRED) +FIND_PACKAGE(OpenRAVE REQUIRED) +find_package(Boost COMPONENTS iostreams python thread system) + +if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX ) + add_definitions("-fno-strict-aliasing -Wall") +endif( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX ) + +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR} ${OpenRAVE_INCLUDE_DIRS}) # kdl name depends on version +if( Boost_INCLUDE_DIRS ) + include_directories(${Boost_INCLUDE_DIRS}) +endif() + +LINK_DIRECTORIES(${OPENRAVE_YARP_PLUGINS_LINK_DIRS}) + +YARP_ADD_PLUGIN(YarpOpenraveRGBDSensor YarpOpenraveRGBDSensor.hpp DeviceDriverImpl.cpp IFrameGrabberImageImpl.cpp) +add_dependencies(YarpOpenraveRGBDSensor COLOR_DEBUG) +set_target_properties(${KEYWORD} PROPERTIES COMPILE_FLAGS "${OpenRAVE_CXXFLAGS}") +set_target_properties(${KEYWORD} PROPERTIES LINK_FLAGS "${OpenRAVE_LINK_FLAGS}") +TARGET_LINK_LIBRARIES(YarpOpenraveRGBDSensor ${OpenRAVE_LIBRARIES} ${OpenRAVE_CORE_LIBRARIES} ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${YARP_LIBRARIES}) + +# Exporting dependencies for TEOConfig.cmake quite manually for now... +set(TEO_INCLUDE_DIRS ${TEO_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} CACHE INTERNAL "appended libraries") +set(TEO_LIBRARIES ${TEO_LIBRARIES} TeoYarp YarpOpenraveRGBDSensor CACHE INTERNAL "appended libraries") + + #install(TARGETS YarpOpenraveRGBDSensor DESTINATION lib) + yarp_install(TARGETS YarpOpenraveRGBDSensor + EXPORT YARP + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) + + yarp_install(FILES YarpOpenraveRGBDSensor.ini + COMPONENT runtime + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + +ENDIF (NOT SKIP_YarpOpenraveRGBDSensor) + diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp new file mode 100644 index 00000000..2dc5e8ca --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -0,0 +1,216 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveRGBDSensor.hpp" + +namespace roboticslab +{ + +// ------------------------------------------------------------------- + +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername) +{ + 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): + viewer->main(true); +} + +// ------------------- DeviceDriver Related ------------------------------------ + +bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { + + CD_DEBUG("config: %s\n",config.toString().c_str()); + + int robotIndex = config.check("robotIndex",-1,"robotIndex").asInt(); + int sensorIndex = config.check("sensorIndex",-1,"sensorIndex").asInt(); + + 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"); + + // Initialize OpenRAVE-core + OpenRAVE::RaveInitialize(true); // Start openrave core + penv = OpenRAVE::RaveCreateEnvironment(); // Create the main OpenRAVE environment, set the EnvironmentBasePtr + penv->StopSimulation(); // NEEDED?? + + if ( config.check("view") ) + { + boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin")); + openraveThreads.add_thread(&openraveViewerThread); + 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; + } + + if( config.check("orPlugin") ) + { + CD_DEBUG("Found --orPlugin parameter.\n"); + + std::string orPluginAndModuleName = config.find("orPlugin").asString(); + if ( ! OpenRAVE::RaveLoadPlugin(orPluginAndModuleName) ) + { + CD_ERROR("Could not load plugin '%s'\n",orPluginAndModuleName.c_str()); + return false; + } + OpenRAVE::ModuleBasePtr pModule = OpenRAVE::RaveCreateModule(penv,orPluginAndModuleName); // create the module + penv->Add(pModule,true); // load the module, calls main and also enables good destroy. + std::stringstream cmdin,cmdout; + cmdin << "open"; + // RAVELOG_INFO("%s\n",cmdin.str().c_str()); + if( ! pModule->SendCommand(cmdout,cmdin) ) + { + CD_ERROR("Bad send 'open' command.\n"); + } + CD_SUCCESS("Sent 'open' command.\n"); + } + + if( config.check("orPlugins") ) + { + CD_DEBUG("Found --orPlugins parameter.\n"); + + if( ! config.find("orPlugins").isList() ) + { + CD_ERROR("orPlugins usage from CLI: (read code) ->. Bye!\n"); //--orPlugins "(plugin1 (module module1) (commands \"open port\"))" + return false; + } + yarp::os::Bottle orPlugins = config.findGroup("orPlugins"); + CD_DEBUG("orPlugins: %s\n",orPlugins.toString().c_str()); + for(int i=1; i. Bye!\n"); //--orPlugins "(plugin1 (module module1) (commands \"open port\"))" + return false; + } + yarp::os::Bottle* orPlugin = orPlugins.get(i).asList(); + CD_DEBUG("orPlugin[%d]: %s\n",i,orPlugin->toString().c_str()); + std::string orPluginName = orPlugin->get(0).asString(); + CD_DEBUG("* orPlugin[%d]: plugin: %s\n",i,orPluginName.c_str()); + std::string orModuleName = orPlugin->find("module").asString(); + CD_DEBUG("* orPlugin[%d]: module: %s\n",i,orModuleName.c_str()); + if( orPlugin->check("commands") ) + { + CD_DEBUG("* orPlugin[%d]: commands: %s\n",i,orPlugin->find("commands").asString().c_str()); + } + + //-- Load plugin (docs say will reload if already loaded) + if ( ! OpenRAVE::RaveLoadPlugin(orPluginName) ) + { + CD_ERROR("Could not load plugin '%s'\n",orPluginName.c_str()); + return false; + } + + //-- Load module from plugin + OpenRAVE::ModuleBasePtr pModule = OpenRAVE::RaveCreateModule(penv,orModuleName); // create the module + penv->Add(pModule,true); // load the module, calls main and also enables good destroy. + //-- Send command if exist + if( orPlugin->check("commands") ) + { + std::stringstream cmdin,cmdout; + cmdin << orPlugin->find("commands").asString(); + // RAVELOG_INFO("%s\n",cmdin.str().c_str()); + if( ! pModule->SendCommand(cmdout,cmdin) ) + { + CD_ERROR("Bad send '%s' command.\n",cmdin.str().c_str()); + } + CD_SUCCESS("Sent '%s' command.\n",cmdin.str().c_str()); + } + } + } + + std::vector vectorOfRobotPtr; + penv->GetRobots(vectorOfRobotPtr); + if(robotIndex >= vectorOfRobotPtr.size()) + { + CD_ERROR("robotIndex %d >= vectorOfRobotPtr.size() %d, not loading yarpPlugin.\n",robotIndex,vectorOfRobotPtr.size()); + return false; + } + else if (robotIndex < 0) + { + CD_ERROR("robotIndex %d < 0, not loading yarpPlugin.\n",robotIndex); + return false; + } + + probot = vectorOfRobotPtr[robotIndex]; + robotName = probot->GetName(); + + std::vector vectorOfSensorPtr = vectorOfRobotPtr.at(robotIndex)->GetAttachedSensors(); + if(sensorIndex >= vectorOfSensorPtr.size()) + { + CD_ERROR("sensorIndex %d >= vectorOfSensorPtr.size() %d, not loading yarpPlugin.\n",sensorIndex,vectorOfSensorPtr.size()); + return false; + } + else if (sensorIndex < 0) + { + CD_ERROR("sensorIndex %d < 0, not loading yarpPlugin.\n",sensorIndex); + return false; + } + + sensorPtrForCameras = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + + std::string tipo = sensorPtrForCameras->GetName(); + + printf("Sensor %d name: %s\n",sensorIndex,tipo.c_str()); + + // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); + + if ( ! sensorPtrForCameras->Supports(OpenRAVE::SensorBase::ST_Camera) ) + { + CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); + } + + // Activate the camera + sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_PowerOn); + + // Show the camera image in a separate window + //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + // Get some camera parameter info + boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorPtrForCameras->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); + _width = pcamerageomdata->width; + _height = pcamerageomdata->height; + + cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorPtrForCameras->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + + return true; +} + +// ----------------------------------------------------------------------------- + +bool YarpOpenraveRGBDSensor::close() { + printf("[YarpOpenraveRGBDSensor] close()\n"); + return true; +} + +// ----------------------------------------------------------------------------- + +} // namespace roboticslab + diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp new file mode 100644 index 00000000..fd61afd9 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp @@ -0,0 +1,51 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveRGBDSensor.hpp" + +namespace roboticslab +{ + +// ------------------ IFrameGrabberImage Related ---------------------------------------- + +bool YarpOpenraveRGBDSensor::getImage(yarp::sig::ImageOf& image) +{ + sensorPtrForCameras->GetSensorData(cameraSensorDataPtr); + + //std::vector currentFrame = pcamerasensordata->vimagedata; + //printf("Vector size: %d\n",currentFrame.size()); // i.e. 480 * 640 * 3 = 921600; + //yarp::sig::ImageOf& i_imagen = ptrVectorOfRgbPortPtr->at(camIter)->prepare(); + //i_imagen.resize(ptrVectorOfCameraWidth->at(camIter),ptrVectorOfCameraHeight->at(camIter)); // Tamaño de la pantalla + yarp::sig::PixelRgb p; + for (int i_x = 0; i_x < image.width(); ++i_x) + { + for (int i_y = 0; i_y < image.height(); ++i_y) + { + p.r = cameraSensorDataPtr->vimagedata[3*(i_x+(i_y*image.width()))]; + p.g = cameraSensorDataPtr->vimagedata[1+3*(i_x+(i_y*image.width()))]; + p.b = cameraSensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; + image.safePixel(i_x,i_y) = p; + } + } + + return true; +} + +// ---------------------------------------------------------------------------- + +int YarpOpenraveRGBDSensor::height() const +{ + CD_DEBUG("%d\n", _height); + return _height; +} + +// ---------------------------------------------------------------------------- + +int YarpOpenraveRGBDSensor::width() const +{ + CD_DEBUG("%d\n", _width); + return _width; +} + +// ---------------------------------------------------------------------------- + +} diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/README.md b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/README.md new file mode 100644 index 00000000..68cc9a88 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/README.md @@ -0,0 +1,3 @@ +# YarpOpenraveRGBDSensor + +Example invocation: `yarpdev --device YarpOpenraveRGBDSensor --name /teoSim/rightArm --env /usr/local/share/teo-openrave-models/contexts/openrave/teo/teo.robot.xml --robotIndex 0 --manipulatorIndex 2 --view` diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp new file mode 100644 index 00000000..57229919 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -0,0 +1,113 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#ifndef __YARP_OPENRAVE_GRABBER_HPP__ +#define __YARP_OPENRAVE_GRABBER_HPP__ + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "ColorDebug.hpp" + +namespace roboticslab +{ + + +// Specific for --env parameter +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername); + +/** + * @ingroup TeoYarp + * \defgroup YarpOpenraveRGBDSensor + * + * @brief Contains teo::YarpOpenraveRGBDSensor. + * + * @section YarpOpenraveRGBDSensor_install Installation + * + * The plugin is compiled when ENABLE_TeoYarp_YarpOpenraveRGBDSensor is activated (not default). For further + * installation steps refer to your own system installation guidelines. + */ + +/** + * @ingroup YarpOpenraveRGBDSensor + * @brief Implements the YARP_dev IFrameGrabberImage, etc. + * interface class member functions. + */ +class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev::IFrameGrabberImage { +public: + + // Set the Thread Rate in the class constructor + YarpOpenraveRGBDSensor() {} + + // ------- DeviceDriver declarations. Implementation in DeviceDriverImageImpl.cpp ------- + /** + * Open the DeviceDriver. + * @param config is a list of parameters for the device. + * Which parameters are effective for your device can vary. + * See \ref dev_examples "device invocation examples". + * If there is no example for your device, + * you can run the "yarpdev" program with the verbose flag + * set to probe what parameters the device is checking. + * If that fails too, + * you'll need to read the source code (please nag one of the + * yarp developers to add documentation for your device). + * @return true/false upon success/failure + */ + virtual bool open(yarp::os::Searchable& config); + + /** + * Close the DeviceDriver. + * @return true/false on success/failure. + */ + virtual bool close(); + + // ------- IFrameGrabberImage declarations. Implementation in IFrameGrabberImageImpl.cpp ------- + /** + * Get an rgb image from the frame grabber, if required + * demosaicking/color reconstruction is applied + * + * @param image the image to be filled + * @return true/false upon success/failure + */ + virtual bool getImage(yarp::sig::ImageOf& image); + + /** + * Return the height of each frame. + * @return image height + */ + virtual int height() const; + + /** + * Return the width of each frame. + * @return image width + */ + virtual int width() const; + +private: + + // General Grabber parameters // + int _height, _width; + + //OpenRAVE// + OpenRAVE::EnvironmentBasePtr penv; + OpenRAVE::RobotBasePtr probot; + std::string robotName; + + OpenRAVE::SensorBasePtr sensorPtrForCameras; + boost::shared_ptr cameraSensorDataPtr; + + // Specific for --env parameter with --view + boost::thread_group openraveThreads; +}; + +} // namespace roboticslab + +#endif // __YARP_OPENRAVE_GRABBER_HPP__ diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.ini b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.ini new file mode 100644 index 00000000..34015950 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.ini @@ -0,0 +1,7 @@ +[plugin YarpOpenraveRGBDSensor] +type device +name YarpOpenraveRGBDSensor +library YarpOpenraveRGBDSensor +part YarpOpenraveRGBDSensor +code "YarpOpenraveRGBDSensor" +wrapper grabber From 9c2a409002b8bea08d65764ff8841904967f7e58 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 15:55:27 +0100 Subject: [PATCH 55/70] inherit fromm IFrameGrabberImage->IRGBDSensor --- .../IFrameGrabberImageImpl.cpp | 24 +--- .../YarpOpenraveRGBDSensor.hpp | 103 +++++++++++++++--- 2 files changed, 94 insertions(+), 33 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp index fd61afd9..46d1d221 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp @@ -5,11 +5,11 @@ namespace roboticslab { -// ------------------ IFrameGrabberImage Related ---------------------------------------- +// ------------------ IRGBDSensor Related ---------------------------------------- -bool YarpOpenraveRGBDSensor::getImage(yarp::sig::ImageOf& image) +bool YarpOpenraveRGBDSensor::getDepthImage(yarp::sig::ImageOf &depthImage, yarp::os::Stamp *timeStamp) { - sensorPtrForCameras->GetSensorData(cameraSensorDataPtr); + /*sensorPtrForCameras->GetSensorData(cameraSensorDataPtr); //std::vector currentFrame = pcamerasensordata->vimagedata; //printf("Vector size: %d\n",currentFrame.size()); // i.e. 480 * 640 * 3 = 921600; @@ -25,27 +25,11 @@ bool YarpOpenraveRGBDSensor::getImage(yarp::sig::ImageOf& i p.b = cameraSensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; image.safePixel(i_x,i_y) = p; } - } + }*/ return true; } // ---------------------------------------------------------------------------- -int YarpOpenraveRGBDSensor::height() const -{ - CD_DEBUG("%d\n", _height); - return _height; -} - -// ---------------------------------------------------------------------------- - -int YarpOpenraveRGBDSensor::width() const -{ - CD_DEBUG("%d\n", _width); - return _width; -} - -// ---------------------------------------------------------------------------- - } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp index 57229919..4d3eddef 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -41,7 +41,7 @@ void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername) * @brief Implements the YARP_dev IFrameGrabberImage, etc. * interface class member functions. */ -class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev::IFrameGrabberImage { +class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev::IRGBDSensor { public: // Set the Thread Rate in the class constructor @@ -69,27 +69,104 @@ class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev: */ virtual bool close(); - // ------- IFrameGrabberImage declarations. Implementation in IFrameGrabberImageImpl.cpp ------- + // ------- IRGBDSensor declarations. Implementation in IRGBDSensorImpl.cpp ------- + /* + * IRgbVisualParams interface. Look at IVisualParams.h for documentation + */ + virtual int getRgbHeight() {} + virtual int getRgbWidth() {} + virtual bool getRgbSupportedConfigurations(yarp::sig::VectorOf &configurations) {} + virtual bool getRgbResolution(int &width, int &height) {} + virtual bool setRgbResolution(int width, int height) {} + virtual bool getRgbFOV(double &horizontalFov, double &verticalFov) {} + virtual bool setRgbFOV(double horizontalFov, double verticalFov) {} + virtual bool getRgbIntrinsicParam(yarp::os::Property &intrinsic) {} + virtual bool getRgbMirroring(bool &mirror) {} + virtual bool setRgbMirroring(bool mirror) {} + + /* + * IDepthVisualParams interface. Look at IVisualParams.h for documentation + */ + virtual int getDepthHeight() {} + virtual int getDepthWidth() {} + virtual bool setDepthResolution(int width, int height) {} + virtual bool getDepthFOV(double &horizontalFov, double &verticalFov) {} + virtual bool setDepthFOV(double horizontalFov, double verticalFov) {} + virtual double getDepthAccuracy() {} + virtual bool setDepthAccuracy(double accuracy) {} + virtual bool getDepthClipPlanes(double &nearPlane, double &farPlane) {} + virtual bool setDepthClipPlanes(double nearPlane, double farPlane) {} + virtual bool getDepthIntrinsicParam(yarp::os::Property &intrinsic) {} + virtual bool getDepthMirroring(bool &mirror) {} + virtual bool setDepthMirroring(bool mirror) {} + + /* + * IRGBDSensor specific interface methods + */ + /** - * Get an rgb image from the frame grabber, if required - * demosaicking/color reconstruction is applied + * Get the extrinsic parameters ofrom the device + * @param extrinsic return a rototranslation matrix describing the position + * of the depth optical frame with respect to the rgb frame + * @return true if success + */ + virtual bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) {} + + /** + * Return an error message in case of error. For debugging purpose and user notification. + * Error message will be reset after any succesful command + * @return A string explaining the last error occurred. + */ + virtual yarp::os::ConstString getLastErrorMsg(yarp::os::Stamp *timeStamp = NULL) {} + + /** + * Get the rgb frame from the device. + * The pixel type of the source image will usually be set as a VOCAB_PIXEL_RGB, + * but the user can call the function with the pixel type of his/her choise. The convertion + * if possible, will be done automatically on client side (TO BO VERIFIED). + * Note: this will consume CPU power because it will not use GPU optimization. + * Use VOCAB_PIXEL_RGB for best performances. * - * @param image the image to be filled - * @return true/false upon success/failure + * @param rgbImage the image to be filled. + * @param timeStamp time in which the image was acquired. Optional, the user must provide memory allocation + * @return True on success */ - virtual bool getImage(yarp::sig::ImageOf& image); + virtual bool getRgbImage(yarp::sig::FlexImage &rgbImage, yarp::os::Stamp *timeStamp = NULL) {} /** - * Return the height of each frame. - * @return image height + * Get the depth frame from the device. + * The pixel type of the source image will usually be set as a VOCAB_PIXEL_RGB, + * but the user can call the function with the pixel type of his/her choise. The convertion + * if possible, will be done automatically on client side. + * Note: this will consume CPU power because it will not use GPU optimization. + * Use VOCAB_PIXEL_RGB for best performances. + * + * @param rgbImage the image to be filled. + * @param timeStamp time in which the image was acquired. Optional, the user must provide memory allocation + * @return True on success */ - virtual int height() const; + virtual bool getDepthImage(yarp::sig::ImageOf &depthImage, yarp::os::Stamp *timeStamp = NULL); /** - * Return the width of each frame. - * @return image width + * Get the both the color and depth frame in a single call. Implementation should assure the best possible synchronization + * is achieved accordingly to synch policy set by the user. + * TimeStamps are referred to acquisition time of the corresponding piece of information. + * If the device is not providing TimeStamps, then 'timeStamp' field should be set to '-1'. + * @param colorFrame pointer to FlexImage data to hold the color frame from the sensor + * @param depthFrame pointer to FlexImage data to hold the depth frame from the sensor + * @param colorStamp pointer to memory to hold the Stamp of the color frame + * @param depthStamp pointer to memory to hold the Stamp of the depth frame + * @return true if able to get both data. + */ + virtual bool getImages(yarp::sig::FlexImage &colorFrame, yarp::sig::ImageOf &depthFrame, yarp::os::Stamp *colorStamp=NULL, yarp::os::Stamp *depthStamp=NULL) {} + + /** + * Get the surrent status of the sensor, using enum type + * + * @return an enum representing the status of the robot or an error code + * if any error is present */ - virtual int width() const; + virtual RGBDSensor_status getSensorStatus() {} private: From 9d2faf11d478d740a662632dbf4e6572e9af7cc1 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 15:58:05 +0100 Subject: [PATCH 56/70] IFrameGrabberImageImpl.cpp -> IRGBDSensorImpl.cpp --- libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt | 2 +- .../{IFrameGrabberImageImpl.cpp => IRGBDSensorImpl.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename libraries/YarpPlugins/YarpOpenraveRGBDSensor/{IFrameGrabberImageImpl.cpp => IRGBDSensorImpl.cpp} (100%) diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt index f5570f7f..7ed5f4d9 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt @@ -25,7 +25,7 @@ endif() LINK_DIRECTORIES(${OPENRAVE_YARP_PLUGINS_LINK_DIRS}) -YARP_ADD_PLUGIN(YarpOpenraveRGBDSensor YarpOpenraveRGBDSensor.hpp DeviceDriverImpl.cpp IFrameGrabberImageImpl.cpp) +YARP_ADD_PLUGIN(YarpOpenraveRGBDSensor YarpOpenraveRGBDSensor.hpp DeviceDriverImpl.cpp IRGBDSensorImpl.cpp) add_dependencies(YarpOpenraveRGBDSensor COLOR_DEBUG) set_target_properties(${KEYWORD} PROPERTIES COMPILE_FLAGS "${OpenRAVE_CXXFLAGS}") set_target_properties(${KEYWORD} PROPERTIES LINK_FLAGS "${OpenRAVE_LINK_FLAGS}") diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp similarity index 100% rename from libraries/YarpPlugins/YarpOpenraveRGBDSensor/IFrameGrabberImageImpl.cpp rename to libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp From 7bb72d7662d4437b38a2f2be0f3f78a43d618689 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 20:36:18 +0100 Subject: [PATCH 57/70] refractor sensorPtrForCameras->sensorBasePtr --- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 12 ++++++------ .../YarpOpenraveRGBDSensor.hpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 2dc5e8ca..2858838d 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -174,31 +174,31 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { return false; } - sensorPtrForCameras = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + sensorBasePtr = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); - std::string tipo = sensorPtrForCameras->GetName(); + std::string tipo = sensorBasePtr->GetName(); printf("Sensor %d name: %s\n",sensorIndex,tipo.c_str()); // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); - if ( ! sensorPtrForCameras->Supports(OpenRAVE::SensorBase::ST_Camera) ) + if ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Camera) ) { CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); } // Activate the camera - sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_PowerOn); + sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorPtrForCameras->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); _width = pcamerageomdata->width; _height = pcamerageomdata->height; - cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorPtrForCameras->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp index 4d3eddef..a5784601 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -178,7 +178,7 @@ class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev: OpenRAVE::RobotBasePtr probot; std::string robotName; - OpenRAVE::SensorBasePtr sensorPtrForCameras; + OpenRAVE::SensorBasePtr sensorBasePtr; boost::shared_ptr cameraSensorDataPtr; // Specific for --env parameter with --view From bf0aec5fade4015bc8e141f33faa5ec1e2f811d7 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 20:53:16 +0100 Subject: [PATCH 58/70] refractor sensorPtrForCameras->sensorBasePtr --- .../YarpOpenraveGrabber/DeviceDriverImpl.cpp | 12 ++++++------ .../YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp | 2 +- .../YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index fd1aadd9..f0f91399 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -174,31 +174,31 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { return false; } - sensorPtrForCameras = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + sensorBasePtr = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); - std::string tipo = sensorPtrForCameras->GetName(); + std::string tipo = sensorBasePtr->GetName(); printf("Sensor %d name: %s\n",sensorIndex,tipo.c_str()); // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); - if ( ! sensorPtrForCameras->Supports(OpenRAVE::SensorBase::ST_Camera) ) + if ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Camera) ) { CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); } // Activate the camera - sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_PowerOn); + sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorPtrForCameras->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); _width = pcamerageomdata->width; _height = pcamerageomdata->height; - cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorPtrForCameras->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index c03e8c89..6904ba6d 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -9,7 +9,7 @@ namespace roboticslab bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& image) { - sensorPtrForCameras->GetSensorData(cameraSensorDataPtr); + sensorBasePtr->GetSensorData(cameraSensorDataPtr); //std::vector currentFrame = pcamerasensordata->vimagedata; //printf("Vector size: %d\n",currentFrame.size()); // i.e. 480 * 640 * 3 = 921600; diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index 5277251d..88622a01 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -101,7 +101,7 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF OpenRAVE::RobotBasePtr probot; std::string robotName; - OpenRAVE::SensorBasePtr sensorPtrForCameras; + OpenRAVE::SensorBasePtr sensorBasePtr; boost::shared_ptr cameraSensorDataPtr; // Specific for --env parameter with --view From 0d4b8fdd83493443d056b30d93f0662243876661 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:00:34 +0100 Subject: [PATCH 59/70] cameraSensorDataPtr->sensorDataPtr --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 +- .../YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp | 8 ++++---- .../YarpOpenraveGrabber/YarpOpenraveGrabber.hpp | 2 +- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 2 +- .../YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index f0f91399..8dc8d008 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -198,7 +198,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { _width = pcamerageomdata->width; _height = pcamerageomdata->height; - cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp index 6904ba6d..d9bd9946 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -9,7 +9,7 @@ namespace roboticslab bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& image) { - sensorBasePtr->GetSensorData(cameraSensorDataPtr); + sensorBasePtr->GetSensorData(sensorDataPtr); //std::vector currentFrame = pcamerasensordata->vimagedata; //printf("Vector size: %d\n",currentFrame.size()); // i.e. 480 * 640 * 3 = 921600; @@ -20,9 +20,9 @@ bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& imag { for (int i_y = 0; i_y < image.height(); ++i_y) { - p.r = cameraSensorDataPtr->vimagedata[3*(i_x+(i_y*image.width()))]; - p.g = cameraSensorDataPtr->vimagedata[1+3*(i_x+(i_y*image.width()))]; - p.b = cameraSensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; + p.r = sensorDataPtr->vimagedata[3*(i_x+(i_y*image.width()))]; + p.g = sensorDataPtr->vimagedata[1+3*(i_x+(i_y*image.width()))]; + p.b = sensorDataPtr->vimagedata[2+3*(i_x+(i_y*image.width()))]; image.safePixel(i_x,i_y) = p; } } diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp index 88622a01..573ca350 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp @@ -102,7 +102,7 @@ class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IF std::string robotName; OpenRAVE::SensorBasePtr sensorBasePtr; - boost::shared_ptr cameraSensorDataPtr; + boost::shared_ptr sensorDataPtr; // Specific for --env parameter with --view boost::thread_group openraveThreads; diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 2858838d..c4a1161a 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -198,7 +198,7 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { _width = pcamerageomdata->width; _height = pcamerageomdata->height; - cameraSensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp index a5784601..10afc66f 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -179,7 +179,7 @@ class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev: std::string robotName; OpenRAVE::SensorBasePtr sensorBasePtr; - boost::shared_ptr cameraSensorDataPtr; + boost::shared_ptr sensorDataPtr; // Specific for --env parameter with --view boost::thread_group openraveThreads; From d7ac72dbd9d424d53be8686ca805e4c3b77c95e9 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:04:06 +0100 Subject: [PATCH 60/70] pcamerageomdata->geomDataPtr --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 8 ++++---- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 8dc8d008..3b5b5026 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -193,10 +193,10 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { // Show the camera image in a separate window //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); - CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); - _width = pcamerageomdata->width; - _height = pcamerageomdata->height; + boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); + _width = geomDataPtr->width; + _height = geomDataPtr->height; sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index c4a1161a..022228dc 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -193,10 +193,10 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // Show the camera image in a separate window //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr pcamerageomdata = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); - CD_DEBUG("Camera width: %d, height: %d.\n",pcamerageomdata->width,pcamerageomdata->height); - _width = pcamerageomdata->width; - _height = pcamerageomdata->height; + boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); + _width = geomDataPtr->width; + _height = geomDataPtr->height; sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); From ee7048de68abb2704352e7bc694f590689cf8026 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:07:50 +0100 Subject: [PATCH 61/70] start specializing sensors --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 +- .../YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 3b5b5026..9863fee2 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -187,7 +187,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); } - // Activate the camera + // Activate the sensor sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 022228dc..86e8f529 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -182,12 +182,12 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // printf("Sensor %d description: %s\n",sensorIter,psensorbase->GetDescription().c_str()); - if ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Camera) ) + if ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Laser) ) { - CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); + CD_ERROR("Sensor %d does not support ST_Laser.\n", sensorIndex ); } - // Activate the camera + // Activate the sensor sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window From 2b01d59e3e96e128b2956e759aa9867f34a69a7f Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:08:31 +0100 Subject: [PATCH 62/70] start specializing sensors --- libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 -- .../YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 9863fee2..f3a457ba 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -190,8 +190,6 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { // Activate the sensor sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); - // Show the camera image in a separate window - //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 86e8f529..ff45bfff 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -191,7 +191,7 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); // Show the camera image in a separate window - //sensorPtrForCameras->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); From 5d9e7884677e7d89993361746f9422b6fec607a9 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:17:50 +0100 Subject: [PATCH 63/70] continue laser specialization --- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 9 ++++----- .../YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index ff45bfff..06f35831 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -193,12 +193,11 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // Show the camera image in a separate window //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); // Get some camera parameter info - boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); - CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); - _width = geomDataPtr->width; - _height = geomDataPtr->height; + boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Laser)); - sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + // + + sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Laser)); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp index 10afc66f..98966fa9 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -179,7 +179,7 @@ class YarpOpenraveRGBDSensor : public yarp::dev::DeviceDriver, public yarp::dev: std::string robotName; OpenRAVE::SensorBasePtr sensorBasePtr; - boost::shared_ptr sensorDataPtr; + boost::shared_ptr sensorDataPtr; // Specific for --env parameter with --view boost::thread_group openraveThreads; From 392d0bb16bc504ea597e6cd3a87b7c92be04bb25 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:20:46 +0100 Subject: [PATCH 64/70] continue laser specialization --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 5 ++++- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 8 +++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index f3a457ba..028f201f 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -190,9 +190,12 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { // Activate the sensor sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); + // Show the sensor image in a separate window + //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + // Get some camera parameter info boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); - CD_DEBUG("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); + CD_INFO("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); _width = geomDataPtr->width; _height = geomDataPtr->height; diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 06f35831..31e191cf 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -190,12 +190,14 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // Activate the sensor sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); - // Show the camera image in a separate window + // Show the sensor image in a separate window // Ok for Laser??? //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + // Get some camera parameter info boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Laser)); - - // + CD_INFO("Laser resolution: %f %f.\n",geomDataPtr->resolution[0],geomDataPtr->resolution[1]); + CD_INFO("Laser min_angle: %f %f.\n",geomDataPtr->min_angle[0],geomDataPtr->min_angle[1]); + CD_INFO("Laser max_angle: %f %f.\n",geomDataPtr->max_angle[0],geomDataPtr->max_angle[1]); sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Laser)); From 5fb13db78e29662c273103ea4bf3fa7f6ae37be7 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:22:20 +0100 Subject: [PATCH 65/70] add openrave-YarpPluginLoader-rgbdsensor.py as copy of grabber --- .../openrave-YarpPluginLoader-rgbdsensor.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 examples/openrave-YarpPluginLoader-rgbdsensor.py diff --git a/examples/openrave-YarpPluginLoader-rgbdsensor.py b/examples/openrave-YarpPluginLoader-rgbdsensor.py new file mode 100644 index 00000000..47e1c839 --- /dev/null +++ b/examples/openrave-YarpPluginLoader-rgbdsensor.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import openravepy +from openravepy import * + +try: + RaveInitialize() + + if not RaveLoadPlugin('OpenraveYarpPluginLoader'): + raveLogError("Plugin not correctly loaded") + + env=Environment() + env.SetViewer('qtcoin') + env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') + + OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') + print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 1 --prefix /drl') + + while 1: + pass + +finally: + RaveDestroy() + From 9b9f129bca461d0a40b5b3b5182ffd3bc0ac5345 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:23:32 +0100 Subject: [PATCH 66/70] rgbdsensor.py load YarpOpenraveRGBDSensor --- examples/openrave-YarpPluginLoader-rgbdsensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/openrave-YarpPluginLoader-rgbdsensor.py b/examples/openrave-YarpPluginLoader-rgbdsensor.py index 47e1c839..7e81abba 100644 --- a/examples/openrave-YarpPluginLoader-rgbdsensor.py +++ b/examples/openrave-YarpPluginLoader-rgbdsensor.py @@ -14,7 +14,7 @@ env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveGrabber --robotIndex 0 --sensorIndex 1 --prefix /drl') + print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveRGBDSensor --robotIndex 1 --sensorIndex 1 --prefix /drl') while 1: pass From de44b83fab03605b0954576b6c4832ec3afceec4 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:24:11 +0100 Subject: [PATCH 67/70] fix index --- examples/openrave-YarpPluginLoader-rgbdsensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/openrave-YarpPluginLoader-rgbdsensor.py b/examples/openrave-YarpPluginLoader-rgbdsensor.py index 7e81abba..8c3ada34 100644 --- a/examples/openrave-YarpPluginLoader-rgbdsensor.py +++ b/examples/openrave-YarpPluginLoader-rgbdsensor.py @@ -14,7 +14,7 @@ env.Load('/usr/local/share/robotDevastation-openrave-models/contexts/openrave/ecro/mapping_room.env.xml') OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') - print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveRGBDSensor --robotIndex 1 --sensorIndex 1 --prefix /drl') + print OpenraveYarpPluginLoader.SendCommand('open --device YarpOpenraveRGBDSensor --robotIndex 0 --sensorIndex 0 --prefix /drl') while 1: pass From fd9709b7c25449a21d931e04fd9b1e22151ff462 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:28:41 +0100 Subject: [PATCH 68/70] make grabber and rgbd more similar --- libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 2 +- .../YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index 028f201f..a1fae357 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -207,7 +207,7 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { // ----------------------------------------------------------------------------- bool YarpOpenraveGrabber::close() { - printf("[YarpOpenraveGrabber] close()\n"); + CD_INFO("\n"); return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index 31e191cf..f4737b0c 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -207,7 +207,7 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // ----------------------------------------------------------------------------- bool YarpOpenraveRGBDSensor::close() { - printf("[YarpOpenraveRGBDSensor] close()\n"); + CD_INFO("\n"); return true; } From 5943ea4c0f87101702ce44889895af0774c74155 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Tue, 5 Dec 2017 21:51:21 +0100 Subject: [PATCH 69/70] make grabber and rgbd more similar --- .../YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp | 8 +++++--- .../YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp | 8 +++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp index a1fae357..809159ea 100644 --- a/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -193,14 +193,16 @@ bool YarpOpenraveGrabber::open(yarp::os::Searchable& config) { // Show the sensor image in a separate window //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); - // Get some camera parameter info + // Get pointer to geom properties of sensor boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Camera)); + + // Get pointer to sensed data + sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); + CD_INFO("Camera width: %d, height: %d.\n",geomDataPtr->width,geomDataPtr->height); _width = geomDataPtr->width; _height = geomDataPtr->height; - sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Camera)); - return true; } diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp index f4737b0c..a9e5d7f8 100644 --- a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -193,14 +193,16 @@ bool YarpOpenraveRGBDSensor::open(yarp::os::Searchable& config) { // Show the sensor image in a separate window // Ok for Laser??? //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); - // Get some camera parameter info + // Get pointer to geom properties of sensor boost::shared_ptr geomDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->GetSensorGeometry(OpenRAVE::SensorBase::ST_Laser)); + + // Get pointer to sensed data + sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Laser)); + CD_INFO("Laser resolution: %f %f.\n",geomDataPtr->resolution[0],geomDataPtr->resolution[1]); CD_INFO("Laser min_angle: %f %f.\n",geomDataPtr->min_angle[0],geomDataPtr->min_angle[1]); CD_INFO("Laser max_angle: %f %f.\n",geomDataPtr->max_angle[0],geomDataPtr->max_angle[1]); - sensorDataPtr = boost::dynamic_pointer_cast(sensorBasePtr->CreateSensorData(OpenRAVE::SensorBase::ST_Laser)); - return true; } From 71516d109bc70ca34a59786a5ee4f0b9914f5ec1 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Wed, 6 Dec 2017 08:09:24 +0100 Subject: [PATCH 70/70] make devices more similar --- .../YarpOpenraveControlboard/DeviceDriverImpl.cpp | 8 +++----- .../YarpOpenraveControlboard/YarpOpenraveControlboard.hpp | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index 3c379c26..dda97f31 100644 --- a/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp @@ -9,7 +9,7 @@ namespace roboticslab // ------------------------------------------------------------------- -void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer) +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername) { OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv,viewername); BOOST_ASSERT(!!viewer); @@ -18,9 +18,7 @@ void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, 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); + viewer->main(true); } // ------------------- DeviceDriver Related ------------------------------------ @@ -50,7 +48,7 @@ bool YarpOpenraveControlboard::open(yarp::os::Searchable& config) { if ( config.check("view") ) { - boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin",1)); + boost::thread openraveViewerThread(boost::bind(SetViewer,penv,"qtcoin")); openraveThreads.add_thread(&openraveViewerThread); yarp::os::Time::delay(0.4); // wait for the viewer to init, in [s] } diff --git a/libraries/YarpPlugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp b/libraries/YarpPlugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp index b15cbcb2..8ecc8f6d 100644 --- a/libraries/YarpPlugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp +++ b/libraries/YarpPlugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp @@ -24,7 +24,7 @@ namespace roboticslab // Specific for --env parameter -void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername, int _viewer); +void SetViewer(OpenRAVE::EnvironmentBasePtr penv, const std::string& viewername); /** * @ingroup TeoYarp