diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bfd2b8d4..64e95a276 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,6 +108,30 @@ macro(list_subdirectories2 retval curdir return_relative) set(${retval} ${list_of_dirs}) endmacro() +# MACRO to add the list of subdirectories to another list +macro(list_subdirectories3 retval curdir return_relative) + file(GLOB sub-dir RELATIVE ${curdir} *) + set(list_of_dirs ${${retval}}) + foreach(dir ${sub-dir}) + if(IS_DIRECTORY ${curdir}/${dir}) + if (${return_relative}) + list (FIND list_of_dirs "${dir}" _index) + if (NOT ${_index} GREATER -1) + if (NOT ${dir} MATCHES "CMakeFiles" AND NOT ${dir} MATCHES ".svn") + set(list_of_dirs ${list_of_dirs} ${dir}) + endif() + endif() + else() + list (FIND list_of_dirs "${curdir}/${dir}" _index) + if (NOT ${_index} GREATER -1) + set(list_of_dirs ${list_of_dirs} ${curdir}/${dir}) + endif() + endif() + endif() + endforeach() + set(${retval} ${list_of_dirs}) +endmacro() + ################### diff --git a/Deps/python/Modules/yaml/CMakeLists.txt b/Deps/python/Modules/yaml/CMakeLists.txt new file mode 100644 index 000000000..5db4c3a01 --- /dev/null +++ b/Deps/python/Modules/yaml/CMakeLists.txt @@ -0,0 +1,12 @@ +set(module "yaml") + +usePython(2) +find_python_module(${module} REQUIRED) + +string(TOUPPER ${module} module_upper) + +if(PY_${module_upper}) + SET (yaml_PY_MODULES PY_${module_upper}) + list(APPEND DEPS python-yaml) + list(APPEND DEPS_DEV python-yaml) +endif(PY_${module_upper}) \ No newline at end of file diff --git a/Deps/yaml-cpp/CMakeLists.txt b/Deps/yaml-cpp/CMakeLists.txt new file mode 100644 index 000000000..00fa78b5b --- /dev/null +++ b/Deps/yaml-cpp/CMakeLists.txt @@ -0,0 +1,9 @@ +find_package(yaml-cpp) + +if (YAML_CPP_INCLUDE_DIR) + message("***YAML-CPP FOUND: ${YAML_CPP_INCLUDE_DIR}") + list(APPEND DEPS libyaml-cpp0.5v5) + list(APPEND DEPS_DEV libyaml-cpp-dev) +else() + message ("*** YAML-CPP NOT FOUND") +endIF() diff --git a/cmake/cpack_metainfo/libs.cmake b/cmake/cpack_metainfo/libs.cmake index d5c565a24..a669e718d 100644 --- a/cmake/cpack_metainfo/libs.cmake +++ b/cmake/cpack_metainfo/libs.cmake @@ -8,14 +8,24 @@ SET(CPACK_COMPONENT_COLORSPACES_DESCRIPTION "Library for Home page https://jderobot.org") -SET(CPACK_DEBIAN_COMM_PACKAGE_DEPENDS "jderobot-types, jderobot-logger, jderobot-interfaces, jderobot-colorspaces, jderobot-setup") +SET(CPACK_DEBIAN_CONFIG_PACKAGE_DEPENDS "jderobot-setup") +SET(CPACK_COMPONENT_CONFIG_DESCRIPTION +"Library for read Yaml config files + Home page https://jderobot.org") + +SET(CPACK_DEBIAN_CONFIG-PYTHON_PACKAGE_DEPENDS "jderobot-setup") +SET(CPACK_COMPONENT_CONFIG-PYTHON_DESCRIPTION +"Library for read Yaml config files + Home page https://jderobot.org") + +SET(CPACK_DEBIAN_COMM_PACKAGE_DEPENDS "jderobot-types, jderobot-config, jderobot-logger, jderobot-interfaces, jderobot-colorspaces, jderobot-setup") SET(CPACK_COMPONENT_COMM_DESCRIPTION -"Library for +"Library for communications. You can use ROS or ICE Home page https://jderobot.org") -SET(CPACK_DEBIAN_COMM-PYTHON_PACKAGE_DEPENDS "jderobot-types-python, jderobot-setup") +SET(CPACK_DEBIAN_COMM-PYTHON_PACKAGE_DEPENDS "jderobot-types-python,jderobot-config-python, jderobot-setup") SET(CPACK_COMPONENT_COMM-PYTHON_DESCRIPTION -"Library for +"Library for communications. You can use ROS or ICE Home page https://jderobot.org") SET(CPACK_DEBIAN_COMM-PYTHON_PACKAGE_CONTROL_EXTRA "${CMAKE_CURRENT_SOURCE_DIR}/scripts/cmake/ice/postinst" diff --git a/cmake/cpack_metainfo/tools.cmake b/cmake/cpack_metainfo/tools.cmake index 7b437e3bb..5242f012d 100644 --- a/cmake/cpack_metainfo/tools.cmake +++ b/cmake/cpack_metainfo/tools.cmake @@ -16,7 +16,7 @@ SET(CPACK_COMPONENT_COLORTUNER-PYTHON_DESCRIPTION Manual page http://jderobot.org/index.php/Tools#ColorTuner_.28Python.29 Home page https://jderobot.org") -SET(CPACK_DEBIAN_CAMERAVIEW_PACKAGE_DEPENDS "jderobot-easyice, jderobot-comm, jderobot-util, jderobot-interfaces, jderobot-resourcelocator, jderobot-colorspaces") +SET(CPACK_DEBIAN_CAMERAVIEW_PACKAGE_DEPENDS "jderobot-config, jderobot-comm, jderobot-util, jderobot-interfaces, jderobot-resourcelocator, jderobot-colorspaces") SET(CPACK_COMPONENT_CAMERAVIEW_DESCRIPTION "Generic viewer for cameras Manual Page http://jderobot.org/index.php/Tools#CameraView @@ -34,7 +34,7 @@ SET(CPACK_COMPONENT_GIRAFFECLIENT_DESCRIPTION Manual Page Home page https://jderobot.org") -SET(CPACK_DEBIAN_KOBUKIVIEWER_PACKAGE_DEPENDS "jderobot-easyice, jderobot-comm") +SET(CPACK_DEBIAN_KOBUKIVIEWER_PACKAGE_DEPENDS "jderobot-config, jderobot-comm") SET(CPACK_COMPONENT_KOBUKIVIEWER_DESCRIPTION "Teleoperator for vehicle-type robots, such as kobuki, pioneer, cars, etc. Manual page http://jderobot.org/index.php/Tools#KobukiViewer @@ -46,7 +46,7 @@ SET(CPACK_COMPONENT_NAMINGSERVICE_DESCRIPTION Manual Page Home page https://jderobot.org") -SET(CPACK_DEBIAN_PANTILTTELEOP-PYTHON_PACKAGE_DEPENDS "python-matplotlib, python-pyqt5, python-pip, python-numpy, python-pyqt5.qtsvg, jderobot-parallelice-python, jderobot-easyice-python") +SET(CPACK_DEBIAN_PANTILTTELEOP-PYTHON_PACKAGE_DEPENDS "python-matplotlib, python-pyqt5, python-pip, python-numpy, python-pyqt5.qtsvg, jderobot-comm-python, jderobot-config-python") SET(CPACK_COMPONENT_PANTILTTELEOP-PYTHON_DESCRIPTION "Teleoperator for IP cameras that allow movement (i.e. Sony EVI camera) Manual Page http://jderobot.org/index.php/Tools#NavigatorCamera diff --git a/src/libs/CMakeLists.txt b/src/libs/CMakeLists.txt index 5eb780b23..a46ff2d73 100644 --- a/src/libs/CMakeLists.txt +++ b/src/libs/CMakeLists.txt @@ -1,4 +1,5 @@ -list_subdirectories( LIST_LIBS ${CMAKE_CURRENT_SOURCE_DIR} 1) +set (LIST_LIBS "config_cpp") #Adding library to be processed first +list_subdirectories3( LIST_LIBS ${CMAKE_CURRENT_SOURCE_DIR} 1) FOREACH (lib ${LIST_LIBS}) if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${lib}/CMakeLists.txt) diff --git a/src/libs/jderobotcomm_cpp/CMakeLists.txt b/src/libs/comm_cpp/CMakeLists.txt similarity index 92% rename from src/libs/jderobotcomm_cpp/CMakeLists.txt rename to src/libs/comm_cpp/CMakeLists.txt index 476c66e27..bcd98d873 100644 --- a/src/libs/jderobotcomm_cpp/CMakeLists.txt +++ b/src/libs/comm_cpp/CMakeLists.txt @@ -6,7 +6,7 @@ ELSE() SET(JDEROBOTCOM_ROS OFF) endif() -project(jderobotcomm) +project(comm) usePython(2) @@ -28,10 +28,12 @@ include_directories( ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ${INTERFACES_CPP_DIR} + ${config_INCLUDE_DIRS} ) set(HEADERS + include/jderobot/comm/communicator.hpp include/jderobot/comm/laserClient.hpp include/jderobot/comm/interfaces/laserClient.hpp include/jderobot/comm/ice/laserIceClient.hpp @@ -48,6 +50,7 @@ set(HEADERS ) set(SOURCES + src/communicator.cpp src/laserClient.cpp src/ice/laserIceClient.cpp src/cameraClient.cpp @@ -83,12 +86,15 @@ ENDIF() ## Adding shared library for common usage add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +add_dependencies(${PROJECT_NAME} ${config_LIBRARIES}) + target_link_libraries(${PROJECT_NAME} ${ZeroCIce_LIBRARIES} logger ${Boost_LIBRARIES} colorspacesmm - ${catkin_LIBRARIES}) + ${catkin_LIBRARIES} + ${config_LIBRARIES}) ## Adding static library for single .so configurations # since target is a shared library, -fPIC must be provided @@ -100,7 +106,8 @@ target_link_libraries(${PROJECT_NAME}-embedded ${Boost_LIBRARIES} colorspacesmm JderobotInterfaces - ${catkin_LIBRARIES}) + ${catkin_LIBRARIES} + ${config_LIBRARIES}) set_property(TARGET ${PROJECT_NAME}-embedded PROPERTY POSITION_INDEPENDENT_CODE 1) diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/cameraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp similarity index 90% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/cameraClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp index 63608c81b..e87eea284 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/cameraClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp @@ -22,14 +22,14 @@ #include #include -#include +#include #include -namespace JdeRobotComm { +namespace Comm { /** * @brief make a CameraClient using propierties @@ -41,7 +41,7 @@ namespace JdeRobotComm { * * @return null if propierties are wrong */ - CameraClient* getCameraClient(Ice::CommunicatorPtr ic, std::string prefix); + CameraClient* getCameraClient(Comm::Communicator* jdrc, std::string prefix); } //NS diff --git a/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp b/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp new file mode 100644 index 000000000..cd1b1a696 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_COMMUNICATOR_H +#define JDEROBOTCOMM_COMMUNICATOR_H + +#include +#include +#include + + +namespace Comm { + + class Communicator { + public: + Communicator(Config::Properties config); + ~Communicator(); + + Config::Properties getConfig(); + Ice::CommunicatorPtr getIceComm(); + + + private: + Config::Properties config; + Ice::CommunicatorPtr ic; + }; + + +} //NS + +#endif // JDEROBOTCOMM_COMMUNICATOR_H \ No newline at end of file diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp similarity index 87% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp index 355a353aa..fcca0d2ce 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp @@ -28,15 +28,17 @@ #include #include #include +#include #include +#include #include #include -namespace JdeRobotComm { +namespace Comm { -class CameraIceClient: public IceUtil::Thread, public JdeRobotComm::CameraClient { +class CameraIceClient: public IceUtil::Thread, public Comm::CameraClient { public: - CameraIceClient(Ice::CommunicatorPtr ic, std::string prefix); + CameraIceClient(Comm::Communicator* jdrc, std::string prefix); virtual ~CameraIceClient(); virtual void run(); @@ -69,5 +71,5 @@ class CameraIceClient: public IceUtil::Thread, public JdeRobotComm::CameraClient }; -} /* namespace JdeRobotComm */ +} /* namespace Comm */ #endif /* JDEROBOTCOMM_CAMERAICECLIENT_H_ */ \ No newline at end of file diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/laserIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/laserIceClient.hpp similarity index 89% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/laserIceClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ice/laserIceClient.hpp index eaf45ff91..0198f5e73 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/laserIceClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/laserIceClient.hpp @@ -31,14 +31,15 @@ #include #include #include +#include #include -namespace JdeRobotComm { +namespace Comm { -class LaserIceClient: public IceUtil::Thread, public JdeRobotComm::LaserClient { +class LaserIceClient: public IceUtil::Thread, public Comm::LaserClient { public: - LaserIceClient(Ice::CommunicatorPtr ic, std::string prefix); + LaserIceClient(Comm::Communicator* jdrc, std::string prefix); virtual ~LaserIceClient(); virtual void run(); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp similarity index 89% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp index 6c1e4375d..c8bbc96e7 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/motorsIceClient.hpp @@ -25,14 +25,15 @@ #include #include #include +#include #include -namespace JdeRobotComm { +namespace Comm { -class MotorsIceClient: public JdeRobotComm::MotorsClient { +class MotorsIceClient: public Comm::MotorsClient { public: - MotorsIceClient(Ice::CommunicatorPtr ic, std::string prefix); + MotorsIceClient(Comm::Communicator* jdrc, std::string prefix); virtual ~MotorsIceClient(); virtual void sendVelocities(JdeRobotTypes::CMDVel vel); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp similarity index 89% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp index 1e98c912c..e3ac48073 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/pose3dIceClient.hpp @@ -28,14 +28,15 @@ #include #include #include +#include #include -namespace JdeRobotComm { +namespace Comm { -class Pose3dIceClient: public IceUtil::Thread, public JdeRobotComm::Pose3dClient { +class Pose3dIceClient: public IceUtil::Thread, public Comm::Pose3dClient { public: - Pose3dIceClient(Ice::CommunicatorPtr ic, std::string prefix); + Pose3dIceClient(Comm::Communicator* jdrc, std::string prefix); virtual ~Pose3dIceClient(); virtual void run(); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp similarity index 98% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp index 3f952ad1e..b4d8f3277 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/cameraClient.hpp @@ -23,7 +23,7 @@ #include -namespace JdeRobotComm { +namespace Comm { /** * @brief LaserClient class. diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/laserClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/laserClient.hpp similarity index 98% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/laserClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/interfaces/laserClient.hpp index 49b95ef58..117abf626 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/laserClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/laserClient.hpp @@ -23,7 +23,7 @@ #include -namespace JdeRobotComm { +namespace Comm { /** * @brief LaserClient class. diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp similarity index 98% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp index d65b32d6c..f6b79f6ac 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/motorsClient.hpp @@ -23,7 +23,7 @@ #include -namespace JdeRobotComm { +namespace Comm { /** * @brief MotorsClient class. diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp similarity index 98% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp index de85cbe66..bd71c46a9 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/pose3dClient.hpp @@ -23,7 +23,7 @@ #include -namespace JdeRobotComm { +namespace Comm { /** * @brief Pose3dClient class. diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/laserClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp similarity index 90% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/laserClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp index 7ccc72b72..c1d8c435d 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/laserClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp @@ -22,13 +22,13 @@ #include #include -#include +#include #include -namespace JdeRobotComm { +namespace Comm { /** * @brief make a LaserClient using propierties @@ -40,7 +40,7 @@ namespace JdeRobotComm { * * @return null if propierties are wrong */ - LaserClient* getLaserClient(Ice::CommunicatorPtr ic, std::string prefix); + LaserClient* getLaserClient(Comm::Communicator* jdrc, std::string prefix); } //NS diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/motorsClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp similarity index 90% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/motorsClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp index ae544b440..40ccb5f0c 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/motorsClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp @@ -22,14 +22,14 @@ #include #include -#include +#include #include -namespace JdeRobotComm { +namespace Comm { /** * @brief make a MotorsClient using propierties @@ -41,7 +41,7 @@ namespace JdeRobotComm { * * @return null if propierties are wrong */ - MotorsClient* getMotorsClient(Ice::CommunicatorPtr ic, std::string prefix); + MotorsClient* getMotorsClient(Comm::Communicator* jdrc, std::string prefix); } //NS diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/pose3dClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp similarity index 90% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/pose3dClient.hpp rename to src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp index 99780ee12..2569d4cbb 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/pose3dClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp @@ -22,14 +22,14 @@ #include #include -#include +#include #include -namespace JdeRobotComm { +namespace Comm { /** * @brief make a Pose3dClient using propierties @@ -41,7 +41,7 @@ namespace JdeRobotComm { * * @return null if propierties are wrong */ - Pose3dClient* getPose3dClient(Ice::CommunicatorPtr ic, std::string prefix); + Pose3dClient* getPose3dClient(Comm::Communicator* jdrc, std::string prefix); } //NS diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerCamera.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerCamera.hpp similarity index 95% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerCamera.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ros/listenerCamera.hpp index 6e7169eb0..2134eb86c 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerCamera.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerCamera.hpp @@ -30,8 +30,8 @@ #include #include -namespace JdeRobotComm { - class ListenerCamera: public JdeRobotComm::CameraClient { +namespace Comm { + class ListenerCamera: public Comm::CameraClient { public: ListenerCamera(int argc, char** argv, std::string nodeName, std::string topic); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerLaser.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerLaser.hpp similarity index 95% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerLaser.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ros/listenerLaser.hpp index 16b95f862..336b753ad 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerLaser.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerLaser.hpp @@ -28,8 +28,8 @@ #include #include -namespace JdeRobotComm { - class ListenerLaser: public JdeRobotComm::LaserClient { +namespace Comm { + class ListenerLaser: public Comm::LaserClient { public: ListenerLaser(int argc, char** argv, std::string nodeName, std::string topic); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerPose.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerPose.hpp similarity index 95% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerPose.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ros/listenerPose.hpp index b6550d65a..5f5b5eb60 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/listenerPose.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerPose.hpp @@ -28,8 +28,8 @@ #include #include -namespace JdeRobotComm { - class ListenerPose: public JdeRobotComm::Pose3dClient { +namespace Comm { + class ListenerPose: public Comm::Pose3dClient { public: ListenerPose(int argc, char** argv, std::string nodeName, std::string topic); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/publisherMotors.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/publisherMotors.hpp similarity index 95% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/publisherMotors.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ros/publisherMotors.hpp index 5a96655b5..674216e3d 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/publisherMotors.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/publisherMotors.hpp @@ -28,8 +28,8 @@ #include #include -namespace JdeRobotComm { - class PublisherMotors: public JdeRobotComm::MotorsClient { +namespace Comm { + class PublisherMotors: public Comm::MotorsClient { public: PublisherMotors(int argc, char** argv, std::string nodeName, std::string topic); diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/translators.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp similarity index 99% rename from src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/translators.hpp rename to src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp index 9241916a0..d8ee4e21a 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ros/translators.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp @@ -42,7 +42,7 @@ #include -namespace JdeRobotComm { +namespace Comm { /** * @brief translate ROS LaserScan messages to JdeRobot LaserData diff --git a/src/libs/jderobotcomm_cpp/package.xml b/src/libs/comm_cpp/package.xml similarity index 98% rename from src/libs/jderobotcomm_cpp/package.xml rename to src/libs/comm_cpp/package.xml index c2daaf6fd..0bdcef583 100644 --- a/src/libs/jderobotcomm_cpp/package.xml +++ b/src/libs/comm_cpp/package.xml @@ -1,6 +1,6 @@ - jderobotcomm + comm 0.0.1 The jderobotcomm library diff --git a/src/libs/jderobotcomm_cpp/src/cameraClient.cpp b/src/libs/comm_cpp/src/cameraClient.cpp similarity index 78% rename from src/libs/jderobotcomm_cpp/src/cameraClient.cpp rename to src/libs/comm_cpp/src/cameraClient.cpp index 6a6af6cdb..f52c57de8 100644 --- a/src/libs/jderobotcomm_cpp/src/cameraClient.cpp +++ b/src/libs/comm_cpp/src/cameraClient.cpp @@ -22,15 +22,14 @@ #include #endif -namespace JdeRobotComm { +namespace Comm { CameraClient* -getCameraClient(Ice::CommunicatorPtr ic, std::string prefix){ +getCameraClient(Comm::Communicator* jdrc, std::string prefix){ CameraClient* client = 0; - Ice::PropertiesPtr prop = ic->getProperties(); - int server = prop->getPropertyAsIntWithDefault(prefix+".Server",0); + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -41,9 +40,9 @@ getCameraClient(Ice::CommunicatorPtr ic, std::string prefix){ { std::cout << "Receiving Image from ICE interfaces" << std::endl; CameraIceClient* cl; - cl = new CameraIceClient(ic, prefix); + cl = new CameraIceClient(jdrc, prefix); cl->start(); - client = (JdeRobotComm::CameraClient*) cl; + client = (Comm::CameraClient*) cl; break; } case 2: @@ -51,13 +50,13 @@ getCameraClient(Ice::CommunicatorPtr ic, std::string prefix){ #ifdef JDERROS std::cout << "Receiving Image from ROS messages" << std::endl; std::string nodeName; - nodeName = prop->getPropertyWithDefault(prefix+".Name","CameraNode"); + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "LaserNode"); std::string topic; - topic = prop->getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); ListenerCamera* lc; lc = new ListenerCamera(0, nullptr, nodeName, topic); lc->start(); - client = (JdeRobotComm::CameraClient*) lc; + client = (Comm::CameraClient*) lc; #else throw "ERROR: ROS is not available"; #endif diff --git a/src/libs/comm_cpp/src/communicator.cpp b/src/libs/comm_cpp/src/communicator.cpp new file mode 100644 index 000000000..6e0a28898 --- /dev/null +++ b/src/libs/comm_cpp/src/communicator.cpp @@ -0,0 +1,46 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ +#include + +namespace Comm { + +Communicator::Communicator(Config::Properties config){ + this->config = config; + this->ic = Ice::initialize(); +} + + +Communicator::~Communicator(){ + this->ic->destroy(); +} + + + +Config::Properties +Communicator::getConfig(){ + return this->config; +} + +Ice::CommunicatorPtr +Communicator::getIceComm(){ + return this->ic; +} + + +}//NS \ No newline at end of file diff --git a/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp b/src/libs/comm_cpp/src/ice/cameraIceClient.cpp similarity index 89% rename from src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp rename to src/libs/comm_cpp/src/ice/cameraIceClient.cpp index 58fe06c97..452aa58b7 100644 --- a/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp +++ b/src/libs/comm_cpp/src/ice/cameraIceClient.cpp @@ -19,28 +19,28 @@ */ -#include #include "jderobot/comm/ice/cameraIceClient.hpp" +#include -namespace JdeRobotComm { +namespace Comm { -CameraIceClient::CameraIceClient(Ice::CommunicatorPtr ic, std::string prefix) { + +CameraIceClient::CameraIceClient(Comm::Communicator* jdrc, std::string prefix) { this->prefix=prefix; - Ice::PropertiesPtr prop; - prop = ic->getProperties(); Ice::ObjectPrx baseCamera; this->refreshRate=0; this->mImageFormat.empty(); - int fps=prop->getPropertyAsIntWithDefault(prefix+".Fps",30); - this->cycle=(float)(1/(float)fps)*1000000; + float fps=jdrc->getConfig().asFloatWithDefault(prefix+".Fps", 30); + this->cycle=(1/fps)*1000000; try{ - baseCamera = ic->propertyToProxy(prefix+".Proxy"); + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + baseCamera = jdrc->getIceComm()->stringToProxy(proxy); if (0==baseCamera){ this->on = false; throw prefix + "Could not create proxy with Camera"; @@ -62,7 +62,7 @@ CameraIceClient::CameraIceClient(Ice::CommunicatorPtr ic, std::string prefix) { } //check if default format is defined - std::string definedFormat=prop->getPropertyWithDefault(prefix+".Format", "RGB8"); + std::string definedFormat=jdrc->getConfig().asStringWithDefault(prefix+".Format", "RGB8"); this->mImageFormat = CameraUtils::negotiateDefaultFormat(this->prx,definedFormat); @@ -129,12 +129,16 @@ CameraIceClient::run(){ try{ + dataPtr = this->prx->getImageData(this->mImageFormat); + + // Putting image data img.data = CameraUtils::getImageFromCameraProxy(dataPtr); + img.format = dataPtr->description->format; img.width = dataPtr->description->width; img.height = dataPtr->description->height; @@ -142,9 +146,10 @@ CameraIceClient::run(){ + } - catch(...){ - LOG(WARNING) << prefix +"error during request (connection error)"; + catch(std::exception& e){ + LOG(WARNING) << prefix +"error during request (connection error): " << e.what() << std::endl; usleep(50000); } diff --git a/src/libs/jderobotcomm_cpp/src/ice/laserIceClient.cpp b/src/libs/comm_cpp/src/ice/laserIceClient.cpp similarity index 91% rename from src/libs/jderobotcomm_cpp/src/ice/laserIceClient.cpp rename to src/libs/comm_cpp/src/ice/laserIceClient.cpp index 61ee6656c..918e8b694 100644 --- a/src/libs/jderobotcomm_cpp/src/ice/laserIceClient.cpp +++ b/src/libs/comm_cpp/src/ice/laserIceClient.cpp @@ -20,19 +20,19 @@ */ #include "jderobot/comm/ice/laserIceClient.hpp" -namespace JdeRobotComm { +namespace Comm { -LaserIceClient::LaserIceClient(Ice::CommunicatorPtr ic, std::string prefix) { +LaserIceClient::LaserIceClient(Comm::Communicator* jdrc, std::string prefix) { this->prefix=prefix; - Ice::PropertiesPtr prop; - prop = ic->getProperties(); + this->refreshRate=0; - int fps=prop->getPropertyAsIntWithDefault(prefix+".Fps",10); - this->cycle=(float)(1/(float)fps)*1000000; + float fps=jdrc->getConfig().asFloatWithDefault(prefix+".Fps",10); + this->cycle=(1/fps)*1000000; - Ice::ObjectPrx baseLaser = ic->propertyToProxy(prefix+".Proxy"); + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx baseLaser = jdrc->getIceComm()->stringToProxy(proxy); if (0==baseLaser){ this->on = false; diff --git a/src/libs/jderobotcomm_cpp/src/ice/motorsIceClient.cpp b/src/libs/comm_cpp/src/ice/motorsIceClient.cpp similarity index 90% rename from src/libs/jderobotcomm_cpp/src/ice/motorsIceClient.cpp rename to src/libs/comm_cpp/src/ice/motorsIceClient.cpp index a2666afd4..bad3806d3 100644 --- a/src/libs/jderobotcomm_cpp/src/ice/motorsIceClient.cpp +++ b/src/libs/comm_cpp/src/ice/motorsIceClient.cpp @@ -19,16 +19,14 @@ #include "jderobot/comm/ice/motorsIceClient.hpp" -namespace JdeRobotComm { +namespace Comm { -MotorsIceClient::MotorsIceClient(Ice::CommunicatorPtr ic, std::string prefix) { +MotorsIceClient::MotorsIceClient(Comm::Communicator* jdrc, std::string prefix) { this->prefix=prefix; - Ice::PropertiesPtr prop; - prop = ic->getProperties(); - - Ice::ObjectPrx baseMotors = ic->propertyToProxy(prefix+".Proxy"); + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx baseMotors = jdrc->getIceComm()->stringToProxy(proxy); if (0==baseMotors){ this->on = false; diff --git a/src/libs/jderobotcomm_cpp/src/ice/pose3dIceClient.cpp b/src/libs/comm_cpp/src/ice/pose3dIceClient.cpp similarity index 92% rename from src/libs/jderobotcomm_cpp/src/ice/pose3dIceClient.cpp rename to src/libs/comm_cpp/src/ice/pose3dIceClient.cpp index 574725b62..6e7e1ce4c 100644 --- a/src/libs/jderobotcomm_cpp/src/ice/pose3dIceClient.cpp +++ b/src/libs/comm_cpp/src/ice/pose3dIceClient.cpp @@ -19,19 +19,18 @@ #include "jderobot/comm/ice/pose3dIceClient.hpp" -namespace JdeRobotComm { +namespace Comm { -Pose3dIceClient::Pose3dIceClient(Ice::CommunicatorPtr ic, std::string prefix) { +Pose3dIceClient::Pose3dIceClient(Comm::Communicator* jdrc, std::string prefix) { this->prefix=prefix; - Ice::PropertiesPtr prop; - prop = ic->getProperties(); + - int fps=prop->getPropertyAsIntWithDefault(prefix+".Fps",30); - this->cycle=(float)(1/(float)fps)*1000000; + float fps=jdrc->getConfig().asFloatWithDefault(prefix+".Fps",30); + this->cycle=(1/fps)*1000000; - - Ice::ObjectPrx basePose = ic->propertyToProxy(prefix+".Proxy"); + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx basePose = jdrc->getIceComm()->stringToProxy(proxy); if (0==basePose){ this->on = false; diff --git a/src/libs/jderobotcomm_cpp/src/laserClient.cpp b/src/libs/comm_cpp/src/laserClient.cpp similarity index 64% rename from src/libs/jderobotcomm_cpp/src/laserClient.cpp rename to src/libs/comm_cpp/src/laserClient.cpp index ad1898664..71de12e20 100644 --- a/src/libs/jderobotcomm_cpp/src/laserClient.cpp +++ b/src/libs/comm_cpp/src/laserClient.cpp @@ -4,15 +4,13 @@ #include #endif -namespace JdeRobotComm { +namespace Comm { LaserClient* -getLaserClient(Ice::CommunicatorPtr ic, std::string prefix){ +getLaserClient(Comm::Communicator* jdrc, std::string prefix){ LaserClient* client = 0; - Ice::PropertiesPtr prop = ic->getProperties(); - - int server = prop->getPropertyAsIntWithDefault(prefix+".Server",0); + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -23,9 +21,9 @@ getLaserClient(Ice::CommunicatorPtr ic, std::string prefix){ { std::cout << "Receiving LaserData from ICE interfaces" << std::endl; LaserIceClient* cl; - cl = new LaserIceClient(ic, prefix); + cl = new LaserIceClient(jdrc, prefix); cl->start(); - client = (JdeRobotComm::LaserClient*) cl; + client = (Comm::LaserClient*) cl; break; } case 2: @@ -33,13 +31,13 @@ getLaserClient(Ice::CommunicatorPtr ic, std::string prefix){ #ifdef JDERROS std::cout << "Receiving LaserData from ROS messages" << std::endl; std::string nodeName; - nodeName = prop->getPropertyWithDefault(prefix+".Name","LaserNode"); - std::string topic; - topic = prop->getPropertyWithDefault(prefix+".Topic",""); + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "LaserNode"); + std::string topic; + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); ListenerLaser* lc; lc = new ListenerLaser(0, nullptr, nodeName, topic); lc->start(); - client = (JdeRobotComm::LaserClient*) lc; + client = (Comm::LaserClient*) lc; #else throw "ERROR: ROS is not available"; #endif diff --git a/src/libs/jderobotcomm_cpp/src/motorsClient.cpp b/src/libs/comm_cpp/src/motorsClient.cpp similarity index 65% rename from src/libs/jderobotcomm_cpp/src/motorsClient.cpp rename to src/libs/comm_cpp/src/motorsClient.cpp index 074edf9d4..8fc10f160 100644 --- a/src/libs/jderobotcomm_cpp/src/motorsClient.cpp +++ b/src/libs/comm_cpp/src/motorsClient.cpp @@ -4,15 +4,14 @@ #include #endif -namespace JdeRobotComm { +namespace Comm { MotorsClient* -getMotorsClient(Ice::CommunicatorPtr ic, std::string prefix){ +getMotorsClient(Comm::Communicator* jdrc, std::string prefix){ MotorsClient* client = 0; - Ice::PropertiesPtr prop = ic->getProperties(); - int server = prop->getPropertyAsIntWithDefault(prefix+".Server",0); + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -23,8 +22,8 @@ getMotorsClient(Ice::CommunicatorPtr ic, std::string prefix){ { std::cout << "Sending Velocities by ICE interfaces" << std::endl; MotorsIceClient* cl; - cl = new MotorsIceClient(ic, prefix); - client = (JdeRobotComm::MotorsClient*) cl; + cl = new MotorsIceClient(jdrc, prefix); + client = (Comm::MotorsClient*) cl; break; } case 2: @@ -32,13 +31,13 @@ getMotorsClient(Ice::CommunicatorPtr ic, std::string prefix){ #ifdef JDERROS std::cout << "Sending Velocities by ROS messages" << std::endl; std::string nodeName; - nodeName = prop->getPropertyWithDefault(prefix+".Name","MotorsNode"); + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "MotorsNode"); std::string topic; - topic = prop->getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); PublisherMotors* pm; pm = new PublisherMotors(0, nullptr, nodeName, topic); pm->start(); - client = (JdeRobotComm::MotorsClient*) pm; + client = (Comm::MotorsClient*) pm; #else throw "ERROR: ROS is not available"; #endif diff --git a/src/libs/jderobotcomm_cpp/src/pose3dClient.cpp b/src/libs/comm_cpp/src/pose3dClient.cpp similarity index 65% rename from src/libs/jderobotcomm_cpp/src/pose3dClient.cpp rename to src/libs/comm_cpp/src/pose3dClient.cpp index 3ea994e45..80c8d2f9b 100644 --- a/src/libs/jderobotcomm_cpp/src/pose3dClient.cpp +++ b/src/libs/comm_cpp/src/pose3dClient.cpp @@ -4,15 +4,14 @@ #include #endif -namespace JdeRobotComm { +namespace Comm { Pose3dClient* -getPose3dClient(Ice::CommunicatorPtr ic, std::string prefix){ +getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ Pose3dClient* client = 0; - Ice::PropertiesPtr prop = ic->getProperties(); - int server = prop->getPropertyAsIntWithDefault(prefix+".Server",0); + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -23,9 +22,9 @@ getPose3dClient(Ice::CommunicatorPtr ic, std::string prefix){ { std::cout << "Receiving Pose3D from ICE interfaces" << std::endl; Pose3dIceClient* cl; - cl = new Pose3dIceClient(ic, prefix); + cl = new Pose3dIceClient(jdrc, prefix); cl->start(); - client = (JdeRobotComm::Pose3dClient*) cl; + client = (Comm::Pose3dClient*) cl; break; } case 2: @@ -33,13 +32,13 @@ getPose3dClient(Ice::CommunicatorPtr ic, std::string prefix){ #ifdef JDERROS std::cout << "Receiving Pose3D from ROS messages" << std::endl; std::string nodeName; - nodeName = prop->getPropertyWithDefault(prefix+".Name","PoseNode"); + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "PoseNode"); std::string topic; - topic = prop->getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); ListenerPose* lc; lc = new ListenerPose(0, nullptr, nodeName, topic); lc->start(); - client = (JdeRobotComm::Pose3dClient*) lc; + client = (Comm::Pose3dClient*) lc; #else throw "ERROR: ROS is not available"; #endif diff --git a/src/libs/jderobotcomm_cpp/src/ros/listenerCamera.cpp b/src/libs/comm_cpp/src/ros/listenerCamera.cpp similarity index 94% rename from src/libs/jderobotcomm_cpp/src/ros/listenerCamera.cpp rename to src/libs/comm_cpp/src/ros/listenerCamera.cpp index 2003a4e95..e62fd8932 100644 --- a/src/libs/jderobotcomm_cpp/src/ros/listenerCamera.cpp +++ b/src/libs/comm_cpp/src/ros/listenerCamera.cpp @@ -1,6 +1,6 @@ #include -namespace JdeRobotComm { +namespace Comm { ListenerCamera::ListenerCamera(int argc, char** argv, std::string nodeName, std::string topic){ pthread_mutex_init(&mutex, NULL); @@ -48,7 +48,7 @@ namespace JdeRobotComm { time_t now; time(&now); pthread_mutex_lock(&mutex); - this->image = JdeRobotComm::translate_image_messages(image_msg); + this->image = Comm::translate_image_messages(image_msg); if (difftime(this->timer, now)>=1){ this->refreshRate = this->cont; this->cont = 0; diff --git a/src/libs/jderobotcomm_cpp/src/ros/listenerLaser.cpp b/src/libs/comm_cpp/src/ros/listenerLaser.cpp similarity index 92% rename from src/libs/jderobotcomm_cpp/src/ros/listenerLaser.cpp rename to src/libs/comm_cpp/src/ros/listenerLaser.cpp index ade23f6c8..89fb659ad 100644 --- a/src/libs/jderobotcomm_cpp/src/ros/listenerLaser.cpp +++ b/src/libs/comm_cpp/src/ros/listenerLaser.cpp @@ -1,6 +1,6 @@ #include -namespace JdeRobotComm { +namespace Comm { ListenerLaser::ListenerLaser(int argc, char** argv, std::string nodeName, std::string topic){ pthread_mutex_init(&mutex, NULL); @@ -46,7 +46,7 @@ namespace JdeRobotComm { void ListenerLaser::lasercallback(const sensor_msgs::LaserScanConstPtr& laser_msg){ pthread_mutex_lock(&mutex); - this->laserData = JdeRobotComm::translate_laser_messages(laser_msg); + this->laserData = Comm::translate_laser_messages(laser_msg); pthread_mutex_unlock(&mutex); } diff --git a/src/libs/jderobotcomm_cpp/src/ros/listenerPose.cpp b/src/libs/comm_cpp/src/ros/listenerPose.cpp similarity index 93% rename from src/libs/jderobotcomm_cpp/src/ros/listenerPose.cpp rename to src/libs/comm_cpp/src/ros/listenerPose.cpp index cce00926b..fb79341cb 100644 --- a/src/libs/jderobotcomm_cpp/src/ros/listenerPose.cpp +++ b/src/libs/comm_cpp/src/ros/listenerPose.cpp @@ -1,6 +1,6 @@ #include -namespace JdeRobotComm { +namespace Comm { ListenerPose::ListenerPose(int argc, char** argv, std::string nodeName, std::string topic){ pthread_mutex_init(&mutex, NULL); @@ -45,7 +45,7 @@ namespace JdeRobotComm { void ListenerPose::posecallback(const nav_msgs::OdometryConstPtr& odom_msg){ pthread_mutex_lock(&mutex); - this->pose = JdeRobotComm::translate_odometry_messages(odom_msg); + this->pose = Comm::translate_odometry_messages(odom_msg); pthread_mutex_unlock(&mutex); } diff --git a/src/libs/jderobotcomm_cpp/src/ros/publisherMotors.cpp b/src/libs/comm_cpp/src/ros/publisherMotors.cpp similarity index 98% rename from src/libs/jderobotcomm_cpp/src/ros/publisherMotors.cpp rename to src/libs/comm_cpp/src/ros/publisherMotors.cpp index 103e5cf75..a117ad558 100644 --- a/src/libs/jderobotcomm_cpp/src/ros/publisherMotors.cpp +++ b/src/libs/comm_cpp/src/ros/publisherMotors.cpp @@ -1,6 +1,6 @@ #include -namespace JdeRobotComm { +namespace Comm { PublisherMotors::PublisherMotors(int argc, char** argv, std::string nodeName, std::string topic){ pthread_mutex_init(&mutex, NULL); diff --git a/src/libs/jderobotcomm_cpp/src/ros/translators.cpp b/src/libs/comm_cpp/src/ros/translators.cpp similarity index 99% rename from src/libs/jderobotcomm_cpp/src/ros/translators.cpp rename to src/libs/comm_cpp/src/ros/translators.cpp index ef8402879..04043fa21 100644 --- a/src/libs/jderobotcomm_cpp/src/ros/translators.cpp +++ b/src/libs/comm_cpp/src/ros/translators.cpp @@ -1,5 +1,5 @@ #include "jderobot/comm/ros/translators.hpp" -namespace JdeRobotComm { +namespace Comm { float PI = 3.1415; diff --git a/src/libs/jderobotcomm_py/CMakeLists.txt b/src/libs/comm_py/CMakeLists.txt similarity index 60% rename from src/libs/jderobotcomm_py/CMakeLists.txt rename to src/libs/comm_py/CMakeLists.txt index 64daf194e..eb29861ca 100644 --- a/src/libs/jderobotcomm_py/CMakeLists.txt +++ b/src/libs/comm_py/CMakeLists.txt @@ -3,12 +3,12 @@ cmake_minimum_required(VERSION 2.8) #macro configure files macro(configure_jderobotcomm_py in) - file(GLOB_RECURSE files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}/jderobotComm/ *.${in}) + file(GLOB_RECURSE files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}/comm/ *.${in}) foreach(file ${files}) string(REGEX REPLACE "\\.${in}$" "" f "${file}") - configure_file_python(jderobotComm/${file} jderobotComm/${f}) + configure_file_python(comm/${file} comm/${f}) endforeach(file ${files}) @@ -21,6 +21,6 @@ ELSE() endif() add_custom_target(jderobotcomm_py ALL) -copy_to_binary_python(jderobotcomm_py jderobotComm) +copy_to_binary_python(jderobotcomm_py comm) -install_python(jderobotComm comm-python) \ No newline at end of file +install_python(comm comm-python) \ No newline at end of file diff --git a/src/libs/comm_py/comm/__init__.py b/src/libs/comm_py/comm/__init__.py new file mode 100644 index 000000000..f14587b65 --- /dev/null +++ b/src/libs/comm_py/comm/__init__.py @@ -0,0 +1,22 @@ +from .communicator import Communicator + + + + +def init (config, prefix): + ''' + Starts JdeRobotComm + + @param config: configuration of client + + @type config: dict + ''' + return Communicator(config, prefix) + + + + + + + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.only-ice.in b/src/libs/comm_py/comm/cameraClient.py.only-ice.in similarity index 58% rename from src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.only-ice.in rename to src/libs/comm_py/comm/cameraClient.py.only-ice.in index 46b696f7e..62e6c3950 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cameraClient.py.only-ice.in @@ -2,12 +2,12 @@ import sys import Ice from .ice.cameraIceClient import CameraIceClient -def __getCameraIceClient(ic, prefix): +def __getCameraIceClient(jdrc, prefix): ''' Returns a Camera Ice Client. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -16,16 +16,16 @@ def __getCameraIceClient(ic, prefix): ''' print("Receiving " + prefix + " Image from ICE interfaces") - client = CameraIceClient(ic, prefix) + client = CameraIceClient(jdrc, prefix) client.start() return client -def __getListenerCamera(ic, prefix): +def __getListenerCamera(jdrc, prefix): ''' Returns a Camera ROS Subscriber. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -37,12 +37,12 @@ def __getListenerCamera(ic, prefix): print(prefix + ": ROS msg are diabled") return None -def __Cameradisabled(ic, prefix): +def __Cameradisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -53,24 +53,23 @@ def __Cameradisabled(ic, prefix): print( prefix + " Disabled") return None -def getCameraClient (ic, prefix, node=None): +def getCameraClient (jdrc, prefix): ''' Returns a Camera Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator - @type prefix: String - @type node: ROS node + @type jdrc: Comm Communicator + @type name: String @return None if Camera is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, name) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.ros.in b/src/libs/comm_py/comm/cameraClient.py.ros.in similarity index 60% rename from src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.ros.in rename to src/libs/comm_py/comm/cameraClient.py.ros.in index a98a8ca46..7f84d847b 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py.ros.in +++ b/src/libs/comm_py/comm/cameraClient.py.ros.in @@ -6,12 +6,12 @@ from .ice.cameraIceClient import CameraIceClient if (sys.version_info[0] == 2): from .ros.listenerCamera import ListenerCamera -def __getCameraIceClient(ic, prefix): +def __getCameraIceClient(jdrc, prefix): ''' Returns a Camera Ice Client. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -20,16 +20,16 @@ def __getCameraIceClient(ic, prefix): ''' print("Receiving " + prefix + " Image from ICE interfaces") - client = CameraIceClient(ic, prefix) + client = CameraIceClient(jdrc, prefix) client.start() return client -def __getListenerCamera(ic, prefix): +def __getListenerCamera(jdrc, prefix): ''' Returns a Camera ROS Subscriber. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -39,20 +39,19 @@ def __getListenerCamera(ic, prefix): ''' if (sys.version_info[0] == 2): print("Receiving " + prefix + " Image from ROS messages") - prop = prop = ic.getProperties() - topic = prop.getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc.getConfig().getProperty(prefix+".Topic") client = ListenerCamera(topic) return client else: print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) return None -def __Cameradisabled(ic, prefix): +def __Cameradisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getCameraClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file @type ic: Ice Communicator @type prefix: String @@ -63,24 +62,23 @@ def __Cameradisabled(ic, prefix): print( prefix + " Disabled") return None -def getCameraClient (ic, prefix, node=None): +def getCameraClient (jdrc, prefix): ''' Returns a Camera Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator - @type prefix: String - @type node: ROS node + @type jdrc: Comm Communicator + @type name: String @return None if Camera is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/communicator.py.only-ice.in b/src/libs/comm_py/comm/communicator.py.only-ice.in new file mode 100644 index 000000000..8fe745cd0 --- /dev/null +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -0,0 +1,113 @@ +import Ice + +from .laserClient import getLaserClient +from .cameraClient import getCameraClient +from .pose3dClient import getPose3dClient +from .motorsClient import getMotorsClient +from .ptMotorsClient import getPTMotorsClient + + +class Communicator: + ''' + Comm Communicator class + + ''' + def __init__ (self, config, prefix): + ''' + Communicator constructor + + @param config: configuration of communicator + + @type config: dict + + ''' + rosserver = False + iceserver = False + + self.__node = None + self.__ic = None + self.config = config + + ymlNode = self.config.getProperty(prefix) + for i in ymlNode: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + iceserver = True + + if iceserver: + id = Ice.InitializationData() + self.__ic = Ice.initialize(None, id) + + def destroy (self): + ''' + Destroys ROS Node and Ice Communicator if it is necessary. + + ''' + if self.__ic: + self.__ic.shutdown() + self.__ic.destroy() + + def getNode(self): + return self.__node + + def getIc(self): + return self.__ic + + def getConfig(self): + return self.config + + + def getCameraClient(self, name): + ''' + Returns a Camera client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getCameraClient(self, name) + + def getMotorsClient(self, name): + ''' + Returns a Motors client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getMotorsClient(self, name) + + def getPTMotorsClient(self, name): + ''' + Returns a PTMotors client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getPTMotorsClient(self, name) + + def getPose3dClient(self, name): + ''' + Returns a Pose3D client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getPose3dClient(self, name) + + def getLaserClient(self, name): + ''' + Returns a Laser client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getLaserClient(self, name) + diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in new file mode 100644 index 000000000..53a65dd03 --- /dev/null +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -0,0 +1,108 @@ +import Ice +import rospy + +from .laserClient import getLaserClient +from .cameraClient import getCameraClient +from .pose3dClient import getPose3dClient +from .motorsClient import getMotorsClient + + +class Communicator: + ''' + Comm Communicator class + + ''' + def __init__ (self, config, prefix): + ''' + Communicator constructor + + @param config: configuration of communicator + + @type config: dict + + ''' + rosserver = False + iceserver = False + + self.__node = None + self.__ic = None + self.config = config + + ymlNode = self.config.getProperty(prefix) + for i in ymlNode: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + iceserver = True + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 2: + rosserver = True + + if rosserver: + self.__node = rospy.init_node(ymlNode["NodeName"], anonymous=True) + if iceserver: + id = Ice.InitializationData() + self.__ic = Ice.initialize(None, id) + + def destroy (self): + ''' + Destroys ROS Node and Ice Communicator if it is necessary. + + ''' + if self.__node: + rospy.signal_shutdown("Node Closed") + if self.__ic: + self.__ic.shutdown() + self.__ic.destroy() + + def getNode(self): + return self.__node + + def getIc(self): + return self.__ic + + def getConfig(self): + return self.config + + + def getCameraClient(self, name): + ''' + Returns a Camera client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getCameraClient(self, name) + + def getMotorsClient(self, name): + ''' + Returns a Motors client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getMotorsClient(self, name) + + def getPose3dClient(self, name): + ''' + Returns a Pose3D client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getPose3dClient(self, name) + + def getLaserClient(self, name): + ''' + Returns a Laser client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getLaserClient(self, name) + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/__init__.py b/src/libs/comm_py/comm/ice/__init__.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ice/__init__.py rename to src/libs/comm_py/comm/ice/__init__.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py b/src/libs/comm_py/comm/ice/cameraIceClient.py similarity index 87% rename from src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py rename to src/libs/comm_py/comm/ice/cameraIceClient.py index ef78c2658..15a7f61e4 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py +++ b/src/libs/comm_py/comm/ice/cameraIceClient.py @@ -31,15 +31,15 @@ class Camera: Camera Connector. Recives image from Ice interface when you run update method. ''' - def __init__(self, ic, prefix): + def __init__(self, jdrc, prefix): ''' Camera Contructor. Exits When it receives a Exception diferent to Ice.ConnectionRefusedException - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String ''' @@ -47,10 +47,12 @@ def __init__(self, ic, prefix): self.image = Image() try: - basecamera = ic.propertyToProxy(prefix+".Proxy") + + ic = jdrc.getIc() + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + basecamera = ic.stringToProxy(proxyStr) self.proxy = jderobot.CameraPrx.checkedCast(basecamera) - prop = ic.getProperties() - self.imgFormat = prop.getProperty(prefix+".Format") + self.imgFormat = jdrc.getConfig().getProperty(prefix+".Format") if not self.imgFormat: self.imgFormat = "RGB8" @@ -117,19 +119,19 @@ class CameraIceClient: ''' Camera Ice Client. Recives image from Ice interface running camera update method in a thread. ''' - def __init__(self,ic,prefix, start = False): + def __init__(self,jdrc,prefix, start = False): ''' CameraIceClient Contructor. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file @param start: indicates if start automatically the client - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @type start: Boolean ''' - self.camera = Camera(ic,prefix) + self.camera = Camera(jdrc,prefix) self.kill_event = threading.Event() self.thread = ThreadSensor(self.camera, self.kill_event) diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py b/src/libs/comm_py/comm/ice/laserIceClient.py similarity index 95% rename from src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py rename to src/libs/comm_py/comm/ice/laserIceClient.py index f89a45ba4..0b7013531 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py +++ b/src/libs/comm_py/comm/ice/laserIceClient.py @@ -30,12 +30,12 @@ class Laser: Laser Connector. Recives LaserData from Ice interface when you run update method. ''' - def __init__(self, ic, prefix): + def __init__(self, jdrc, prefix): ''' Laser Contructor. Exits When it receives a Exception diferent to Ice.ConnectionRefusedException - @param ic: Ice Communicator + @param jdrc: Comm Communicator @param prefix: prefix name of client in config file @type ic: Ice Communicator @@ -45,9 +45,10 @@ def __init__(self, ic, prefix): self.laser = LaserData() try: - base = ic.propertyToProxy(prefix+".Proxy") + ic = jdrc.getIc() + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) self.proxy = jderobot.LaserPrx.checkedCast(base) - prop = ic.getProperties() self.update() diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py b/src/libs/comm_py/comm/ice/motorsIceClient.py similarity index 82% rename from src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py rename to src/libs/comm_py/comm/ice/motorsIceClient.py index 53a269609..f2ba33eb2 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py +++ b/src/libs/comm_py/comm/ice/motorsIceClient.py @@ -27,11 +27,23 @@ class MotorsIceClient: - def __init__(self, ic, prefix): + + ''' + Motors Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param jdrc: Comm Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + ''' + def __init__(self, jdrc, prefix): self.lock = threading.Lock() - prop = ic.getProperties() + ic = jdrc.getIc() + - maxWstr = prop.getProperty(prefix+".maxW") + maxWstr = jdrc.getConfig().getProperty(prefix+".maxW") if maxWstr: self.maxW = float(maxWstr) else: @@ -39,7 +51,7 @@ def __init__(self, ic, prefix): print (prefix+".maxW not provided, the default value is used: "+ repr(self.maxW)) - maxVstr = prop.getProperty(prefix+".maxV") + maxVstr = jdrc.getConfig().getProperty(prefix+".maxV") if maxWstr: self.maxV = float(maxVstr) else: @@ -47,7 +59,8 @@ def __init__(self, ic, prefix): print (prefix+".maxV not provided, the default value is used: "+ repr(self.maxV)) try: - base = ic.propertyToProxy(prefix+".Proxy") + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) self.proxy = jderobot.MotorsPrx.checkedCast(base) diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py b/src/libs/comm_py/comm/ice/pose3dIceClient.py similarity index 96% rename from src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py rename to src/libs/comm_py/comm/ice/pose3dIceClient.py index 1da49c129..256dc20c4 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py +++ b/src/libs/comm_py/comm/ice/pose3dIceClient.py @@ -31,12 +31,12 @@ class Pose3D: Pose3d Connector. Recives Pose3d from Ice interface when you run update method. ''' - def __init__(self, ic, prefix): + def __init__(self, jdrc, prefix): ''' Pose3d Contructor. Exits When it receives a Exception diferent to Ice.ConnectionRefusedException - @param ic: Ice Communicator + @param jdrc: Comm Communicator @param prefix: prefix name of client in config file @type ic: Ice Communicator @@ -47,7 +47,9 @@ def __init__(self, ic, prefix): self.pose = Pose3d() try: - base = ic.propertyToProxy(prefix+".Proxy") + ic = jdrc.getIc() + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) self.proxy = jderobot.Pose3DPrx.checkedCast(base) prop = ic.getProperties() diff --git a/src/libs/comm_py/comm/ice/ptMotorsIceClient.py b/src/libs/comm_py/comm/ice/ptMotorsIceClient.py new file mode 100644 index 000000000..18f972ef7 --- /dev/null +++ b/src/libs/comm_py/comm/ice/ptMotorsIceClient.py @@ -0,0 +1,123 @@ +# -*- coding: utf-8 -*- +# +# Copyright (C) 1997-2016 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# + +import traceback +import jderobot +import threading +import Ice +from .threadSensor import ThreadSensor + + +class PTMotors: + + def __init__(self, ic, prefix): + self.lock = threading.Lock() + + self.data=jderobot.PTMotorsData() + + self.params=jderobot.PTMotorsParams() + prop = ic.getProperties() + + try: + base = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.PTMotorsPrx.checkedCast(base) + + if not self.proxy: + print ('Interface ' + prefix + ' not configured') + + else: + self.params = self.proxy.getPTMotorsParams() + + print ("+++ MAX/MIN Pan/Tilt Values +++") + print ("+ Min Pan: " + str(self.params.minPan) + " +") + print ("+ Max Pan: " + str(self.params.maxPan) + " +") + print ("+ Max Pan speed: " + str(self.params.maxPanSpeed) + " +") + print ("+ Min Tilt: " + str(self.params.minTilt) + " +") + print ("+ Max Tilt: " + str(self.params.maxTilt) + " +") + print ("+ Max Tilt speed: " + str(self.params.maxTiltSpeed) + " +") + print ("++++++++++++++++++++++++++++++") + + except Ice.ConnectionRefusedException: + print(prefix + ': connection refused') + + except: + traceback.print_exc() + exit(-1) + + def getLimits(self): + self.lock.acquire() + params = self.params + self.lock.release() + + return params + + def setPTMotorsData(self, pan, tilt, panspeed, tiltspeed): + + self.lock.acquire() + self.data.pan = pan + self.data.tilt = tilt + self.data.panSpeed = panspeed + self.data.tiltSpeed = tiltspeed + self.lock.release() + + + + def sendPTMotorsData(self): + if self.hasproxy(): + self.lock.acquire() + self.proxy.setPTMotorsData(self.data) + self.lock.release() + + def hasproxy (self): + return hasattr(self,"proxy") and self.proxy + + def update(self): + self.sendPTMotorsData() + + + +class PTMotorsIceClient: + def __init__(self,ic,prefix, start = False): + self.motors = PTMotors(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.motors, self.kill_event) + self.thread.daemon = True + + if start: + self.start() + + # if client is stopped you can not start again, Threading.Thread raised error + def start(self): + self.kill_event.clear() + self.thread.start() + + # if client is stopped you can not start again + def stop(self): + self.kill_event.set() + + def getLimits(self): + return self.motors.getLimits() + + def hasproxy(): + return self.motors.hasproxy() + + def setPTMotorsData(self, pan, tilt, panspeed, tiltspeed): + self.motors.setPTMotorsData(pan, tilt, panspeed, tiltspeed) diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/threadSensor.py b/src/libs/comm_py/comm/ice/threadSensor.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ice/threadSensor.py rename to src/libs/comm_py/comm/ice/threadSensor.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/laserClient.py.only-ice.in b/src/libs/comm_py/comm/laserClient.py.only-ice.in similarity index 52% rename from src/libs/jderobotcomm_py/jderobotComm/laserClient.py.only-ice.in rename to src/libs/comm_py/comm/laserClient.py.only-ice.in index 7f25d9e74..4769be465 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/laserClient.py.only-ice.in +++ b/src/libs/comm_py/comm/laserClient.py.only-ice.in @@ -3,32 +3,32 @@ import Ice from .ice.laserIceClient import LaserIceClient -def __getLaserIceClient(ic, prefix): +def __getLaserIceClient(jdrc, prefix): ''' Returns a Laser Ice Client. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Laser Ice Client ''' print("Receiving " + prefix + " LaserData from ICE interfaces") - client = LaserIceClient(ic, prefix) + client = LaserIceClient(jdrc, prefix) client.start() return client -def __getListenerLaser(ic, prefix): +def __getListenerLaser(jdrc, prefix): ''' Returns a Laser ROS Subscriber. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Laser ROS Subscriber @@ -37,14 +37,14 @@ def __getListenerLaser(ic, prefix): print(prefix + ": ROS msg are diabled") return None -def __Laserdisabled(ic, prefix): +def __Laserdisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -53,24 +53,23 @@ def __Laserdisabled(ic, prefix): print( prefix + " Disabled") return None -def getLaserClient (ic, prefix, node=None): +def getLaserClient (jdrc, prefix): ''' Returns a Laser Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param name: name of client in config file - @type ic: Ice Communicator - @type prefix: String - @type node: ROS node + @type jdrc: Comm Communicator + @type name: String @return None if Laser is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/laserClient.py.ros.in b/src/libs/comm_py/comm/laserClient.py.ros.in similarity index 56% rename from src/libs/jderobotcomm_py/jderobotComm/laserClient.py.ros.in rename to src/libs/comm_py/comm/laserClient.py.ros.in index 934cd025e..c6145f208 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/laserClient.py.ros.in +++ b/src/libs/comm_py/comm/laserClient.py.ros.in @@ -6,32 +6,32 @@ from .ice.laserIceClient import LaserIceClient if (sys.version_info[0] == 2): from .ros.listenerLaser import ListenerLaser -def __getLaserIceClient(ic, prefix): +def __getLaserIceClient(jdrc, prefix): ''' Returns a Laser Ice Client. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Laser Ice Client ''' print("Receiving " + prefix + " LaserData from ICE interfaces") - client = LaserIceClient(ic, prefix) + client = LaserIceClient(jdrc, prefix) client.start() return client -def __getListenerLaser(ic, prefix): +def __getListenerLaser(jdrc, prefix): ''' Returns a Laser ROS Subscriber. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Laser ROS Subscriber @@ -39,22 +39,21 @@ def __getListenerLaser(ic, prefix): ''' if (sys.version_info[0] == 2): print("Receiving " + prefix + " LaserData from ROS messages") - prop = prop = ic.getProperties() - topic = prop.getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc.getConfig().getProperty(prefix+".Topic") client = ListenerLaser(topic) return client else: print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) return None -def __Laserdisabled(ic, prefix): +def __Laserdisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getLaserClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -63,24 +62,23 @@ def __Laserdisabled(ic, prefix): print( prefix + " Disabled") return None -def getLaserClient (ic, prefix, node=None): +def getLaserClient (jdrc, prefix): ''' Returns a Laser Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator - @type prefix: String - @type node: ROS node + @type jdrc: Comm Communicator + @type name: String @return None if Laser is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.only-ice.in b/src/libs/comm_py/comm/motorsClient.py.only-ice.in similarity index 54% rename from src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.only-ice.in rename to src/libs/comm_py/comm/motorsClient.py.only-ice.in index 63dff0f06..e27632584 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/motorsClient.py.only-ice.in @@ -4,32 +4,32 @@ from .ice.motorsIceClient import MotorsIceClient -def __getMotorsIceClient(ic, prefix): +def __getMotorsIceClient(jdrc, prefix): ''' Returns a Motors Ice Client. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Motors Ice Client ''' - print("Publishing Motors with ICE interfaces") - client = MotorsIceClient(ic, prefix) + print("Publishing "+ prefix +" with ICE interfaces") + client = MotorsIceClient(jdrc, prefix) client.start() return client -def __getPublisherMotors(ic, prefix): +def __getPublisherMotors(jdrc, prefix): ''' Returns a Motors ROS Publisher. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Motors ROS Publisher @@ -39,14 +39,14 @@ def __getPublisherMotors(ic, prefix): print(prefix + ": ROS msg are diabled") return None -def __Motorsdisabled(ic, prefix): +def __Motorsdisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -55,24 +55,25 @@ def __Motorsdisabled(ic, prefix): print(prefix + " Disabled") return None -def getMotorsClient (ic, prefix, node=None): +def getMotorsClient (jdrc, prefix): ''' Returns a Motors Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file @param node: ROS node - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @type node: ROS node @return None if Motors is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.ros.in b/src/libs/comm_py/comm/motorsClient.py.ros.in similarity index 54% rename from src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.ros.in rename to src/libs/comm_py/comm/motorsClient.py.ros.in index 787d5f9c9..58859a2bd 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py.ros.in +++ b/src/libs/comm_py/comm/motorsClient.py.ros.in @@ -6,54 +6,49 @@ from .ice.motorsIceClient import MotorsIceClient if (sys.version_info[0] == 2): from .ros.publisherMotors import PublisherMotors -def __getMotorsIceClient(ic, prefix): +def __getMotorsIceClient(jdrc, prefix): ''' Returns a Motors Ice Client. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Motors Ice Client ''' - print("Publishing Motors with ICE interfaces") - client = MotorsIceClient(ic, prefix) + print("Publishing "+ prefix +" with ICE interfaces") + client = MotorsIceClient(jdrc, prefix) client.start() return client -def __getPublisherMotors(ic, prefix): +def __getPublisherMotors(jdrc, prefix): ''' Returns a Motors ROS Publisher. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Motors ROS Publisher ''' if (sys.version_info[0] == 2): - print("Publishing Motors with ROS messages") - prop = prop = ic.getProperties() - topic = prop.getPropertyWithDefault(prefix+".Topic","") - - maxWstr = prop.getProperty(prefix+".maxW") - if maxWstr: - maxW = float(maxWstr) - else: + print("Publishing "+ prefix + " with ROS messages") + topic = jdrc.getConfig().getProperty(prefix+".Topic") + + maxW = jdrc.getConfig().getProperty(prefix+".maxW") + if not maxW: maxW = 0.5 print (prefix+".maxW not provided, the default value is used: "+ repr(maxW)) - maxVstr = prop.getProperty(prefix+".maxV") - if maxWstr: - maxV = float(maxVstr) - else: + maxV = jdrc.getConfig().getProperty(prefix+".maxV") + if not maxV: maxV = 5 print (prefix+".maxV not provided, the default value is used: "+ repr(maxV)) @@ -64,14 +59,14 @@ def __getPublisherMotors(ic, prefix): print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) return None -def __Motorsdisabled(ic, prefix): +def __Motorsdisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getMotorsClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -80,24 +75,25 @@ def __Motorsdisabled(ic, prefix): print(prefix + " Disabled") return None -def getMotorsClient (ic, prefix, node=None): +def getMotorsClient (jdrc, prefix): ''' Returns a Motors Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: Name of client in config file @param node: ROS node - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @type node: ROS node @return None if Motors is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.only-ice.in b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in similarity index 54% rename from src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.only-ice.in rename to src/libs/comm_py/comm/pose3dClient.py.only-ice.in index adb4b3163..dc66d9326 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.only-ice.in +++ b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in @@ -3,32 +3,32 @@ import Ice from .ice.pose3dIceClient import Pose3dIceClient -def __getPoseIceClient(ic, prefix): +def __getPoseIceClient(jdrc, prefix): ''' Returns a Pose3D Ice Client. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @@param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Pose3D Ice Client ''' print("Receiving " + prefix + " from ICE interfaces") - client = Pose3dIceClient(ic, prefix) + client = Pose3dIceClient(jdrc, prefix) client.start() return client -def __getListenerPose(ic, prefix): +def __getListenerPose(jdrc, prefix): ''' Returns a Pose3D ROS Subscriber. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Pose3D ROS Subscriber @@ -38,14 +38,14 @@ def __getListenerPose(ic, prefix): return None -def __Posedisabled(ic, prefix): +def __Posedisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -54,24 +54,23 @@ def __Posedisabled(ic, prefix): print(prefix + " Disabled") return None -def getPose3dClient (ic, prefix, node=None): +def getPose3dClient (jdrc, prefix): ''' Returns a Pose3D Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String - @type node: ROS node @return None if pose3d is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.ros.in b/src/libs/comm_py/comm/pose3dClient.py.ros.in similarity index 57% rename from src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.ros.in rename to src/libs/comm_py/comm/pose3dClient.py.ros.in index f052ce976..3ed047f00 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py.ros.in +++ b/src/libs/comm_py/comm/pose3dClient.py.ros.in @@ -8,32 +8,32 @@ if (sys.version_info[0] == 2): -def __getPoseIceClient(ic, prefix): +def __getPoseIceClient(jdrc, prefix): ''' Returns a Pose3D Ice Client. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @@param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Pose3D Ice Client ''' print("Receiving " + prefix + " from ICE interfaces") - client = Pose3dIceClient(ic, prefix) + client = Pose3dIceClient(jdrc, prefix) client.start() return client -def __getListenerPose(ic, prefix): +def __getListenerPose(jdrc, prefix): ''' Returns a Pose3D ROS Subscriber. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return Pose3D ROS Subscriber @@ -41,8 +41,7 @@ def __getListenerPose(ic, prefix): ''' if (sys.version_info[0] == 2): print("Receiving " + prefix + " from ROS messages") - prop = prop = ic.getProperties() - topic = prop.getPropertyWithDefault(prefix+".Topic",""); + topic = jdrc.getConfig().getProperty(prefix+".Topic") client = ListenerPose3d(topic) return client else: @@ -50,14 +49,14 @@ def __getListenerPose(ic, prefix): return None -def __Posedisabled(ic, prefix): +def __Posedisabled(jdrc, prefix): ''' Prints a warning that the client is disabled. This function should never be used. Use getPose3dClient instead of this - @param ic: Ice Communicator - @param prefix: prefix name of client in config file + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String @return None @@ -66,24 +65,23 @@ def __Posedisabled(ic, prefix): print(prefix + " Disabled") return None -def getPose3dClient (ic, prefix, node=None): +def getPose3dClient (jdrc, prefix): ''' Returns a Pose3D Client. - @param ic: Ice Communicator - @param prefix: prefix name of client in config file - @param node: ROS node + @param jdrc: Comm Communicator + @param prefix: name of client in config file - @type ic: Ice Communicator + @type jdrc: Comm Communicator @type prefix: String - @type node: ROS node @return None if pose3d is disabled ''' - prop = prop = ic.getProperties() - server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](ic, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in new file mode 100644 index 000000000..7052214ba --- /dev/null +++ b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.ptMotorsIceClient import PTMotorsIceClient + + + +def __getPTMotorsIceClient(jdrc, prefix): + ''' + Returns a PTMotors Ice Client. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return PTMotors Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = PTMotorsIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherPTMotors(jdrc, prefix): + ''' + Returns a PTMotors ROS Publisher. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return PTMotors ROS Publisher + + ''' + + print(prefix + ": ROS msg are diabled") + return None + +def __PTMotorsdisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return None + + ''' + print(prefix + " Disabled") + return None + +def getPTMotorsClient (jdrc, prefix): + ''' + Returns a PTMotors Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if PTMotors is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in new file mode 100644 index 000000000..0895d616d --- /dev/null +++ b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.ptMotorsIceClient import PTMotorsIceClient + + + +def __getPTMotorsIceClient(jdrc, prefix): + ''' + Returns a PTMotors Ice Client. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return PTMotors Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = PTMotorsIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherPTMotors(jdrc, prefix): + ''' + Returns a PTMotors ROS Publisher. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return PTMotors ROS Publisher + + ''' + + print(prefix + ": This Interface doesn't support ROS msg") + return None + +def __PTMotorsdisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getPTMotorsClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return None + + ''' + print(prefix + " Disabled") + return None + +def getPTMotorsClient (jdrc, prefix): + ''' + Returns a PTMotors Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if PTMotors is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/__init__.py b/src/libs/comm_py/comm/ros/__init__.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ros/__init__.py rename to src/libs/comm_py/comm/ros/__init__.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py b/src/libs/comm_py/comm/ros/listenerCamera.py similarity index 90% rename from src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py rename to src/libs/comm_py/comm/ros/listenerCamera.py index a16ecf7e9..a9376dedb 100644 --- a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py +++ b/src/libs/comm_py/comm/ros/listenerCamera.py @@ -95,4 +95,13 @@ def getImage(self): return image + def hasproxy (self): + ''' + Returns if Subscriber has ben created or not. + + @return if Subscriber has ben created or not (Boolean) + + ''' + return hasattr(self,"sub") and self.sub + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerLaser.py b/src/libs/comm_py/comm/ros/listenerLaser.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ros/listenerLaser.py rename to src/libs/comm_py/comm/ros/listenerLaser.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerPose3d.py b/src/libs/comm_py/comm/ros/listenerPose3d.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ros/listenerPose3d.py rename to src/libs/comm_py/comm/ros/listenerPose3d.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/publisherMotors.py b/src/libs/comm_py/comm/ros/publisherMotors.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ros/publisherMotors.py rename to src/libs/comm_py/comm/ros/publisherMotors.py diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/threadPublisher.py b/src/libs/comm_py/comm/ros/threadPublisher.py similarity index 100% rename from src/libs/jderobotcomm_py/jderobotComm/ros/threadPublisher.py rename to src/libs/comm_py/comm/ros/threadPublisher.py diff --git a/src/libs/comm_py/test.yml b/src/libs/comm_py/test.yml new file mode 100644 index 000000000..e22b1cb39 --- /dev/null +++ b/src/libs/comm_py/test.yml @@ -0,0 +1,38 @@ +Test: + Motors: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Motors:default -h localhost -p 9001" + Topic: "/cmd_vel_mux/input/teleop" + Name: testMotors + maxV: 3 + maxW: 0.7 + + Camera1: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraL:default -h localhost -p 9001" + Format: RGB8 + Topic: "/camera/rgb/image_raw" + Name: testCamera1 + + Camera2: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraR:default -h localhost -p 9001" + Format: RGB8 + Topic: "/camera/rgb/image_raw" + Name: testCamera2 + + Pose3D: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Pose3D:default -h localhost -p 9001" + Topic: "/odom" + Name: testPose3d + + Laser: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Laser:default -h localhost -p 9001" + Topic: "/scan" + Name: testLaser + + Vmax: 3 + Wmax: 0.7 + NodeName: JdeRobotCommTest \ No newline at end of file diff --git a/src/libs/comm_py/testCamera.py b/src/libs/comm_py/testCamera.py new file mode 100755 index 000000000..dece71e67 --- /dev/null +++ b/src/libs/comm_py/testCamera.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python2 +import config +import comm +import sys +import time +import signal + +from jderobotTypes import Image + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + client = jdrc.getCameraClient("Test.Camera1") + + for i in range (10): + #print("client1", end=":") + image = client.getImage() + print(image) + time.sleep(1) + + client.stop() + + + jdrc.destroy() + + + + + + + + + diff --git a/src/libs/jderobotcomm_py/testLaser.py b/src/libs/comm_py/testLaser.py similarity index 55% rename from src/libs/jderobotcomm_py/testLaser.py rename to src/libs/comm_py/testLaser.py index 39d3135e2..75de3f623 100755 --- a/src/libs/jderobotcomm_py/testLaser.py +++ b/src/libs/comm_py/testLaser.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python3 -import easyiceconfig as EasyIce -import jderobotComm as comm +#!/usr/bin/env python2 +import config +import comm import sys import time import signal @@ -12,11 +12,11 @@ if __name__ == '__main__': - ic = EasyIce.initialize(sys.argv) - ic, node = comm.init(ic) + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") - client = comm.getLaserClient(ic, "kobukiViewer.Laser") - client2 = comm.getLaserClient(ic, "kobukiViewer.Laser") + client = jdrc.getLaserClient("Test.Laser") + client2 = jdrc.getLaserClient("Test.Laser") for i in range (10): #print("client1", end=":") @@ -30,7 +30,7 @@ print(laser) time.sleep(1) - comm.destroy(ic, node) + jdrc.destroy() diff --git a/src/libs/comm_py/testMotors.py b/src/libs/comm_py/testMotors.py new file mode 100644 index 000000000..1b9b554c7 --- /dev/null +++ b/src/libs/comm_py/testMotors.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +import config +import comm +import sys +import time +import signal + +from jderobotTypes import CMDVel + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + vel = CMDVel() + vel.vx = 1 + vel.az = 0.1 + + client = jdrc.getMotorsClient("Test.Motors") + for i in range (10): + client.sendVelocities(vel) + time.sleep(1) + + jdrc.destroy() \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/testPose.py b/src/libs/comm_py/testPose.py similarity index 55% rename from src/libs/jderobotcomm_py/testPose.py rename to src/libs/comm_py/testPose.py index eaf4b026c..d2171a9fe 100644 --- a/src/libs/jderobotcomm_py/testPose.py +++ b/src/libs/comm_py/testPose.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -import easyiceconfig as EasyIce -import jderobotComm as comm +import config +import comm import sys import time import signal @@ -12,10 +12,10 @@ if __name__ == '__main__': - ic = EasyIce.initialize(sys.argv) - ic, node = comm.init(ic) + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") - client = comm.getPose3dClient(ic, "kobukiViewer.Pose3D") + client = jdrc.getPose3dClient("Test.Pose3D") for i in range (10): #print("client1", end=":") @@ -23,4 +23,4 @@ print(laser) time.sleep(1) - comm.destroy(ic, node) \ No newline at end of file + jdrc.destroy() \ No newline at end of file diff --git a/src/libs/config_cpp/CMakeLists.txt b/src/libs/config_cpp/CMakeLists.txt new file mode 100644 index 000000000..5a93f1ab4 --- /dev/null +++ b/src/libs/config_cpp/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8) +project(config) + +### Project config +include_directories( + include + ${CMAKE_CURRENT_BINARY_DIR}/include +) + +configure_file( + include/jderobot/config/hardcodedlocations.h.in + include/jderobot/config/hardcodedlocations.h + @ONLY +) + +set(HEADERS + include/jderobot/config/config.h + ${CMAKE_CURRENT_BINARY_DIR}/include/jderobot/config/hardcodedlocations.h + include/jderobot/config/loader.hpp + include/jderobot/config/properties.hpp + include/jderobot/config/stdutils.hpp +) + +set(SOURCES + src/loader.cpp + src/properties.cpp +) + + +## Adding shared library for common usage +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES}) + +## Adding static library for single .so configurations +# since target is a shared library, -fPIC must be provided +add_library(${PROJECT_NAME}-embedded STATIC ${SOURCES} ${HEADERS}) +target_link_libraries(${PROJECT_NAME}-embedded ${YAML_CPP_LIBRARIES}) +set_property(TARGET ${PROJECT_NAME}-embedded PROPERTY POSITION_INDEPENDENT_CODE 1) + +## Export library variables (like find_package) +set(${PROJECT_NAME}_FOUND 1 CACHE BOOL "Find(${PROJECT_NAME})") +set(${PROJECT_NAME}_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_BINARY_DIR}/include" CACHE PATH "Find(${PROJECT_NAME})") +set(${PROJECT_NAME}_LIBRARY_DIRS "${CMAKE_CURRENT_BINARY_DIR}" CACHE PATH "Find(${PROJECT_NAME})") +set(${PROJECT_NAME}_LIBRARIES "${PROJECT_NAME}" CACHE STRINGS "Find(${PROJECT_NAME})") + + +## demo +add_executable(config_demo src/demo/demo.cpp ${HEADERS}) +target_link_libraries(config_demo ${YAML_CPP_LIBRARIES} ${PROJECT_NAME}) +configure_file(src/demo/demo.yml demo.yml) + + +## tests +#add_executable(config_test_stdutils src/tests/test_stdutils.cpp) + +#add_executable(config_test_loader src/tests/test_loader.cpp) +#target_link_libraries(config_test_loader ${PROJECT_NAME} ${YAML_CPP_LIBRARIES}) + + +### Install +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}-embedded + DESTINATION ${CMAKE_INSTALL_PREFIX}/lib + COMPONENT config +) +install(DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_PREFIX}/include + COMPONENT config +) diff --git a/src/libs/config_cpp/include/jderobot/config/config.h b/src/libs/config_cpp/include/jderobot/config/config.h new file mode 100644 index 000000000..ab9bec257 --- /dev/null +++ b/src/libs/config_cpp/include/jderobot/config/config.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOT_CONFIG_CONFIG_H +#define JDEROBOT_CONFIG_CONFIG_H + +/** + * @mainpage Config + * JdeRobot COnfig library + * + * @author Aitor Martinez Fernandez , .aitor.martinez.fernandez@gmail.com + * @date September 2017 + * @version 0.9.0 (alpha) + */ + +#include +#include +#include + +namespace Config{ + + +/** + * @brief loads propierties from a file + * + * + * @param filename + * + * + * @return config class with all properties + */ +inline +Config::Properties load(int argc, char* argv[]) +{ + std::string filename (argv[1]); + return jderobotconfig::loader::load(filename);} + +} //NS +#endif // JDEROBOT_CONFIG_CONFIG_H diff --git a/src/libs/config_cpp/include/jderobot/config/hardcodedlocations.h.in b/src/libs/config_cpp/include/jderobot/config/hardcodedlocations.h.in new file mode 100644 index 000000000..1f32e8b41 --- /dev/null +++ b/src/libs/config_cpp/include/jderobot/config/hardcodedlocations.h.in @@ -0,0 +1,32 @@ +/* + * Copyright (C) 1997-2016 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Victor Arribas Raigadas + */ + + +#ifndef JDEROBOT_CONFIG_HARDCOREDLOCATIONS_H +#define JDEROBOT_CONFIG_HARDCOREDLOCATIONS_H + + +namespace Config{ +const char* HARDCORED_LOCATIONS = +"@CMAKE_INSTALL_PREFIX@/share/jderobot/conf\ +"; + +} //NS +#endif // JDEROBOT_CONFIG_HARDCOREDLOCATIONS_H + diff --git a/src/libs/config_cpp/include/jderobot/config/loader.hpp b/src/libs/config_cpp/include/jderobot/config/loader.hpp new file mode 100644 index 000000000..825f98930 --- /dev/null +++ b/src/libs/config_cpp/include/jderobot/config/loader.hpp @@ -0,0 +1,58 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOT_CONFIG_LOADER_H +#define JDEROBOT_CONFIG_LOADER_H + +#include +#include +#include +#include +#include +#include +#include + + +namespace jderobotconfig { +namespace loader { + + +/** + * @brief Find filename into all defined search paths. + * Order is: + * 1. current dir + * 2. jderobot paths (*) + * + * @return empty if file was not found. + */ +std::string findConfigFile(const std::string& filename); + +/** + * @brief Loads File configuration from passed file. + * + * @return new Config::Config or passed one. + */ +Config::Properties load(std::string filename); + + + +}}//NS + + +#endif // JDEROBOT_CONFIG_LOADER_H diff --git a/src/libs/config_cpp/include/jderobot/config/properties.hpp b/src/libs/config_cpp/include/jderobot/config/properties.hpp new file mode 100644 index 000000000..187c142f5 --- /dev/null +++ b/src/libs/config_cpp/include/jderobot/config/properties.hpp @@ -0,0 +1,149 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOT_CONFIG_CLASS_H +#define JDEROBOT_CONFIG_CLASS_H + +/** + * @mainpage Config + * JdeRobot Config library + * + * @author Aitor Martinez Fernandez , .aitor.martinez.fernandez@gmail.com + * @date September 2017 + * @version 0.9.0 (alpha) + */ + +#include +#include +#include +#include +#include + +namespace Config{ + +class Properties { +public: + Properties(); + Properties(YAML::Node node); + //~Properties(); + + /** + * @brief returns as string the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * + */ + std::string asString(std::string element); + + + /** + * @brief returns as string the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * @param default value + * + */ + std::string asStringWithDefault(std::string element, std::string dataDefault); + + /** + * @brief returns as float the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * + */ + float asFloat(std::string element); + + /** + * @brief returns as float the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * @param default value + * + */ + float asFloatWithDefault(std::string element, float dataDefault); + + /** + * @brief returns as integer the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * + */ + int asInt(std::string element); + + /** + * @brief returns as integer the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * @param default value + * + */ + int asIntWithDefault(std::string element, int dataDefault); + + /** + * @brief returns as double the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * + */ + double asDouble(std::string element); + + /** + * @brief returns as double the propery given + * + * @param route to element separated by dots (example: "kobukiViewer.Camera.proxy") + * @param default value + * + */ + double asDoubleWithDefault(std::string element, double dataDefault); + + + + YAML::Node getNode(); + + +private: + YAML::Node node; + + /** + * @brief makes recursively sear for element given in names + * + * + * @param yaml node in which search + * @param vector of elements names (route to element of last position of vector) + * + * + * @return yaml node of element + */ + YAML::Node searchNode(YAML::Node n, std::vector names); + +}; + + +/** + * @brief function to make printable config class + */ +inline +std::ostream& operator<< (std::ostream & out, Properties & data) { + out << data.getNode(); + return out ; +} + +}//NS + +#endif // JDEROBOT_CONFIG_CLASS_H \ No newline at end of file diff --git a/src/libs/config_cpp/include/jderobot/config/stdutils.hpp b/src/libs/config_cpp/include/jderobot/config/stdutils.hpp new file mode 100644 index 000000000..860a31e78 --- /dev/null +++ b/src/libs/config_cpp/include/jderobot/config/stdutils.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Victor Arribas Raigadas <.varribas.urjc@gmail.com> + */ + +#ifndef STDUTILS_HPP +#define STDUTILS_HPP + +//// string wrapper for stdlib.h::getenv + +#include +#include + +inline +std::string getEnvironmentVariable(std::string var){ + char* _env = getenv(var.c_str()); + return std::string((_env)?_env:""); +} + + +//// Fallback std::split +/// source: http://stackoverflow.com/questions/5607589/right-way-to-split-an-stdstring-into-a-vectorstring + +#include +#include +#include + +namespace std { +inline +vector split(string str, string del){ + vector vstrings; + boost::split(vstrings, str, boost::is_any_of(del)); + return vstrings; +} +}//NS + + + +//// Check if file exists +/// For Linux works for files and directories +/// source: http://www.cplusplus.com/forum/general/1796/ +#include + +namespace std { +inline +bool fileexists(std::string filepath){ + ifstream ifile(filepath.c_str(), ios_base::in); + return ifile.is_open(); +} +}//NS + +#endif // STDUTILS_HPP diff --git a/src/libs/config_cpp/src/demo/demo.cpp b/src/libs/config_cpp/src/demo/demo.cpp new file mode 100644 index 000000000..e673d6169 --- /dev/null +++ b/src/libs/config_cpp/src/demo/demo.cpp @@ -0,0 +1,19 @@ +#include +#include +#include +#include +#include + + +int +main( int argc, char* argv[] ){ + Config::Properties props = Config::load(argc, argv); + std::cout << props << std::endl; + std::cout << props.asString("Demo.Motors.Proxy")<< std::endl; + std::cout << props.asStringWithDefault("Demo.Motors.Proxy2", "Proxy2")<< std::endl; + std::cout << props.asFloat("Demo.Motors.maxW")<< std::endl; + std::cout << props.asInt("Demo.Motors.maxV")<< std::endl; + + + return 0; +} diff --git a/src/libs/config_cpp/src/demo/demo.yml b/src/libs/config_cpp/src/demo/demo.yml new file mode 100644 index 000000000..04a17ada4 --- /dev/null +++ b/src/libs/config_cpp/src/demo/demo.yml @@ -0,0 +1,18 @@ +Demo: + Motors: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Motors:default -h localhost -p 9001" + Topic: "/turtlebotROS/mobile_base/commands/velocity" + Name: basic_component_pyCamera + maxW: 0.7 + maxV: 4 + + Camera: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraL:default -h localhost -p 9001" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + NodeName: demo + diff --git a/src/libs/config_cpp/src/loader.cpp b/src/libs/config_cpp/src/loader.cpp new file mode 100644 index 000000000..c3aeeda36 --- /dev/null +++ b/src/libs/config_cpp/src/loader.cpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 1997-2015 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Victor Arribas Raigadas <.varribas.urjc@gmail.com> + */ + + +#include "jderobot/config/loader.hpp" + +namespace jderobotconfig{ +namespace loader{ + +std::string +findConfigFile(const std::string& filename){ + if (std::fileexists(filename)) + return filename; + + std::string path_holders[] = {Config::HARDCORED_LOCATIONS}; + for (int i=0; i<2; i++){ + if (path_holders[i].empty()) continue; + for (std::string path : std::split(path_holders[i], ":")){ + if (path.empty()) continue; + std::string filepath(path+"/"+filename); + if (std::fileexists(filepath)) + return filepath; + } + } + + return ""; +} + +Config::Properties +load(std::string filename){ + std::string filepath = findConfigFile(filename); + if (filepath.empty()){ + YAML::Exception e(YAML::Mark(),"jderobot/config/loader.cpp: file " + filepath + " Not Found"); + throw e; + } + YAML::Node nodeConfig = YAML::LoadFile(filepath); + + Config::Properties config(nodeConfig); + std::cout<<"[Info] loaded YAML Config file: "<setProperty("Ice.Config", filepath); + return config; +} + + + +}}//NS diff --git a/src/libs/config_cpp/src/properties.cpp b/src/libs/config_cpp/src/properties.cpp new file mode 100644 index 000000000..336592517 --- /dev/null +++ b/src/libs/config_cpp/src/properties.cpp @@ -0,0 +1,141 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#include "jderobot/config/properties.hpp" + +namespace Config{ + + +Properties::Properties(){ +} + +Properties::Properties(YAML::Node node){ + this->node = node; +} + +std::string +Properties::asString(std::string element){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + return nod.as(); +} + +std::string +Properties::asStringWithDefault(std::string element, std::string dataDefault){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + std::string data; + try{ + data = nod.as(); + }catch(YAML::BadConversion e){ + data = dataDefault; + } + return data; +} + +float +Properties::asFloat(std::string element){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + return nod.as(); +} + +float +Properties::asFloatWithDefault(std::string element, float dataDefault){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + float data; + try{ + data = nod.as(); + }catch(YAML::BadConversion e){ + data = dataDefault; + } + return data; +} + +int +Properties::asInt(std::string element){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + return nod.as(); +} + +int +Properties::asIntWithDefault(std::string element, int dataDefault){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + int data; + try{ + data = nod.as(); + }catch(YAML::BadConversion e){ + data = dataDefault; + } + return data; +} + +double +Properties::asDouble(std::string element){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + return nod.as(); +} + +double +Properties::asDoubleWithDefault(std::string element, double dataDefault){ + std::vector v = std::split(element, "."); + + YAML::Node nod = this->searchNode(this->node, v); + double data; + try{ + data = nod.as(); + }catch(YAML::BadConversion e){ + data = dataDefault; + } + return data; +} + +YAML::Node +Properties::getNode(){ + + return node; +} + + + +YAML::Node +Properties::searchNode(YAML::Node n, std::vector names){ + YAML::Node nod = n[names[0]]; + names.erase(names.begin()); + + if (names.size()>0){ + return this->searchNode(nod, names); + }else{ + return nod; + } +} + + +}//NS \ No newline at end of file diff --git a/src/libs/config_cpp/src/tests/test_loader.cpp b/src/libs/config_cpp/src/tests/test_loader.cpp new file mode 100644 index 000000000..2a7b9a9a8 --- /dev/null +++ b/src/libs/config_cpp/src/tests/test_loader.cpp @@ -0,0 +1,33 @@ +/* + * Copyright (C) 1997-2015 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Victor Arribas Raigadas <.varribas.urjc@gmail.com> + * + * Thanks to: + * http://stackoverflow.com/questions/5607589/right-way-to-split-an-stdstring-into-a-vectorstring + * http://www.cplusplus.com/forum/general/1796/ + */ + +#include +#include +#include + + + +int main(int argc, char* argv[]){ + Config::Properties props = Config::load(argv[1]); + std::cout << props << std::endl; +} diff --git a/src/libs/config_cpp/src/tests/test_stdutils.cpp b/src/libs/config_cpp/src/tests/test_stdutils.cpp new file mode 100644 index 000000000..ad2fb63e3 --- /dev/null +++ b/src/libs/config_cpp/src/tests/test_stdutils.cpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 1997-2015 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Victor Arribas Raigadas <.varribas.urjc@gmail.com> + * + * Thanks to: + * http://stackoverflow.com/questions/5607589/right-way-to-split-an-stdstring-into-a-vectorstring + * http://www.cplusplus.com/forum/general/1796/ + */ + + +#include +#include + + +int main(){ + std::string PATH = getEnvironmentVariable("PATH"); + std::vector paths = std::split(PATH, ":"); + std::copy(paths.begin(), paths.end(), std::ostream_iterator(std::cout, "\n")); + + std::cout << std::fileexists("/bin/bash"); + std::cout << std::fileexists("/dev/null"); + std::cout << std::fileexists("/tmp/foo"); + for (std::string path : paths){ + std::cout << std::fileexists(path); + } +} diff --git a/src/libs/config_py/.gitignore b/src/libs/config_py/.gitignore new file mode 100644 index 000000000..2e96e31b7 --- /dev/null +++ b/src/libs/config_py/.gitignore @@ -0,0 +1,108 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*,cover + +# Translations +*.mo +*.pot + +# Django stuff: +*.log + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + + +# https://raw.githubusercontent.com/github/gitignore/master/Global/JetBrains.gitignore +# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm + +*.iml + +## Directory-based project format: +.idea/ +# if you remove the above rule, at least ignore the following: + +# User-specific stuff: +# .idea/workspace.xml +# .idea/tasks.xml +# .idea/dictionaries +# .idea/shelf + +# Sensitive or high-churn files: +# .idea/dataSources.ids +# .idea/dataSources.xml +# .idea/sqlDataSources.xml +# .idea/dynamic.xml +# .idea/uiDesigner.xml + +# Gradle: +# .idea/gradle.xml +# .idea/libraries + +# Mongo Explorer plugin: +# .idea/mongoSettings.xml + +## File-based project format: +*.ipr +*.iws + +## Plugin-specific files: + +# IntelliJ +/out/ + +# mpeltonen/sbt-idea plugin +.idea_modules/ + +# JIRA plugin +atlassian-ide-plugin.xml + +# Crashlytics plugin (for Android Studio and IntelliJ) +com_crashlytics_export_strings.xml +crashlytics.properties +crashlytics-build.properties +fabric.properties diff --git a/src/libs/config_py/CMakeLists.txt b/src/libs/config_py/CMakeLists.txt new file mode 100644 index 000000000..d104cf238 --- /dev/null +++ b/src/libs/config_py/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8) + +configure_module_python(config) + +add_custom_target(config_py ALL) +copy_to_binary_python(config_py config) + +install_python(config config-python) diff --git a/src/libs/config_py/config/__init__.py b/src/libs/config_py/config/__init__.py new file mode 100644 index 000000000..bf0e04445 --- /dev/null +++ b/src/libs/config_py/config/__init__.py @@ -0,0 +1,11 @@ +__author__ = 'aitormf' + +## import simplification +# empty __init__.py: +# from jderobotconfig import config as config +# new option: +# import jderobotconfig as config +from .config import * + +from .properties import Properties + diff --git a/src/libs/config_py/config/config.py b/src/libs/config_py/config/config.py new file mode 100644 index 000000000..7c04ca59a --- /dev/null +++ b/src/libs/config_py/config/config.py @@ -0,0 +1,73 @@ +# +# Copyright (C) 1997-2017 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# +__author__ = 'aitormf' + +import sys, os +import yaml +from .hardcodedpaths import HARDCODED_PATHS +from .properties import Properties + + +def findConfigFile(filename): + ''' + Returns filePath or None if it couldn't find the file + + @param filename: Name of the file + + @type filename: String + + @return String with path or None + + ''' + paths = "." + if HARDCODED_PATHS: + paths = paths+":"+HARDCODED_PATHS + + for path in paths.split(":"): + file_path = os.path.join(path, filename) + if os.path.exists(file_path): + return file_path + + return None + + +def load(filename): + ''' + Returns the configuration as dict + + @param filename: Name of the file + + @type filename: String + + @return a dict with propierties reader from file + + ''' + filepath = findConfigFile(filename) + prop= None + if (filepath): + print ('loading Config file %s' %(filepath)) + + with open(filepath, 'r') as stream: + cfg=yaml.load(stream) + prop = Properties(cfg) + else: + msg = "Ice.Config file '%s' could not being found" % (filename) + raise ValueError(msg) + + return prop \ No newline at end of file diff --git a/src/libs/config_py/config/hardcodedpaths.py.in b/src/libs/config_py/config/hardcodedpaths.py.in new file mode 100644 index 000000000..2bf148bb6 --- /dev/null +++ b/src/libs/config_py/config/hardcodedpaths.py.in @@ -0,0 +1,49 @@ +# +# Copyright (C) 1997-2017 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Victor Arribas Raigadas +# +__author__ = 'varribas' + +import os +import platform + +if ('Windows')==platform.system(): + JDEROBOT_PATHS = "" + JDEROBOT_GAZEBO_PLUGINS_PATHS = "" +else: + # JdeRobot main Ice.Config path + JDEROBOT_PATHS = "@CMAKE_INSTALL_PREFIX@/share/jderobot/conf" + + + # JdeRobot Gazebo's plugins Ice.Config path + JDEROBOT_GAZEBO_PLUGINS_BASE_PATH = '@CMAKE_INSTALL_PREFIX@/share/jderobot/gazebo/plugins' + + if os.path.exists(JDEROBOT_GAZEBO_PLUGINS_BASE_PATH): + + gazebo_plugins = list() + for dir in os.listdir(JDEROBOT_GAZEBO_PLUGINS_BASE_PATH): + plugin_dir = os.path.join(JDEROBOT_GAZEBO_PLUGINS_BASE_PATH, dir) + gazebo_plugins.append(plugin_dir) + + JDEROBOT_GAZEBO_PLUGINS_PATHS = ':'.join(str(x) for x in gazebo_plugins) + + else: + JDEROBOT_GAZEBO_PLUGINS_PATHS = "" + + +# "Hardcoded" PATHS +HARDCODED_PATHS = "%s:%s" %(JDEROBOT_PATHS, JDEROBOT_GAZEBO_PLUGINS_PATHS) diff --git a/src/libs/config_py/config/properties.py b/src/libs/config_py/config/properties.py new file mode 100644 index 000000000..dc7bfe28b --- /dev/null +++ b/src/libs/config_py/config/properties.py @@ -0,0 +1,57 @@ +# +# Copyright (C) 1997-2017 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# +__author__ = 'aitormf' + +import yaml + +class Properties: + def __init__(self, cfg): + self._config = cfg + + def getNode(self): + return self._config + + def getProperty(self, name): + + names = name.split(".") + + return self._searchNode(self._config, names) + + def getPropertyWithDefault(self, name, dataDefault): + + try: + return self.getProperty(name) + + except KeyError: + return dataDefault + + + def _searchNode(self, node, lst): + name = lst.pop(0) + + nod = node[name] + + if (len(lst) > 0): + return (self._searchNode(nod, lst)) + else: + return nod + + def __str__(self): + return yaml.dump(self._config) + diff --git a/src/libs/config_py/demo/demo.py b/src/libs/config_py/demo/demo.py new file mode 100644 index 000000000..a0700ac91 --- /dev/null +++ b/src/libs/config_py/demo/demo.py @@ -0,0 +1,30 @@ +# +# Copyright (C) 1997-2015 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# +__author__ = 'aitormf' + +import sys + +import config + +cfg = config.load(sys.argv[1]) + +print (cfg.getProperty("Demo.Motors.Server")) +print (cfg.getPropertyWithDefault("Demo.Motors.Server2", "Server2")) +print cfg + diff --git a/src/libs/config_py/demo/demo.yml b/src/libs/config_py/demo/demo.yml new file mode 100644 index 000000000..abc5b1518 --- /dev/null +++ b/src/libs/config_py/demo/demo.yml @@ -0,0 +1,18 @@ +Demo: + Motors: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: Motors:default -h localhost -p 9001 + Topic: '/turtlebotROS/mobile_base/commands/velocity' + Name: basic_component_pyCamera + maxW: 0.7 + maxV: 4 + + Camera: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraL:default -h localhost -p 9001" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + NodeName: demo + diff --git a/src/libs/jderobotcomm_py/jderobotComm/__init__.py b/src/libs/jderobotcomm_py/jderobotComm/__init__.py deleted file mode 100644 index fce5fb55b..000000000 --- a/src/libs/jderobotcomm_py/jderobotComm/__init__.py +++ /dev/null @@ -1,56 +0,0 @@ -import Ice -import rospy - -from .laserClient import getLaserClient -from .cameraClient import getCameraClient -from .pose3dClient import getPose3dClient -from .motorsClient import getMotorsClient - - -def init (ic): - ''' - Starts ROS Node if it is necessary. - - @param ic: Ice Communicator - - @type ic: Ice Communicator - ''' - node = None - rosserver = False - - prop = ic.getProperties() - nodeName = prop.getPropertyWithDefault("NodeName", "JdeRobot") - - l = prop.getPropertiesForPrefix("") - keys = l.keys() - for i in keys: - if (i.endswith(".Server") and l[i] == "2"): - rosserver = True - - if (rosserver): - node = rospy.init_node(nodeName, anonymous=True) - - return ic,node - -def destroy(ic=None, node=None): - ''' - Destroys ROS Node and Ice Communicator if it is necessary. - - @param ic: Ice Communicator - @param node: ROS Node - - @type ic: Ice Communicator - @type node: ROS Node - ''' - if node: - rospy.signal_shutdown("Node Closed") - if ic: - ic.destroy() - - - - - - - - diff --git a/src/libs/jderobotcomm_py/test.cfg b/src/libs/jderobotcomm_py/test.cfg deleted file mode 100644 index b14bdd63f..000000000 --- a/src/libs/jderobotcomm_py/test.cfg +++ /dev/null @@ -1,36 +0,0 @@ - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Motors.Server=2 -kobukiViewer.Motors.Proxy=Motors:tcp -h localhost -p 9001 -kobukiViewer.Motors.Topic=/cmd_vel_mux/input/teleop -kobukiViewer.Motors.Name=kobukiViewerMotors - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Pose3D.Server=2 -kobukiViewer.Pose3D.Proxy=Pose3D:tcp -h localhost -p 9001 -kobukiViewer.Pose3D.Topic=/odom -kobukiViewer.Pose3D.Name=kobukiViewerPose3d - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Camera1.Server=2 -kobukiViewer.Camera1.Proxy=CameraL:tcp -h localhost -p 9001 -kobukiViewer.Camera1.Format=RGB8 -kobukiViewer.Camera1.Topic=/camera/rgb/image_raw -kobukiViewer.Camera1.Name=kobukiViewerCamera1 - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Camera2.Server=1 -kobukiViewer.Camera2.Proxy=CameraR:tcp -h localhost -p 9001 -kobukiViewer.Camera2.Format=RGB8 -kobukiViewer.Camera2.Topic=/scan -kobukiViewer.Camera2.Name=kobukiViewerCamera2 - - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Laser.Server=2 -kobukiViewer.Laser.Proxy=Laser:tcp -h localhost -p 9001 -kobukiViewer.Laser.Topic=/scan -kobukiViewer.Laser.Name=kobukiViewerLaser - -kobukiViewer.Vmax=3 -kobukiViewer.Wmax=0.7 \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/testCamera.py b/src/libs/jderobotcomm_py/testCamera.py deleted file mode 100755 index 75dc99e00..000000000 --- a/src/libs/jderobotcomm_py/testCamera.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python3 -import easyiceconfig as EasyIce -import jderobotComm as comm -import sys -import time -import signal - -from jderobotTypes import LaserData - - - - -if __name__ == '__main__': - - ic = EasyIce.initialize(sys.argv) - ic, node = comm.init(ic) - - client = comm.getCameraClient(ic, "kobukiViewer.Camera1") - - for i in range (10): - #print("client1", end=":") - image = client.getImage() - print(image) - time.sleep(1) - - client.stop() - - - comm.destroy(ic, node) - - - - - - - - - diff --git a/src/libs/jderobotcomm_py/testMotors.py b/src/libs/jderobotcomm_py/testMotors.py deleted file mode 100644 index ae5107009..000000000 --- a/src/libs/jderobotcomm_py/testMotors.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python3 -import easyiceconfig as EasyIce -import jderobotComm as comm -import sys -import time -import signal - -from jderobotTypes import CMDVel - - - - -if __name__ == '__main__': - - ic = EasyIce.initialize(sys.argv) - ic, node = comm.init(ic) - - vel = CMDVel() - vel.vx = 1 - vel.az = 0.1 - - client = comm.getMotorsClient(ic, "kobukiViewer.Motors") - #client2 = comm.getLaserClient(ic, "kobukiViewer.Laser") - for i in range (10): - client.sendVelocities(vel) - time.sleep(1) - - comm.destroy(ic, node) \ No newline at end of file diff --git a/src/samples/basic_component_py/CMakeLists.txt b/src/samples/basic_component_py/CMakeLists.txt index c43a55d60..8b9b5dd2c 100644 --- a/src/samples/basic_component_py/CMakeLists.txt +++ b/src/samples/basic_component_py/CMakeLists.txt @@ -25,4 +25,4 @@ INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/gui DESTINATION share/jderobot/py # Install resources #INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/resources DESTINATION share/jderobot/python/basic_component_py COMPONENT tools PATTERN .svn EXCLUDE) -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/basic_component_py.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/basic_component_py.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf) diff --git a/src/samples/basic_component_py/basic_component.py b/src/samples/basic_component_py/basic_component.py index 9057c4b1e..069d13065 100755 --- a/src/samples/basic_component_py/basic_component.py +++ b/src/samples/basic_component_py/basic_component.py @@ -21,25 +21,25 @@ import sys -import jderobotComm as comm +import comm from gui.threadGUI import ThreadGUI from gui.GUI import MainWindow from PyQt5.QtWidgets import QApplication -import easyiceconfig as EasyIce +import config import signal signal.signal(signal.SIGINT, signal.SIG_DFL) if __name__ == '__main__': - ic = EasyIce.initialize(sys.argv) + cfg = config.load(sys.argv[1]) #starting comm - ic, node = comm.init(ic) + jdrc= comm.init(cfg, 'basic_component') - camera = comm.getCameraClient(ic,"basic_component.Camera") - motors = comm.getMotorsClient(ic,"basic_component.Motors") + camera = jdrc.getCameraClient("basic_component.Camera") + motors = jdrc.getMotorsClient("basic_component.Motors") app = QApplication(sys.argv) frame = MainWindow() diff --git a/src/samples/basic_component_py/basic_component_py.cfg b/src/samples/basic_component_py/basic_component_py.cfg deleted file mode 100644 index 262f8d9cb..000000000 --- a/src/samples/basic_component_py/basic_component_py.cfg +++ /dev/null @@ -1,16 +0,0 @@ -############ Motors ####################### -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -basic_component.Motors.Server=1 -basic_component.Motors.Proxy=Motors:default -h localhost -p 8999 -basic_component.Motors.Topic=/cmd_vel_mux/input/teleop -basic_component.Motors.Name=FolowLineMotors - - -############ Camera ####################### -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -basic_component.Camera.Server=1 -basic_component.Camera.Proxy=cam_turtlebot_right:default -h localhost -p 8994 -basic_component.Camera.Format=RGB8 -basic_component.Camera.Topic=/camera/rgb/image_raw -basic_component.Camera.Name=basic_component_pyCamera - diff --git a/src/samples/basic_component_py/basic_component_py.yml b/src/samples/basic_component_py/basic_component_py.yml new file mode 100644 index 000000000..f2b282125 --- /dev/null +++ b/src/samples/basic_component_py/basic_component_py.yml @@ -0,0 +1,17 @@ +basic_component: + Motors: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Motors:default -h localhost -p 9001" + Topic: "/turtlebotROS/mobile_base/commands/velocity" + Name: basic_component_pyCamera + maxW: 0.7 + maxV: 4 + + Camera: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraL:default -h localhost -p 9001" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + NodeName: basic_component_py diff --git a/src/tools/cameraview/CMakeLists.txt b/src/tools/cameraview/CMakeLists.txt index 10eec4c09..c848705eb 100644 --- a/src/tools/cameraview/CMakeLists.txt +++ b/src/tools/cameraview/CMakeLists.txt @@ -11,15 +11,15 @@ include_directories( ${gtkglextmm_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS} ${resourcelocator_INCLUDE_DIRS} - ${easyiceconfig_INCLUDE_DIRS} ${jderobottypes_INCLUDE_DIRS} - ${jderobotcomm_INCLUDE_DIRS} + ${comm_INCLUDE_DIRS} + ${config_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ) link_directories( ${resourcelocator_LIBRARY_DIRS} - ${easyiceconfig_LIBRARY_DIRS} - ${jderobotcomm_LIBRARY_DIRS} + ${comm_LIBRARY_DIRS} + ${config_LIBRARY_DIRS} ) add_executable (cameraview ${SOURCE_FILES}) @@ -31,13 +31,13 @@ TARGET_LINK_LIBRARIES(cameraview ${gtkmm_LIBRARIES} ${libglademm_LIBRARIES} colorspacesmm - ${easyiceconfig_LIBRARIES} ${ZeroCIce_LIBRARIES} ${ZLIB_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${resourcelocator_LIBRARIES} ${catkin_LIBRARIES} - ${jderobotcomm_LIBRARIES} + ${comm_LIBRARIES} + ${config_LIBRARIES} ${GLOG_LIBRARIES} JderobotInterfaces jderobotutil @@ -48,5 +48,6 @@ install(TARGETS cameraview COMPONENT cameraview ) + INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/cameraview.glade DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/glade COMPONENT cameraview) -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/cameraview.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT cameraview) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/cameraview.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT cameraview) diff --git a/src/tools/cameraview/cameraview.cfg b/src/tools/cameraview/cameraview.cfg deleted file mode 100644 index 3dbc09deb..000000000 --- a/src/tools/cameraview/cameraview.cfg +++ /dev/null @@ -1,6 +0,0 @@ -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -Cameraview.Camera.Server=1 -Cameraview.Camera.Proxy=cameraA:tcp -h localhost -p 9999 -Cameraview.Camera.Format=RGB8 -Cameraview.Camera.Topic=/camera/rgb/image_raw -Cameraview.Camera.Name=cameraA \ No newline at end of file diff --git a/src/tools/cameraview/cameraview.cpp b/src/tools/cameraview/cameraview.cpp index 61484afd7..7cd807bf3 100644 --- a/src/tools/cameraview/cameraview.cpp +++ b/src/tools/cameraview/cameraview.cpp @@ -24,20 +24,22 @@ #include #include #include "viewer.h" -#include "easyiceconfig/EasyIce.h" +#include +#include #include #include int main(int argc, char** argv){ cameraview::Viewer viewer; - Ice::CommunicatorPtr ic; + - JdeRobotComm::CameraClient* camRGB; + Comm::CameraClient* camRGB; - ic = EasyIce::initialize(argc,argv); + Config::Properties cfg = Config::load(argc, argv); + Comm::Communicator* jdrc = new Comm::Communicator(cfg); - camRGB = JdeRobotComm::getCameraClient(ic, "Cameraview.Camera"); + camRGB = Comm::getCameraClient(jdrc, "Cameraview.Camera"); JdeRobotTypes::Image rgb; @@ -49,5 +51,7 @@ int main(int argc, char** argv){ viewer.displayFrameRate(0); } + delete jdrc; + return 0; } diff --git a/src/tools/cameraview/cameraview.yml b/src/tools/cameraview/cameraview.yml new file mode 100644 index 000000000..794c83f25 --- /dev/null +++ b/src/tools/cameraview/cameraview.yml @@ -0,0 +1,10 @@ +Cameraview: + Camera: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraA:tcp -h localhost -p 9999" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: cameraA + Fps: 30 + + NodeName: cameraview \ No newline at end of file diff --git a/src/tools/colorTuner_py/CMakeLists.txt b/src/tools/colorTuner_py/CMakeLists.txt index 7fa4c50ad..ff2d0eb24 100644 --- a/src/tools/colorTuner_py/CMakeLists.txt +++ b/src/tools/colorTuner_py/CMakeLists.txt @@ -9,19 +9,21 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/colorTuner PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ DESTINATION bin + COMPONENT colortuner-python ) INSTALL(FILES colorTuner.py DESTINATION share/jderobot/python/colorTuner_py/ COMPONENT tools) -INSTALL(FILES resources_rc.py DESTINATION share/jderobot/python/colorTuner_py/ COMPONENT tools) +INSTALL(FILES resources_rc.py DESTINATION share/jderobot/python/colorTuner_py/ COMPONENT colortuner-python) # Install gui -INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/gui DESTINATION share/jderobot/python/colorTuner_py COMPONENT tools PATTERN .svn EXCLUDE) +INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/gui DESTINATION share/jderobot/python/colorTuner_py COMPONENT colortuner-python PATTERN .svn EXCLUDE) # Install sensors -INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/sensors DESTINATION share/jderobot/python/colorTuner_py COMPONENT tools PATTERN .svn EXCLUDE) +INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/sensors DESTINATION share/jderobot/python/colorTuner_py COMPONENT colortuner-python PATTERN .svn EXCLUDE) # Install Filters -INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/filters DESTINATION share/jderobot/python/colorTuner_py COMPONENT tools PATTERN .svn EXCLUDE) +INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/filters DESTINATION share/jderobot/python/colorTuner_py COMPONENT colortuner-python PATTERN .svn EXCLUDE) -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/colorTuner_py.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/colorTuner_py.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf +COMPONENT colortuner-python) diff --git a/src/tools/kobukiViewer/CMakeLists.txt b/src/tools/kobukiViewer/CMakeLists.txt index f56f54f3b..67fa587e0 100644 --- a/src/tools/kobukiViewer/CMakeLists.txt +++ b/src/tools/kobukiViewer/CMakeLists.txt @@ -34,16 +34,16 @@ if (${QT5_COMPILE} AND ${roscpp_FOUND}) ${INTERFACES_CPP_DIR} ${LIBS_DIR}/ ${CMAKE_CURRENT_SOURCE_DIR} - ${easyiceconfig_INCLUDE_DIRS} + ${config_INCLUDE_DIRS} ${jderobottypes_INCLUDE_DIRS} - ${jderobotcomm_INCLUDE_DIRS} + ${comm_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS} ) link_directories(${JDE_LIBS} ${easyiceconfig_LIBRARY_DIRS} - ${jderobotcomm_LIBRARY_DIRS} + ${comm_LIBRARY_DIRS} ) add_executable( kobukiViewer ${SOURCE} ${INTERFACES_CPP_DIR}) @@ -55,8 +55,8 @@ if (${QT5_COMPILE} AND ${roscpp_FOUND}) ${Qt5OpenGL_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${OpenCV_LIBRARIES} - ${easyiceconfig_LIBRARIES} - ${jderobotcomm_LIBRARIES} + ${config_LIBRARIES} + ${comm_LIBRARIES} ${ZeroCIce_LIBRARIES} JderobotInterfaces jderobotutil @@ -69,5 +69,6 @@ if (${QT5_COMPILE} AND ${roscpp_FOUND}) DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ COMPONENT kobukiviewer ) - INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/kobukiViewer.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT kobukiviewer) + + INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/kobukiViewer.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT kobukiviewer) ENDIF() diff --git a/src/tools/kobukiViewer/gui/gui.cpp b/src/tools/kobukiViewer/gui/gui.cpp index 36d522c30..537885160 100644 --- a/src/tools/kobukiViewer/gui/gui.cpp +++ b/src/tools/kobukiViewer/gui/gui.cpp @@ -1,6 +1,6 @@ #include "gui.h" -GUI::GUI(Robot* robot, Ice::CommunicatorPtr ic) +GUI::GUI(Robot* robot, Config::Properties props) { this->robot = robot; @@ -22,7 +22,7 @@ GUI::GUI(Robot* robot, Ice::CommunicatorPtr ic) currentW = new QLabel("0"); canvasVW = new controlVW(); - canvasVW->setIC(ic); + canvasVW->setProps(props); laserWidget =new LaserWidget(); diff --git a/src/tools/kobukiViewer/gui/gui.h b/src/tools/kobukiViewer/gui/gui.h index 6b1d9203f..0b37a0789 100644 --- a/src/tools/kobukiViewer/gui/gui.h +++ b/src/tools/kobukiViewer/gui/gui.h @@ -5,6 +5,8 @@ #include "../robot/robot.h" +#include + #include "widget/controlvw.h" #include "widget/cameraswidget.h" #include "widget/laserwidget.h" @@ -14,7 +16,7 @@ class GUI:public QWidget Q_OBJECT public: - GUI(Robot* robot, Ice::CommunicatorPtr ic); + GUI(Robot* robot, Config::Properties props); void updateThreadGUI(); private: diff --git a/src/tools/kobukiViewer/gui/threadupdategui.cpp b/src/tools/kobukiViewer/gui/threadupdategui.cpp index 566a167aa..0167829a7 100644 --- a/src/tools/kobukiViewer/gui/threadupdategui.cpp +++ b/src/tools/kobukiViewer/gui/threadupdategui.cpp @@ -1,10 +1,10 @@ #include "threadupdategui.h" -ThreadUpdateGUI::ThreadUpdateGUI(Robot* robot, Ice::CommunicatorPtr ic) +ThreadUpdateGUI::ThreadUpdateGUI(Robot* robot, Config::Properties props) { this->robot = robot; - gui = new GUI(robot, ic); + gui = new GUI(robot, props); gui->show(); } diff --git a/src/tools/kobukiViewer/gui/threadupdategui.h b/src/tools/kobukiViewer/gui/threadupdategui.h index cb93512a3..d0d781f62 100644 --- a/src/tools/kobukiViewer/gui/threadupdategui.h +++ b/src/tools/kobukiViewer/gui/threadupdategui.h @@ -9,12 +9,14 @@ #include #include +#include + #define cycle_update_gui 50 //miliseconds class ThreadUpdateGUI: public QThread { public: - ThreadUpdateGUI(Robot *robot, Ice::CommunicatorPtr ic); + ThreadUpdateGUI(Robot *robot, Config::Properties props); private: GUI* gui; diff --git a/src/tools/kobukiViewer/gui/widget/controlvw.cpp b/src/tools/kobukiViewer/gui/widget/controlvw.cpp index 02e6c6f47..2e6500c2b 100644 --- a/src/tools/kobukiViewer/gui/widget/controlvw.cpp +++ b/src/tools/kobukiViewer/gui/widget/controlvw.cpp @@ -27,13 +27,12 @@ controlVW::controlVW(QWidget *parent) : } -void controlVW::setIC(Ice::CommunicatorPtr ic) +void controlVW::setProps(Config::Properties props) { - this->ic = ic; - Ice::PropertiesPtr prop = ic->getProperties(); + this->props = props; - QString svmax = QString::fromUtf8(prop->getPropertyWithDefault("kobukiViewer.Vmax", "5").c_str()); - QString swmax = QString::fromUtf8(prop->getPropertyWithDefault("kobukiViewer.Wmax", "0.5").c_str()); + QString svmax = QString::fromUtf8(props.asStringWithDefault("kobukiViewer.Vmax", "5").c_str()); + QString swmax = QString::fromUtf8(props.asStringWithDefault("kobukiViewer.Wmax", "0.5").c_str()); this->v_max = svmax.toFloat(); this->w_max = swmax.toFloat(); diff --git a/src/tools/kobukiViewer/gui/widget/controlvw.h b/src/tools/kobukiViewer/gui/widget/controlvw.h index 1d773857d..504ab899f 100644 --- a/src/tools/kobukiViewer/gui/widget/controlvw.h +++ b/src/tools/kobukiViewer/gui/widget/controlvw.h @@ -6,7 +6,7 @@ #include #include -#include "easyiceconfig/EasyIce.h" +#include class controlVW : public QWidget { @@ -15,7 +15,7 @@ class controlVW : public QWidget explicit controlVW(QWidget *parent = 0); void Stop(); - void setIC(Ice::CommunicatorPtr ic); + void setProps(Config::Properties props); float getV(); float getW(); @@ -24,7 +24,7 @@ class controlVW : public QWidget QImage qimage; float v, w, v_max, w_max; - Ice::CommunicatorPtr ic; + Config::Properties props; protected: void paintEvent(QPaintEvent *); diff --git a/src/tools/kobukiViewer/kobukiViewer.cfg b/src/tools/kobukiViewer/kobukiViewer.cfg deleted file mode 100644 index db28984e3..000000000 --- a/src/tools/kobukiViewer/kobukiViewer.cfg +++ /dev/null @@ -1,36 +0,0 @@ - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Motors.Server=1 -kobukiViewer.Motors.Proxy=Motors:tcp -h localhost -p 9001 -kobukiViewer.Motors.Topic=/turtlebotROS/mobile_base/commands/velocity -kobukiViewer.Motors.Name=kobukiViewerMotors - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Pose3D.Server=1 -kobukiViewer.Pose3D.Proxy=Pose3D:tcp -h localhost -p 9001 -kobukiViewer.Pose3D.Topic=/turtlebotROS/odom -kobukiViewer.Pose3D.Name=kobukiViewerPose3d - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Camera1.Server=1 -kobukiViewer.Camera1.Proxy=CameraL:tcp -h localhost -p 9001 -kobukiViewer.Camera1.Format=RGB8 -kobukiViewer.Camera1.Topic=/TurtlebotROS/cameraL/image_raw -kobukiViewer.Camera1.Name=kobukiViewerCamera1 - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Camera2.Server=1 -kobukiViewer.Camera2.Proxy=CameraR:tcp -h localhost -p 9001 -kobukiViewer.Camera2.Format=RGB8 -kobukiViewer.Camera2.Topic=/TurtlebotROS/cameraR/image_raw -kobukiViewer.Camera2.Name=kobukiViewerCamera2 - - -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -kobukiViewer.Laser.Server=1 -kobukiViewer.Laser.Proxy=Laser:tcp -h localhost -p 9001 -kobukiViewer.Laser.Topic=/turtlebotROS/laser/scan -kobukiViewer.Laser.Name=kobukiViewerLaser - -kobukiViewer.Vmax=3 -kobukiViewer.Wmax=0.7 diff --git a/src/tools/kobukiViewer/kobukiViewer.yml b/src/tools/kobukiViewer/kobukiViewer.yml new file mode 100644 index 000000000..c571b1f7f --- /dev/null +++ b/src/tools/kobukiViewer/kobukiViewer.yml @@ -0,0 +1,38 @@ +kobukiViewer: + Motors: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Motors:default -h localhost -p 9001" + Topic: "/cmd_vel_mux/input/teleop" + Name: testMotors + maxV: 3 + maxW: 0.7 + + Camera1: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraL:default -h localhost -p 9001" + Format: RGB8 + Topic: "/camera/rgb/image_raw" + Name: testCamera1 + + Camera2: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CameraR:default -h localhost -p 9001" + Format: RGB8 + Topic: "/camera/rgb/image_raw" + Name: testCamera2 + + Pose3D: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Pose3D:default -h localhost -p 9001" + Topic: "/odom" + Name: testPose3d + + Laser: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Laser:default -h localhost -p 9001" + Topic: "/scan" + Name: testLaser + + Vmax: 3 + Wmax: 0.7 + NodeName: kobukiViewer \ No newline at end of file diff --git a/src/tools/kobukiViewer/main.cpp b/src/tools/kobukiViewer/main.cpp index dbccecc33..53966ca96 100644 --- a/src/tools/kobukiViewer/main.cpp +++ b/src/tools/kobukiViewer/main.cpp @@ -1,24 +1,29 @@ #include "robot/robot.h" #include "gui/threadupdategui.h" -#include "easyiceconfig/EasyIce.h" +#include "jderobot/config/config.h" +#include "jderobot/comm/communicator.hpp" int main(int argc, char *argv[]) { QApplication app(argc, argv); - Ice::CommunicatorPtr ic; try { - //-----------------ICE----------------// - ic = EasyIce::initialize(argc, argv); + + Config::Properties props = Config::load(argc, argv); + + + + //-----------------Comm----------------// + Comm::Communicator* jdrc = new Comm::Communicator(props); // Variables Compartidas // Robot -> Sensores, navegacion, actuadores - Robot *robot = new Robot(ic); + Robot *robot = new Robot(jdrc); - ThreadUpdateGUI* thread_update_gui = new ThreadUpdateGUI(robot, ic); + ThreadUpdateGUI* thread_update_gui = new ThreadUpdateGUI(robot, props); thread_update_gui->start(); } catch (const Ice::Exception& ex) { diff --git a/src/tools/kobukiViewer/robot/actuators.cpp b/src/tools/kobukiViewer/robot/actuators.cpp index 1db51cc87..8af861fef 100755 --- a/src/tools/kobukiViewer/robot/actuators.cpp +++ b/src/tools/kobukiViewer/robot/actuators.cpp @@ -1,10 +1,10 @@ #include "actuators.h" -Actuators::Actuators(Ice::CommunicatorPtr ic) +Actuators::Actuators(Comm::Communicator* jdrc) { - this->ic = ic; + this->jdrc = jdrc; - this->motorsClient = JdeRobotComm::getMotorsClient(ic, "kobukiViewer.Motors"); + this->motorsClient = Comm::getMotorsClient(jdrc, "kobukiViewer.Motors"); } diff --git a/src/tools/kobukiViewer/robot/actuators.h b/src/tools/kobukiViewer/robot/actuators.h index 83e804c94..33097df4e 100755 --- a/src/tools/kobukiViewer/robot/actuators.h +++ b/src/tools/kobukiViewer/robot/actuators.h @@ -7,12 +7,13 @@ #include #include +#include "jderobot/comm/communicator.hpp" #include class Actuators { public: - Actuators(Ice::CommunicatorPtr ic); + Actuators(Comm::Communicator* jdrc); float getMotorV(); float getMotorW(); @@ -28,10 +29,10 @@ class Actuators QMutex mutex; - Ice::CommunicatorPtr ic; + Comm::Communicator* jdrc; // ICE INTERFACES - JdeRobotComm::MotorsClient* motorsClient; + Comm::MotorsClient* motorsClient; }; #endif // ACTUATORS_H diff --git a/src/tools/kobukiViewer/robot/robot.cpp b/src/tools/kobukiViewer/robot/robot.cpp index bb699925e..4dd61356a 100644 --- a/src/tools/kobukiViewer/robot/robot.cpp +++ b/src/tools/kobukiViewer/robot/robot.cpp @@ -1,9 +1,9 @@ #include "robot.h" -Robot::Robot(Ice::CommunicatorPtr ic) +Robot::Robot(Comm::Communicator* jdrc) { - sensors = new Sensors(ic); - actuators = new Actuators(ic); + sensors = new Sensors(jdrc); + actuators = new Actuators(jdrc); pthread_mutex_init (&mutex, NULL); diff --git a/src/tools/kobukiViewer/robot/robot.h b/src/tools/kobukiViewer/robot/robot.h index f079e153e..76375882a 100644 --- a/src/tools/kobukiViewer/robot/robot.h +++ b/src/tools/kobukiViewer/robot/robot.h @@ -6,6 +6,8 @@ #include "sensors.h" #include "actuators.h" +#include "jderobot/comm/communicator.hpp" + #include class Robot: public QObject @@ -13,7 +15,7 @@ class Robot: public QObject Q_OBJECT public: - Robot(Ice::CommunicatorPtr ic); + Robot(Comm::Communicator* jdrc); void update(); diff --git a/src/tools/kobukiViewer/robot/sensors.cpp b/src/tools/kobukiViewer/robot/sensors.cpp index 030a54c04..99911950a 100755 --- a/src/tools/kobukiViewer/robot/sensors.cpp +++ b/src/tools/kobukiViewer/robot/sensors.cpp @@ -1,24 +1,24 @@ #include "sensors.h" -Sensors::Sensors(Ice::CommunicatorPtr ic) +Sensors::Sensors(Comm::Communicator* jdrc) { - this-> ic = ic; + this-> jdrc = jdrc; - this->poseClient = JdeRobotComm::getPose3dClient(ic, "kobukiViewer.Pose3D"); + this->poseClient = Comm::getPose3dClient(jdrc, "kobukiViewer.Pose3D"); ////////////////////////////// CAMERA1 ///////////////////////////// - this->camera1 = JdeRobotComm::getCameraClient(ic, "kobukiViewer.Camera1"); + this->camera1 = Comm::getCameraClient(jdrc, "kobukiViewer.Camera1"); ////////////////////////////// CAMERA2 ///////////////////////////// - this->camera2 = JdeRobotComm::getCameraClient(ic, "kobukiViewer.Camera2"); + this->camera2 = Comm::getCameraClient(jdrc, "kobukiViewer.Camera2"); ////////////////////////////// LASER ////////////////////////////// // Contact to LASER interface - this->laserClient = JdeRobotComm::getLaserClient(ic, "kobukiViewer.Laser"); + this->laserClient = Comm::getLaserClient(jdrc, "kobukiViewer.Laser"); } JdeRobotTypes::Image Sensors::getImage1() diff --git a/src/tools/kobukiViewer/robot/sensors.h b/src/tools/kobukiViewer/robot/sensors.h index 98f150e9c..c5b1782fc 100755 --- a/src/tools/kobukiViewer/robot/sensors.h +++ b/src/tools/kobukiViewer/robot/sensors.h @@ -13,6 +13,7 @@ #include #include +#include "jderobot/comm/communicator.hpp" #include #include #include @@ -20,7 +21,7 @@ class Sensors { public: - Sensors(Ice::CommunicatorPtr ic); + Sensors(Comm::Communicator* jdrc); JdeRobotTypes::Pose3d getPose(); JdeRobotTypes::LaserData getLaserData(); @@ -31,15 +32,15 @@ class Sensors private: - Ice::CommunicatorPtr ic; + Comm::Communicator* jdrc; //LASER DATA - JdeRobotComm::LaserClient* laserClient; + Comm::LaserClient* laserClient; - JdeRobotComm::CameraClient* camera1; - JdeRobotComm::CameraClient* camera2; + Comm::CameraClient* camera1; + Comm::CameraClient* camera2; - JdeRobotComm::Pose3dClient* poseClient; + Comm::Pose3dClient* poseClient; diff --git a/src/tools/pantilt_teleop_py/CMakeLists.txt b/src/tools/pantilt_teleop_py/CMakeLists.txt index a038269e8..d41d43364 100644 --- a/src/tools/pantilt_teleop_py/CMakeLists.txt +++ b/src/tools/pantilt_teleop_py/CMakeLists.txt @@ -26,4 +26,4 @@ INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/gui DESTINATION share/jderobot/py # Install resources #INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/resources DESTINATION share/jderobot/python/pantilt_teleop_py COMPONENT panTiltTeleop-python PATTERN .svn EXCLUDE) -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/pantilt_teleop.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT panTiltTeleop-python) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/pantilt_teleop.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT panTiltTeleop-python) diff --git a/src/tools/pantilt_teleop_py/pantilt_teleop.cfg b/src/tools/pantilt_teleop_py/pantilt_teleop.cfg deleted file mode 100644 index 93053541f..000000000 --- a/src/tools/pantilt_teleop_py/pantilt_teleop.cfg +++ /dev/null @@ -1,10 +0,0 @@ -PanTiltTeleop.PTMotors.Proxy = PanTilt:default -h localhost -p 9977 - -############ Camera ####################### -# 0 -> Deactivate, 1 -> Ice , 2 -> ROS -PanTiltTeleop.Camera.Server=2 -PanTiltTeleop.Camera.Proxy=cameraA:default -h localhost -p 9999 -PanTiltTeleop.Camera.Format=RGB8 -PanTiltTeleop.Camera.Topic=/usb_cam/image_raw -PanTiltTeleop.Camera.Name=pantilt_teleop_pyCamera - diff --git a/src/tools/pantilt_teleop_py/pantilt_teleop.py b/src/tools/pantilt_teleop_py/pantilt_teleop.py index c1ab24fbc..8735475b5 100755 --- a/src/tools/pantilt_teleop_py/pantilt_teleop.py +++ b/src/tools/pantilt_teleop_py/pantilt_teleop.py @@ -21,25 +21,24 @@ import sys -import jderobotComm as comm -from parallelIce.ptMotors import PTMotorsClient +import comm from gui.threadGUI import ThreadGUI from gui.GUI import MainWindow from PyQt5.QtWidgets import QApplication -import easyiceconfig as EasyIce +import config import signal signal.signal(signal.SIGINT, signal.SIG_DFL) if __name__ == '__main__': - ic = EasyIce.initialize(sys.argv) + cfg = config.load(sys.argv[1]) #starting comm - ic, node = comm.init(ic) + jdrc= comm.init(cfg, 'pantilt_teleop') - camera = comm.getCameraClient(ic, "PanTiltTeleop.Camera") - motors = PTMotorsClient(ic,"PanTiltTeleop.PTMotors", True) + camera = jdrc.getCameraClient("PanTiltTeleop.Camera") + motors = jdrc.getMotorsClient("PanTiltTeleop.PTMotors") app = QApplication(sys.argv) frame = MainWindow() diff --git a/src/tools/pantilt_teleop_py/pantilt_teleop.yml b/src/tools/pantilt_teleop_py/pantilt_teleop.yml new file mode 100644 index 000000000..2fd472ba6 --- /dev/null +++ b/src/tools/pantilt_teleop_py/pantilt_teleop.yml @@ -0,0 +1,15 @@ +pantilt_teleop: + PTMotors: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS (Not supported) + Proxy: "PanTilt:default -h localhost -p 9977" + Name: basic_component_pyPTMotors + + Camera: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraA:default -h localhost -p 9999" + Format: RGB8 + Topic: "/usb_cam/image_raw" + Name: pantilt_teleop_pyCamera + + NodeName: pantilt_teleop_py +