Merge branch 'rdr-36' into develop

- asrob-uc3m/robotDevastation-robots#36

CMakeLists.txt

@@ -29,6 +29,7 @@ option(ENABLE_YarpOpenraveControlboard "Enable/disable option YarpOpenraveContro
 option(ENABLE_YarpOpenraveControlboardCollision "Enable/disable option YarpOpenraveControlboardCollision" TRUE)
 option(ENABLE_YarpOpenraveGrabber "Enable/disable option YarpOpenraveGrabber" TRUE)
 option(ENABLE_YarpOpenraveRGBDSensor  "Enable/disable option YarpOpenraveRGBDSensor" TRUE)
+option(ENABLE_YarpOpenraveRobotManager "Enable/disable option YarpOpenraveRobotManager" TRUE)
 
 ### options: cpp programs
 option(ENABLE_teoSim "Choose if you want to compile (deprecated) teoSim" TRUE)

libraries/YarpPlugins/CMakeLists.txt

@@ -25,6 +25,7 @@ include(YarpPlugin)
     add_subdirectory(YarpOpenraveControlboardCollision)
     add_subdirectory(YarpOpenraveGrabber)
     add_subdirectory(YarpOpenraveRGBDSensor)
+    add_subdirectory(YarpOpenraveRobotManager)
 #  yarp_end_plugin_library(yarpplugins)
 
 #ENDIF (ENABLE_yarpplugins)

libraries/YarpPlugins/YarpOpenraveBase/YarpOpenraveBase.cpp

@@ -75,6 +75,15 @@ bool YarpOpenraveBase::configureEnvironment(yarp::os::Searchable& config)
         CD_ERROR("Please use --env or --penv parameter. Bye!\n");
         return false;
     }
+
+    if( config.check("physics") )
+    {
+        std::string physics = config.find("physics").asString();
+        CD_DEBUG("Found --physics parameter: %s.\n", physics.c_str());
+        penv->SetPhysicsEngine(OpenRAVE::RaveCreatePhysicsEngine(penv,physics));
+        penv->GetPhysicsEngine()->SetGravity(OpenRAVE::Vector(0,0,-9.8));
+    }
+
     return true;
 }
 

libraries/YarpPlugins/YarpOpenraveRobotManager/CMakeLists.txt

@@ -0,0 +1,50 @@
+# Copyright: (C) 2013 Universidad Carlos III de Madrid
+# Author: Juan G. Victores
+
+yarp_prepare_plugin(YarpOpenraveRobotManager
+                    CATEGORY device
+                    TYPE roboticslab::YarpOpenraveRobotManager
+                    INCLUDE YarpOpenraveRobotManager.hpp
+                    WRAPPER RobotServer)
+
+IF (NOT SKIP_YarpOpenraveRobotManager)
+
+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} ${OPENRAVE_YARP_PLUGINS_INCLUDE_DIRS})
+if( Boost_INCLUDE_DIRS )
+  include_directories(${Boost_INCLUDE_DIRS})
+endif()
+
+LINK_DIRECTORIES(${OPENRAVE_YARP_PLUGINS_LINK_DIRS})
+
+YARP_ADD_PLUGIN(YarpOpenraveRobotManager YarpOpenraveRobotManager.hpp DeviceDriverImpl.cpp IRobotManagerImpl.cpp)
+add_dependencies(YarpOpenraveRobotManager COLOR_DEBUG YarpOpenraveBase)
+set_target_properties(YarpOpenraveRobotManager PROPERTIES COMPILE_FLAGS "${OpenRAVE_CXXFLAGS}")
+set_target_properties(YarpOpenraveRobotManager PROPERTIES LINK_FLAGS "${OpenRAVE_LINK_FLAGS}")
+TARGET_LINK_LIBRARIES(YarpOpenraveRobotManager ${OpenRAVE_LIBRARIES} ${OpenRAVE_CORE_LIBRARIES} ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${YARP_LIBRARIES} YarpOpenraveBase)
+
+# Exporting dependencies for ...Config.cmake quite manually for now...
+set(OPENRAVE_YARP_PLUGINS_INCLUDE_DIRS ${OPENRAVE_YARP_PLUGINS_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} CACHE INTERNAL "appended header dirs")
+#set(OPENRAVE_YARP_PLUGINS_LIBRARIES ${OPENRAVE_YARP_PLUGINS_LIBRARIES} YarpOpenraveRobotManager CACHE INTERNAL "appended libraries")
+
+  #install(TARGETS YarpOpenraveRobotManager DESTINATION lib)
+  yarp_install(TARGETS YarpOpenraveRobotManager
+             EXPORT YARP
+             COMPONENT runtime
+             LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+             ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR})
+
+  yarp_install(FILES YarpOpenraveRobotManager.ini
+              COMPONENT runtime
+              DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR})
+
+ENDIF (NOT SKIP_YarpOpenraveRobotManager)
+

libraries/YarpPlugins/YarpOpenraveRobotManager/DeviceDriverImpl.cpp

@@ -0,0 +1,54 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "YarpOpenraveRobotManager.hpp"
+
+namespace roboticslab
+{
+
+// ------------------- DeviceDriver Related ------------------------------------
+
+bool YarpOpenraveRobotManager::open(yarp::os::Searchable& config)
+{
+    CD_DEBUG("config: %s\n",config.toString().c_str());
+
+    if ( ! configureEnvironment(config) )
+        return false;
+
+    if ( ! configureOpenravePlugins(config) )
+        return false;
+
+    if ( ! configureRobot(config) )
+        return false;
+
+    //-- Create the controller, make sure to lock environment!
+    {
+        OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
+
+        pcontrol = OpenRAVE::RaveCreateController(penv,"idealvelocitycontroller");  // idealcontroller, odevelocity, idealvelocitycontroller
+
+        std::vector<int> dofindices( probot->GetDOF() );
+        for(int i = 0; i < probot->GetDOF(); ++i)
+        {
+            dofindices[i] = i;
+        }
+
+        probot->SetController(pcontrol,dofindices,0);
+    }
+
+    penv->StopSimulation();
+    penv->StartSimulation(0.01);
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::close()
+{
+    CD_INFO("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+}  // namespace roboticslab

libraries/YarpPlugins/YarpOpenraveRobotManager/IRobotManager.hpp

@@ -0,0 +1,55 @@
+// Authors: see AUTHORS.md at project root.
+// CopyPolicy: released under the terms of the LGPLv2.1, see LICENSE at project root.
+// URL: https://github.com/asrob-uc3m/robotDevastation
+
+#ifndef __RD_I_ROBOT_MANAGER_HPP__
+#define __RD_I_ROBOT_MANAGER_HPP__
+
+
+namespace rd{
+
+/**
+ * @ingroup rd_libraries
+ *
+ * \defgroup RobotLib
+ *
+  * @brief Base class for Robot Managers. Original copy of this file at https://github.com/asrob-uc3m/robotDevastation/blob/develop/src/libraries/RobotLib/IRobotManager.hpp
+ */
+class IRobotManager
+{
+    public:
+        //-- Robot movement related functions    
+        virtual bool moveForward(int velocity) = 0;
+        virtual bool moveBackwards(int velocity) = 0;
+        virtual bool turnLeft(int velocity) = 0;
+        virtual bool turnRight(int velocity) = 0;
+        virtual bool stopMovement() = 0;
+
+        //-- Robot camera related functions
+        virtual bool tiltUp(int velocity) = 0;
+        virtual bool tiltDown(int velocity) = 0;
+        virtual bool panLeft(int velocity) = 0;
+        virtual bool panRight(int velocity) = 0;
+        virtual bool stopCameraMovement() = 0;
+
+        //-- Robot connection related functions
+        /// @brief Connect to the remote robot
+        virtual bool connect() = 0;
+
+        /// @brief Disconnect from the remote robot
+        virtual bool disconnect() = 0;
+
+        /// @brief Test connection (not in used yet)
+        virtual bool test() = 0;
+
+        /// @brief Enable/disable sending commands through the manager
+        virtual void setEnabled(bool enabled) = 0;
+
+        //-- Other
+        virtual void onDestroy() = 0;
+        virtual ~IRobotManager() {}
+};
+
+}
+
+#endif //-- __RD_I_ROBOT_MANAGER_HPP__

libraries/YarpPlugins/YarpOpenraveRobotManager/IRobotManagerImpl.cpp

@@ -0,0 +1,148 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "YarpOpenraveRobotManager.hpp"
+
+namespace roboticslab
+{
+
+// ------------------- DeviceDriver Related ------------------------------------
+
+bool YarpOpenraveRobotManager::moveForward(int velocity)
+{
+    CD_DEBUG("\n");
+    std::vector<OpenRAVE::dReal> values(4, velocity);
+    pcontrol->SetDesired(values);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::moveBackwards(int velocity)
+{
+    CD_DEBUG("\n");
+    std::vector<OpenRAVE::dReal> values(4, -velocity);
+    pcontrol->SetDesired(values);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::turnLeft(int velocity)
+{
+    CD_DEBUG("\n");
+    std::vector<OpenRAVE::dReal> values(4);
+    values[0] = -velocity;
+    values[1] = velocity;
+    values[2] = -velocity;
+    values[3] = velocity;
+    pcontrol->SetDesired(values);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::turnRight(int velocity)
+{
+    CD_DEBUG("\n");
+    std::vector<OpenRAVE::dReal> values(4);
+    values[0] = velocity;
+    values[1] = -velocity;
+    values[2] = velocity;
+    values[3] = -velocity;
+    pcontrol->SetDesired(values);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::stopMovement()
+{
+    CD_DEBUG("\n");
+    std::vector<OpenRAVE::dReal> values(4, 0.0);
+    pcontrol->SetDesired(values);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::tiltUp(int velocity)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::tiltDown(int velocity)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::panLeft(int velocity)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::panRight(int velocity)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::stopCameraMovement()
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::connect()
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::disconnect()
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool YarpOpenraveRobotManager::test()
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+void YarpOpenraveRobotManager::setEnabled(bool enabled)
+{
+    CD_DEBUG("\n");
+    return;
+}
+
+// -----------------------------------------------------------------------------
+
+void YarpOpenraveRobotManager::onDestroy()
+{
+    CD_DEBUG("\n");
+    return;
+}
+
+// -----------------------------------------------------------------------------
+
+}  // namespace roboticslab