diff --git a/CMakeLists.txt b/CMakeLists.txt index f21a49f5..d8f78670 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,8 @@ 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) +option(ENABLE_YarpOpenraveRGBDSensor "Enable/disable option YarpOpenraveRGBDSensor" TRUE) ### options: cpp libraries (openrave plugins) option(ENABLE_OpenraveYarpControlboard "Enable/disable option OpenraveYarpControlboard" TRUE) @@ -25,6 +27,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/examples/openrave-YarpPluginLoader-controlboard.py b/examples/openrave-YarpPluginLoader-controlboard.py new file mode 100644 index 00000000..7a545710 --- /dev/null +++ b/examples/openrave-YarpPluginLoader-controlboard.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') + + OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') + print OpenraveYarpPluginLoader.SendCommand('open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --manipulatorIndex 0 --prefix /drl') + + while 1: + pass + +finally: + RaveDestroy() + diff --git a/examples/openrave-YarpPluginLoader-grabber.py b/examples/openrave-YarpPluginLoader-grabber.py new file mode 100644 index 00000000..47e1c839 --- /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 YarpOpenraveGrabber --robotIndex 0 --sensorIndex 1 --prefix /drl') + + while 1: + pass + +finally: + RaveDestroy() + diff --git a/examples/openrave-YarpPluginLoader-rgbdsensor.py b/examples/openrave-YarpPluginLoader-rgbdsensor.py new file mode 100644 index 00000000..8c3ada34 --- /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 YarpOpenraveRGBDSensor --robotIndex 0 --sensorIndex 0 --prefix /drl') + + while 1: + pass + +finally: + RaveDestroy() + 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..9175cb63 --- /dev/null +++ b/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp @@ -0,0 +1,207 @@ +/** + * 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 + +#include "ColorDebug.hpp" + +class OpenraveYarpPluginLoader : public OpenRAVE::ModuleBase +{ +public: + OpenraveYarpPluginLoader(OpenRAVE::EnvironmentBasePtr penv) : OpenRAVE::ModuleBase(penv) + { + __description = "OpenraveYarpPluginLoader plugin."; + RegisterCommand("open",boost::bind(&OpenraveYarpPluginLoader::Open, this,_1,_2),"opens port"); + } + + virtual ~OpenraveYarpPluginLoader() + { + //-- Note that we start on element 1, first elem was not via new!! + for(size_t i=1;iclose(); + delete yarpPlugin; + yarpPlugin = 0; + } + + 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(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"); + + //-- 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); + + while(sinput) + { + std::string str; + sinput >> str; + 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); + } + + //for(size_t i=0;i vectorOfRobotPtr; + GetEnv()->GetRobots(vectorOfRobotPtr); + + if(robotPtrIdx >= vectorOfRobotPtr.size()) + { + 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(); + + if( options.check("manipulatorIndex") ) + { + 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; + } + else if (manipulatorPtrIdx < 0) + { + CD_ERROR("manipulatorPtrIdx %d < 0, not loading yarpPlugin.\n",manipulatorPtrIdx); + return false; + } + + 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); + + if( ! yarpPlugin->isValid() ) + { + CD_ERROR("yarpPlugin not valid.\n"); + return false; + } + CD_SUCCESS("Valid yarpPlugin.\n"); + + return true; + } + +private: + std::vector argv; + + yarp::os::Network yarp; + yarp::dev::PolyDriver* yarpPlugin; +}; + +OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv) +{ + if( type == OpenRAVE::PT_Module && interfacename == "openraveyarppluginloader") + { + return OpenRAVE::InterfaceBasePtr(new OpenraveYarpPluginLoader(penv)); + } + return OpenRAVE::InterfaceBasePtr(); +} + +void GetPluginAttributesValidated(OpenRAVE::PLUGININFO& info) +{ + info.interfacenames[OpenRAVE::PT_Module].push_back("OpenraveYarpPluginLoader"); +} + +OPENRAVE_PLUGIN_API void DestroyPlugin() +{ + RAVELOG_INFO("destroying plugin\n"); +} diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt index a6cd606d..e57206d6 100644 --- a/libraries/YarpPlugins/CMakeLists.txt +++ b/libraries/YarpPlugins/CMakeLists.txt @@ -24,6 +24,8 @@ endif () add_subdirectory(YarpOpenraveControlboard) 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/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp index e3c41629..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 ------------------------------------ @@ -29,6 +27,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") ) ) @@ -48,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] } @@ -73,19 +73,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 +151,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(); 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 diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveGrabber/CMakeLists.txt new file mode 100644 index 00000000..0ca1d92c --- /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 grabber) + +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..809159ea --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/DeviceDriverImpl.cpp @@ -0,0 +1,219 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveGrabber.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 YarpOpenraveGrabber::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; + } + + sensorBasePtr = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + + 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 ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Camera) ) + { + CD_ERROR("Sensor %d does not support ST_Camera.\n", sensorIndex ); + } + + // Activate the sensor + sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); + + // Show the sensor image in a separate window + //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + + // 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; + + return true; +} + +// ----------------------------------------------------------------------------- + +bool YarpOpenraveGrabber::close() { + CD_INFO("\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..d9bd9946 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/IFrameGrabberImageImpl.cpp @@ -0,0 +1,51 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveGrabber.hpp" + +namespace roboticslab +{ + +// ------------------ IFrameGrabberImage Related ---------------------------------------- + +bool YarpOpenraveGrabber::getImage(yarp::sig::ImageOf& image) +{ + sensorBasePtr->GetSensorData(sensorDataPtr); + + //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 = 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; + } + } + + return true; +} + +// ---------------------------------------------------------------------------- + +int YarpOpenraveGrabber::height() const +{ + CD_DEBUG("%d\n", _height); + return _height; +} + +// ---------------------------------------------------------------------------- + +int YarpOpenraveGrabber::width() const +{ + CD_DEBUG("%d\n", _width); + return _width; +} + +// ---------------------------------------------------------------------------- + +} diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/README.md b/libraries/YarpPlugins/YarpOpenraveGrabber/README.md new file mode 100644 index 00000000..f9f78ff4 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/README.md @@ -0,0 +1,3 @@ +# YarpOpenraveGrabber + +Example invocation: `yarpdev --device YarpOpenraveGrabber --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/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.hpp new file mode 100644 index 00000000..573ca350 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.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 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 IFrameGrabberImage, etc. + * interface class member functions. + */ +class YarpOpenraveGrabber : public yarp::dev::DeviceDriver, public yarp::dev::IFrameGrabberImage { +public: + + // Set the Thread Rate in the class constructor + YarpOpenraveGrabber() {} + + // ------- 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 sensorBasePtr; + boost::shared_ptr sensorDataPtr; + + // Specific for --env parameter with --view + boost::thread_group openraveThreads; +}; + +} // namespace roboticslab + +#endif // __YARP_OPENRAVE_GRABBER_HPP__ diff --git a/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini b/libraries/YarpPlugins/YarpOpenraveGrabber/YarpOpenraveGrabber.ini new file mode 100644 index 00000000..f8cd0352 --- /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 grabber diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/CMakeLists.txt new file mode 100644 index 00000000..7ed5f4d9 --- /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 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}") +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..a9e5d7f8 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/DeviceDriverImpl.cpp @@ -0,0 +1,219 @@ +// -*- 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; + } + + sensorBasePtr = vectorOfSensorPtr.at(sensorIndex)->GetSensor(); + + 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 ( ! sensorBasePtr->Supports(OpenRAVE::SensorBase::ST_Laser) ) + { + CD_ERROR("Sensor %d does not support ST_Laser.\n", sensorIndex ); + } + + // Activate the sensor + sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_PowerOn); + + // Show the sensor image in a separate window // Ok for Laser??? + //sensorBasePtr->Configure(OpenRAVE::SensorBase::CC_RenderDataOn); + + // 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]); + + return true; +} + +// ----------------------------------------------------------------------------- + +bool YarpOpenraveRGBDSensor::close() { + CD_INFO("\n"); + return true; +} + +// ----------------------------------------------------------------------------- + +} // namespace roboticslab + diff --git a/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp new file mode 100644 index 00000000..46d1d221 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp @@ -0,0 +1,35 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "YarpOpenraveRGBDSensor.hpp" + +namespace roboticslab +{ + +// ------------------ IRGBDSensor Related ---------------------------------------- + +bool YarpOpenraveRGBDSensor::getDepthImage(yarp::sig::ImageOf &depthImage, yarp::os::Stamp *timeStamp) +{ + /*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/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..98966fa9 --- /dev/null +++ b/libraries/YarpPlugins/YarpOpenraveRGBDSensor/YarpOpenraveRGBDSensor.hpp @@ -0,0 +1,190 @@ +// -*- 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::IRGBDSensor { +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(); + + // ------- 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 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 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 getRgbImage(yarp::sig::FlexImage &rgbImage, yarp::os::Stamp *timeStamp = NULL) {} + + /** + * 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 bool getDepthImage(yarp::sig::ImageOf &depthImage, yarp::os::Stamp *timeStamp = NULL); + + /** + * 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 RGBDSensor_status getSensorStatus() {} + +private: + + // General Grabber parameters // + int _height, _width; + + //OpenRAVE// + OpenRAVE::EnvironmentBasePtr penv; + OpenRAVE::RobotBasePtr probot; + std::string robotName; + + OpenRAVE::SensorBasePtr sensorBasePtr; + boost::shared_ptr sensorDataPtr; + + // 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