diff --git a/src/libs/comm_cpp/CMakeLists.txt b/src/libs/comm_cpp/CMakeLists.txt index 0dabe7c67..5719923e9 100644 --- a/src/libs/comm_cpp/CMakeLists.txt +++ b/src/libs/comm_cpp/CMakeLists.txt @@ -34,6 +34,7 @@ include_directories( set(HEADERS include/jderobot/comm/communicator.hpp + include/jderobot/comm/tools.hpp include/jderobot/comm/laserClient.hpp include/jderobot/comm/interfaces/laserClient.hpp include/jderobot/comm/ice/laserIceClient.hpp @@ -65,6 +66,7 @@ set(HEADERS set(SOURCES src/communicator.cpp + src/tools.cpp src/laserClient.cpp src/ice/laserIceClient.cpp src/cameraClient.cpp diff --git a/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp index 46a7a786e..eddd3078b 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp @@ -22,8 +22,13 @@ #include +#include #include #include +#include +#ifdef JDERROS +//#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp index c9c3227a9..11755e770 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp index e87eea284..961ec0d9f 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/cameraClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp index fd9d078dc..fb659ed05 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +//#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp b/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp index cd1b1a696..666301a92 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/communicator.hpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace Comm { diff --git a/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp index c1d8c435d..ab2d0db66 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/laserClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp index 40ccb5f0c..cfb5ed465 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/motorsClient.hpp @@ -22,8 +22,14 @@ #include #include +#include #include #include +#include +#include +#ifdef JDERROS +#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp index ad9771f36..2d5fabcd4 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +//#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp index 2569d4cbb..b10bb8555 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/pose3dClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/rgbdClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/rgbdClient.hpp index a868b77b6..0bbfda494 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/rgbdClient.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/rgbdClient.hpp @@ -22,8 +22,13 @@ #include #include +#include #include #include +#include +#ifdef JDERROS +//#include +#endif diff --git a/src/libs/comm_cpp/include/jderobot/comm/tools.hpp b/src/libs/comm_cpp/include/jderobot/comm/tools.hpp new file mode 100644 index 000000000..528e0b31c --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/tools.hpp @@ -0,0 +1,33 @@ +/* + * Copyright (C) 1997-2018 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_TOOLS_H +#define JDEROBOTCOMM_TOOLS_H + +#include + + +namespace Comm { + + int server2int (std::string server); + + +} //NS + +#endif // JDEROBOTCOMM_TOOLS_H \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ardroneextraClient.cpp b/src/libs/comm_cpp/src/ardroneextraClient.cpp index 0bdfddc92..a63248896 100644 --- a/src/libs/comm_cpp/src/ardroneextraClient.cpp +++ b/src/libs/comm_cpp/src/ardroneextraClient.cpp @@ -1,8 +1,5 @@ #include -#include -#ifdef JDERROS -//#include -#endif + namespace Comm { @@ -12,14 +9,7 @@ getArDroneExtraClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/bumperClient.cpp b/src/libs/comm_cpp/src/bumperClient.cpp index f6baee909..b9699d754 100644 --- a/src/libs/comm_cpp/src/bumperClient.cpp +++ b/src/libs/comm_cpp/src/bumperClient.cpp @@ -1,8 +1,5 @@ -#include -#include -#ifdef JDERROS -#include -#endif +#include + namespace Comm { @@ -12,14 +9,7 @@ getBumperClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/cameraClient.cpp b/src/libs/comm_cpp/src/cameraClient.cpp index 3466cbd26..15013f324 100644 --- a/src/libs/comm_cpp/src/cameraClient.cpp +++ b/src/libs/comm_cpp/src/cameraClient.cpp @@ -17,10 +17,7 @@ * Aitor Martinez Fernandez */ #include -#include -#ifdef JDERROS -#include -#endif + namespace Comm { @@ -30,14 +27,7 @@ getCameraClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/cmdvelClient.cpp b/src/libs/comm_cpp/src/cmdvelClient.cpp index 19a916ecc..c093ab990 100644 --- a/src/libs/comm_cpp/src/cmdvelClient.cpp +++ b/src/libs/comm_cpp/src/cmdvelClient.cpp @@ -1,8 +1,5 @@ #include -#include -#ifdef JDERROS -//#include -#endif + namespace Comm { @@ -12,14 +9,7 @@ getCMDVelClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/laserClient.cpp b/src/libs/comm_cpp/src/laserClient.cpp index 7ea71475c..93d6eee9d 100644 --- a/src/libs/comm_cpp/src/laserClient.cpp +++ b/src/libs/comm_cpp/src/laserClient.cpp @@ -1,8 +1,5 @@ -#include -#include -#ifdef JDERROS -#include -#endif +#include + namespace Comm { @@ -12,14 +9,7 @@ getLaserClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/motorsClient.cpp b/src/libs/comm_cpp/src/motorsClient.cpp index 6081ef874..6c97400cd 100644 --- a/src/libs/comm_cpp/src/motorsClient.cpp +++ b/src/libs/comm_cpp/src/motorsClient.cpp @@ -1,8 +1,4 @@ #include -#include -#ifdef JDERROS -#include -#endif namespace Comm { @@ -12,14 +8,7 @@ getMotorsClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/navdataClient.cpp b/src/libs/comm_cpp/src/navdataClient.cpp index 7ee90d81e..bcdd802f6 100644 --- a/src/libs/comm_cpp/src/navdataClient.cpp +++ b/src/libs/comm_cpp/src/navdataClient.cpp @@ -1,8 +1,5 @@ -#include -#include -#ifdef JDERROS -//#include -#endif +#include + namespace Comm { @@ -13,13 +10,7 @@ getNavdataClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/pose3dClient.cpp b/src/libs/comm_cpp/src/pose3dClient.cpp index 0e9bd93bb..d1f74e721 100644 --- a/src/libs/comm_cpp/src/pose3dClient.cpp +++ b/src/libs/comm_cpp/src/pose3dClient.cpp @@ -1,8 +1,5 @@ #include -#include -#ifdef JDERROS -#include -#endif + namespace Comm { @@ -13,13 +10,7 @@ getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/rgbdClient.cpp b/src/libs/comm_cpp/src/rgbdClient.cpp index 6db445d31..7ad2d1b8e 100644 --- a/src/libs/comm_cpp/src/rgbdClient.cpp +++ b/src/libs/comm_cpp/src/rgbdClient.cpp @@ -17,10 +17,7 @@ * Aitor Martinez Fernandez */ #include -#include -#ifdef JDERROS -//#include -#endif + namespace Comm { @@ -30,14 +27,7 @@ getRgbdClient(Comm::Communicator* jdrc, std::string prefix){ int server; std::string server_name = jdrc->getConfig().asString(prefix+".Server"); - std::transform(server_name.begin(), server_name.end(), server_name.begin(), ::tolower); - if(server_name == "ice"){ - server = 1; - } - else if(server_name == "ros"){ - server = 2; - } - else server = 0; + server = server2int(server_name); switch (server){ case 0: diff --git a/src/libs/comm_cpp/src/tools.cpp b/src/libs/comm_cpp/src/tools.cpp new file mode 100644 index 000000000..ed8c959bf --- /dev/null +++ b/src/libs/comm_cpp/src/tools.cpp @@ -0,0 +1,33 @@ +/* + * 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 { + int server2int(std::string server){ + std::transform(server.begin(), server.end(), server.begin(), ::tolower); + if(server == "ice" || server == "1"){ + return 1; + } + else if(server == "ros"|| server == "2"){ + return 2; + } + else return 0; + } + +}//NS \ No newline at end of file diff --git a/src/libs/comm_py/comm/__init__.py b/src/libs/comm_py/comm/__init__.py index f14587b65..efcac4bb5 100644 --- a/src/libs/comm_py/comm/__init__.py +++ b/src/libs/comm_py/comm/__init__.py @@ -1,22 +1,24 @@ from .communicator import Communicator +from .tools import server2int + def init (config, prefix): - ''' + ''' Starts JdeRobotComm @param config: configuration of client @type config: dict ''' - return Communicator(config, prefix) + return Communicator(config, prefix) - + diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in index 71b1073ee..7610816db 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.ardroneextraIceClient import ArDroneExtraIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in index 63b562d8d..6af92b4a4 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.ardroneextraIceClient import ArDroneExtraIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] diff --git a/src/libs/comm_py/comm/bumperClient.py.only-ice.in b/src/libs/comm_py/comm/bumperClient.py.only-ice.in index dfad02341..37059711f 100644 --- a/src/libs/comm_py/comm/bumperClient.py.only-ice.in +++ b/src/libs/comm_py/comm/bumperClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.bumperIceClient import BumperIceClient +from .tools import server2int def __getBumperIceClient(jdrc, prefix): @@ -67,11 +68,7 @@ def getBumperClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] diff --git a/src/libs/comm_py/comm/bumperClient.py.ros.in b/src/libs/comm_py/comm/bumperClient.py.ros.in index 3025b9401..731bb5698 100644 --- a/src/libs/comm_py/comm/bumperClient.py.ros.in +++ b/src/libs/comm_py/comm/bumperClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.bumperIceClient import BumperIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.listenerBumper import ListenerBumper @@ -76,11 +77,7 @@ def getBumperClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] diff --git a/src/libs/comm_py/comm/cameraClient.py.only-ice.in b/src/libs/comm_py/comm/cameraClient.py.only-ice.in index b245b3e2b..e1aed373d 100644 --- a/src/libs/comm_py/comm/cameraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cameraClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.cameraIceClient import CameraIceClient +from .tools import server2int def __getCameraIceClient(jdrc, prefix): ''' @@ -67,12 +68,8 @@ def getCameraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](jdrc, name) + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/cameraClient.py.ros.in b/src/libs/comm_py/comm/cameraClient.py.ros.in index e64c18e92..5c423d91c 100644 --- a/src/libs/comm_py/comm/cameraClient.py.ros.in +++ b/src/libs/comm_py/comm/cameraClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.cameraIceClient import CameraIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.listenerCamera import ListenerCamera @@ -76,11 +77,7 @@ def getCameraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] diff --git a/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in index 2c6317100..a31a67247 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.cmdvelIceClient import CMDVelIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] diff --git a/src/libs/comm_py/comm/cmdvelClient.py.ros.in b/src/libs/comm_py/comm/cmdvelClient.py.ros.in index 6a9af146a..83672407f 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.ros.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.ros.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.cmdvelIceClient import CMDVelIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] diff --git a/src/libs/comm_py/comm/communicator.py.only-ice.in b/src/libs/comm_py/comm/communicator.py.only-ice.in index 54ab093fb..e0e994167 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -11,6 +11,7 @@ from .navdataClient import getNavdataClient from .cmdvelClient import getCMDVelClient from .bumperClient import getBumperClient from .sonarClient import getSonarClient +from .tools import server2int @@ -37,7 +38,7 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": + if type(ymlNode[i]) is dict and server2int(ymlNode[i]["Server"]) == 1 : iceserver = True if iceserver: diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in index 41bec377e..cde4dac25 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -12,6 +12,7 @@ from .cmdvelClient import getCMDVelClient from .ptMotorsClient import getPTMotorsClient from .bumperClient import getBumperClient from .sonarClient import getSonarClient +from .tools import server2int @@ -38,9 +39,9 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": + if type(ymlNode[i]) is dict and server2int(ymlNode[i]["Server"]) == 1: iceserver = True - if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ros": + if type(ymlNode[i]) is dict and server2int(ymlNode[i]["Server"]) == 2: rosserver = True if rosserver: diff --git a/src/libs/comm_py/comm/laserClient.py.only-ice.in b/src/libs/comm_py/comm/laserClient.py.only-ice.in index b0dd3fb77..1f88c4689 100644 --- a/src/libs/comm_py/comm/laserClient.py.only-ice.in +++ b/src/libs/comm_py/comm/laserClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.laserIceClient import LaserIceClient +from .tools import server2int def __getLaserIceClient(jdrc, prefix): @@ -67,11 +68,7 @@ def getLaserClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] diff --git a/src/libs/comm_py/comm/laserClient.py.ros.in b/src/libs/comm_py/comm/laserClient.py.ros.in index fcc73bd66..f7fe427bf 100644 --- a/src/libs/comm_py/comm/laserClient.py.ros.in +++ b/src/libs/comm_py/comm/laserClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.laserIceClient import LaserIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.listenerLaser import ListenerLaser @@ -76,11 +77,7 @@ def getLaserClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] diff --git a/src/libs/comm_py/comm/motorsClient.py.only-ice.in b/src/libs/comm_py/comm/motorsClient.py.only-ice.in index e6a33857a..514d802af 100644 --- a/src/libs/comm_py/comm/motorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/motorsClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.motorsIceClient import MotorsIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getMotorsClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] diff --git a/src/libs/comm_py/comm/motorsClient.py.ros.in b/src/libs/comm_py/comm/motorsClient.py.ros.in index 7765a4829..0eb4b7d04 100644 --- a/src/libs/comm_py/comm/motorsClient.py.ros.in +++ b/src/libs/comm_py/comm/motorsClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.motorsIceClient import MotorsIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.publisherMotors import PublisherMotors @@ -91,11 +92,7 @@ def getMotorsClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] diff --git a/src/libs/comm_py/comm/navdataClient.py.only-ice.in b/src/libs/comm_py/comm/navdataClient.py.only-ice.in index a5d59d803..73d3469d7 100644 --- a/src/libs/comm_py/comm/navdataClient.py.only-ice.in +++ b/src/libs/comm_py/comm/navdataClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.navdataIceClient import NavdataIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] diff --git a/src/libs/comm_py/comm/navdataClient.py.ros.in b/src/libs/comm_py/comm/navdataClient.py.ros.in index 1ecd6601c..c462e4d07 100644 --- a/src/libs/comm_py/comm/navdataClient.py.ros.in +++ b/src/libs/comm_py/comm/navdataClient.py.ros.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.navdataIceClient import NavdataIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] diff --git a/src/libs/comm_py/comm/pose3dClient.py.only-ice.in b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in index 39627f385..7507ae784 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.only-ice.in +++ b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.pose3dIceClient import Pose3dIceClient +from .tools import server2int def __getPoseIceClient(jdrc, prefix): @@ -68,11 +69,7 @@ def getPose3dClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] diff --git a/src/libs/comm_py/comm/pose3dClient.py.ros.in b/src/libs/comm_py/comm/pose3dClient.py.ros.in index 71586b449..1b37452a0 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.ros.in +++ b/src/libs/comm_py/comm/pose3dClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.pose3dIceClient import Pose3dIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.listenerPose3d import ListenerPose3d @@ -79,11 +80,7 @@ def getPose3dClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in index f59a419b4..ae73107de 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.ptMotorsIceClient import PTMotorsIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getPTMotorsClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in index aa05b3fb4..e95e47fb7 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.ptMotorsIceClient import PTMotorsIceClient +from .tools import server2int @@ -71,11 +72,7 @@ def getPTMotorsClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] diff --git a/src/libs/comm_py/comm/rgbdClient.py.only-ice.in b/src/libs/comm_py/comm/rgbdClient.py.only-ice.in index ffaa70d50..8607787b3 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.only-ice.in +++ b/src/libs/comm_py/comm/rgbdClient.py.only-ice.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.rgbdIceClient import RgbdIceClient +from .tools import server2int #if (sys.version_info[0] == 2): # from .ros.listenerRgbd import ListenerRgbd @@ -70,11 +71,7 @@ def getRgbdClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Rgbddisabled, __getRgbdIceClient, __getListenerRgbd] diff --git a/src/libs/comm_py/comm/rgbdClient.py.ros.in b/src/libs/comm_py/comm/rgbdClient.py.ros.in index 0ad12e22f..716a8d355 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.ros.in +++ b/src/libs/comm_py/comm/rgbdClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.rgbdIceClient import RgbdIceClient +from .tools import server2int if (sys.version_info[0] == 2): from .ros.listenerRgbd import ListenerRgbd @@ -79,11 +80,7 @@ def getRgbdClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Rgbddisabled, __getRgbdIceClient, __getListenerRgbd] diff --git a/src/libs/comm_py/comm/sonarClient.py.only-ice.in b/src/libs/comm_py/comm/sonarClient.py.only-ice.in index ba7172253..55e284212 100644 --- a/src/libs/comm_py/comm/sonarClient.py.only-ice.in +++ b/src/libs/comm_py/comm/sonarClient.py.only-ice.in @@ -1,6 +1,7 @@ import sys import Ice from .ice.sonarIceClient import SonarIceClient +from .tools import server2int def __getSonarIceClient(jdrc, prefix): @@ -67,11 +68,7 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] diff --git a/src/libs/comm_py/comm/sonarClient.py.ros.in b/src/libs/comm_py/comm/sonarClient.py.ros.in index f5f2226f2..147265482 100644 --- a/src/libs/comm_py/comm/sonarClient.py.ros.in +++ b/src/libs/comm_py/comm/sonarClient.py.ros.in @@ -2,6 +2,7 @@ import sys import Ice import rospy from .ice.sonarIceClient import SonarIceClient +from .tools import server2int def __getSonarIceClient(jdrc, prefix): @@ -68,11 +69,7 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if server.lower() == "ice": - server=1 - elif server.lower() == "ros": - server=2 - else : server = 0 + server = server2int(server) cons = [__Sonardisabled, __getSonarIceClient, __getListenerSonar] diff --git a/src/libs/comm_py/comm/tools.py b/src/libs/comm_py/comm/tools.py new file mode 100644 index 000000000..f07a44357 --- /dev/null +++ b/src/libs/comm_py/comm/tools.py @@ -0,0 +1,9 @@ + + +def server2int(server): + serverl = server.lower() + if serverl == "ice" or serverl == "1": + return 1 + elif serverl == "ros" or serverl == "2": + return 2 + else : return 0 \ No newline at end of file diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index a181895d9..5bcc73a1e 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -166,9 +167,11 @@ int main(int argc, char** argv){ std::cerr <<"Error :" << msg << std::endl; return 1; } - cameraRGBActive=(bool)cfg.asIntWithDefault("rgbdViewer.CameraRGB.Server",0); - cameraDepthActive=(bool)cfg.asIntWithDefault("rgbdViewer.CameraDEPTH.Server",0); - cameraRGBDActive=(bool)cfg.asIntWithDefault("rgbdViewer.RGBD.Server",0); + + + cameraRGBActive=(bool)Comm::server2int(cfg.asStringWithDefault("rgbdViewer.CameraRGB.Server","0")); + cameraDepthActive=(bool)Comm::server2int(cfg.asStringWithDefault("rgbdViewer.CameraDEPTH.Server","0")); + cameraRGBDActive=(bool)Comm::server2int(cfg.asStringWithDefault("rgbdViewer.RGBD.Server","0")); if (cameraRGBDActive && (cameraRGBActive || cameraDepthActive)){ LOG(ERROR) << "RGBD and single cameras cannot be selected at the same time";