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