From 5bcbd22ce79e8c22a3d8ada7372d48c60be45e7f Mon Sep 17 00:00:00 2001 From: Pushkal Katara Date: Tue, 20 Feb 2018 16:08:33 +0530 Subject: [PATCH 01/10] [Libs] Makes YML config file description more readable Instead of ``` Server: 1 or Server: 2 ``` Changed to ``` Server: "Ice" or Server: "ROS" ``` Solves issue https://github.com/JdeRobot/JdeRobot/issues/1060 --- .../MAVLinkServer/MAVProxy/uav_viewer_py.yml | 10 +++--- src/drivers/YoutubeServer/youtubeserver.yml | 3 +- .../basic_component/basic_component.yml | 7 ++-- .../basic_component_py/basic_component_py.yml | 6 ++-- src/libs/comm_cpp/src/ardroneextraClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/bumperClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/cameraClient.cpp | 15 +++++++-- src/libs/comm_cpp/src/cmdvelClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/laserClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/motorsClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/navdataClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/pose3dClient.cpp | 15 +++++++-- src/libs/comm_cpp/src/rgbdClient.cpp | 15 +++++++-- .../comm/ardroneextraClient.py.only-ice.in | 11 ++++--- .../comm_py/comm/ardroneextraClient.py.ros.in | 11 ++++--- .../comm_py/comm/bumperClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/bumperClient.py.ros.in | 9 +++-- .../comm_py/comm/cameraClient.py.only-ice.in | 11 +++++-- src/libs/comm_py/comm/cameraClient.py.ros.in | 9 +++-- .../comm_py/comm/cmdvelClient.py.only-ice.in | 11 ++++--- src/libs/comm_py/comm/cmdvelClient.py.ros.in | 11 ++++--- .../comm_py/comm/communicator.py.only-ice.in | 29 ++++++++-------- src/libs/comm_py/comm/communicator.py.ros.in | 33 +++++++++---------- .../comm_py/comm/laserClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/laserClient.py.ros.in | 9 +++-- .../comm_py/comm/motorsClient.py.only-ice.in | 11 +++++-- src/libs/comm_py/comm/motorsClient.py.ros.in | 13 +++++--- .../comm_py/comm/navdataClient.py.only-ice.in | 11 ++++--- src/libs/comm_py/comm/navdataClient.py.ros.in | 11 ++++--- .../comm_py/comm/pose3dClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/pose3dClient.py.ros.in | 9 +++-- .../comm/ptMotorsClient.py.only-ice.in | 11 +++++-- .../comm_py/comm/ptMotorsClient.py.ros.in | 11 +++++-- .../comm_py/comm/rgbdClient.py.only-ice.in | 7 +++- src/libs/comm_py/comm/rgbdClient.py.ros.in | 7 +++- .../comm_py/comm/sonarClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/sonarClient.py.ros.in | 11 ++++--- src/libs/comm_py/tests/test.yml | 22 ++++++------- src/libs/config_cpp/src/demo/demo.yml | 7 ++-- src/libs/config_py/demo/demo.yml | 7 ++-- src/tools/cameraview/cameraview.yml | 4 +-- src/tools/colorTuner_py/colorTuner_py.yml | 2 +- src/tools/kobukiViewer/kobukiViewer.yml | 10 +++--- .../pantilt_teleop_py/pantilt_teleop.yml | 7 ++-- src/tools/rgbdViewer/rgbdViewer.yml | 12 +++---- src/tools/scratch2jderobot/cfg/drone.yml | 14 ++++---- src/tools/scratch2jderobot/cfg/robot.yml | 8 ++--- src/tools/uav_viewer_py/uav_viewer_py.yml | 12 +++---- 48 files changed, 355 insertions(+), 184 deletions(-) diff --git a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml index d2e72d586..845888521 100644 --- a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml +++ b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml @@ -1,30 +1,30 @@ Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "default -h 0.0.0.0 -p 9999" Format: RGB8 Topic: "/MavLink/image_raw" Name: MavLinkCamera Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "default -h 0.0.0.0 -p 9998" Topic: "/MavLink/Pose3D" Name: MavLinkPose3d CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "default -h 0.0.0.0 -p 9997" Topic: "/MavLink/CMDVel" Name: MavLinkCMDVel Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "default -h 0.0.0.0 -p 9996" Topic: "/MavLink/Navdata" Name: MavLinkNavdata Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "default -h 0.0.0.0 -p 9995" Topic: "/MavLink/Extra" Name: MavLinkExtra diff --git a/src/drivers/YoutubeServer/youtubeserver.yml b/src/drivers/YoutubeServer/youtubeserver.yml index 9f0a4692b..3138e66bc 100644 --- a/src/drivers/YoutubeServer/youtubeserver.yml +++ b/src/drivers/YoutubeServer/youtubeserver.yml @@ -1,6 +1,6 @@ youtubeServer: ImageSrv: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS (Not supported) + Server: Ice #Deactivate, Ice , ROS (Not supported) Proxy: "default -h 0.0.0.0 -p 9999" URL: "https://www.youtube.com/watch?v=zw47_q9wbBE" LiveBroadcast: False #True for youtube live events @@ -10,4 +10,3 @@ youtubeServer: Name: youtubeServer_py NodeName: youtubeServerCfg - diff --git a/src/examples/basic_component/basic_component.yml b/src/examples/basic_component/basic_component.yml index 0c473255d..b0570ee77 100644 --- a/src/examples/basic_component/basic_component.yml +++ b/src/examples/basic_component/basic_component.yml @@ -1,14 +1,14 @@ basic_component: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: basic_componentCamera maxW: 0.7 maxV: 4 - + Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" @@ -17,4 +17,3 @@ basic_component: NodeName: basic_component Vmax: 5 Wmax: 0.5 - diff --git a/src/examples/basic_component_py/basic_component_py.yml b/src/examples/basic_component_py/basic_component_py.yml index 561135419..94369b82c 100644 --- a/src/examples/basic_component_py/basic_component_py.yml +++ b/src/examples/basic_component_py/basic_component_py.yml @@ -1,14 +1,14 @@ basic_component: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , 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: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" diff --git a/src/libs/comm_cpp/src/ardroneextraClient.cpp b/src/libs/comm_cpp/src/ardroneextraClient.cpp index 330c70389..0bdfddc92 100644 --- a/src/libs/comm_cpp/src/ardroneextraClient.cpp +++ b/src/libs/comm_cpp/src/ardroneextraClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -ArDroneExtraClient* +ArDroneExtraClient* getArDroneExtraClient(Comm::Communicator* jdrc, std::string prefix){ ArDroneExtraClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/bumperClient.cpp b/src/libs/comm_cpp/src/bumperClient.cpp index 090a6034d..f6baee909 100644 --- a/src/libs/comm_cpp/src/bumperClient.cpp +++ b/src/libs/comm_cpp/src/bumperClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -BumperClient* +BumperClient* getBumperClient(Comm::Communicator* jdrc, std::string prefix){ BumperClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/cameraClient.cpp b/src/libs/comm_cpp/src/cameraClient.cpp index f52c57de8..3466cbd26 100644 --- a/src/libs/comm_cpp/src/cameraClient.cpp +++ b/src/libs/comm_cpp/src/cameraClient.cpp @@ -24,12 +24,21 @@ namespace Comm { -CameraClient* +CameraClient* getCameraClient(Comm::Communicator* jdrc, std::string prefix){ CameraClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -76,4 +85,4 @@ getCameraClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS diff --git a/src/libs/comm_cpp/src/cmdvelClient.cpp b/src/libs/comm_cpp/src/cmdvelClient.cpp index 43943e5d6..19a916ecc 100644 --- a/src/libs/comm_cpp/src/cmdvelClient.cpp +++ b/src/libs/comm_cpp/src/cmdvelClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -CMDVelClient* +CMDVelClient* getCMDVelClient(Comm::Communicator* jdrc, std::string prefix){ CMDVelClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/laserClient.cpp b/src/libs/comm_cpp/src/laserClient.cpp index 71de12e20..7ea71475c 100644 --- a/src/libs/comm_cpp/src/laserClient.cpp +++ b/src/libs/comm_cpp/src/laserClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -LaserClient* +LaserClient* getLaserClient(Comm::Communicator* jdrc, std::string prefix){ LaserClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/motorsClient.cpp b/src/libs/comm_cpp/src/motorsClient.cpp index 8fc10f160..6081ef874 100644 --- a/src/libs/comm_cpp/src/motorsClient.cpp +++ b/src/libs/comm_cpp/src/motorsClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -MotorsClient* +MotorsClient* getMotorsClient(Comm::Communicator* jdrc, std::string prefix){ MotorsClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/navdataClient.cpp b/src/libs/comm_cpp/src/navdataClient.cpp index 65a1a6029..7ee90d81e 100644 --- a/src/libs/comm_cpp/src/navdataClient.cpp +++ b/src/libs/comm_cpp/src/navdataClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -NavdataClient* +NavdataClient* getNavdataClient(Comm::Communicator* jdrc, std::string prefix){ NavdataClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/pose3dClient.cpp b/src/libs/comm_cpp/src/pose3dClient.cpp index 80c8d2f9b..0e9bd93bb 100644 --- a/src/libs/comm_cpp/src/pose3dClient.cpp +++ b/src/libs/comm_cpp/src/pose3dClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -Pose3dClient* +Pose3dClient* getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ Pose3dClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -57,4 +66,4 @@ getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS diff --git a/src/libs/comm_cpp/src/rgbdClient.cpp b/src/libs/comm_cpp/src/rgbdClient.cpp index 9c961809e..6db445d31 100644 --- a/src/libs/comm_cpp/src/rgbdClient.cpp +++ b/src/libs/comm_cpp/src/rgbdClient.cpp @@ -24,12 +24,21 @@ namespace Comm { -RgbdClient* +RgbdClient* getRgbdClient(Comm::Communicator* jdrc, std::string prefix){ RgbdClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -77,4 +86,4 @@ getRgbdClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS 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 f5b06fa96..71b1073ee 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherArDroneExtra(jdrc, prefix): @return ArDroneExtra ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in index b29238ffb..63b562d8d 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherArDroneExtra(jdrc, prefix): @return ArDroneExtra ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 9947e1070..dfad02341 100644 --- a/src/libs/comm_py/comm/bumperClient.py.only-ice.in +++ b/src/libs/comm_py/comm/bumperClient.py.only-ice.in @@ -66,8 +66,13 @@ def getBumperClient (jdrc, prefix): @return None if Bumper is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/bumperClient.py.ros.in b/src/libs/comm_py/comm/bumperClient.py.ros.in index e15e7a875..3025b9401 100644 --- a/src/libs/comm_py/comm/bumperClient.py.ros.in +++ b/src/libs/comm_py/comm/bumperClient.py.ros.in @@ -75,8 +75,13 @@ def getBumperClient (jdrc, prefix): @return None if Bumper is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 ba3082568..b245b3e2b 100644 --- a/src/libs/comm_py/comm/cameraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cameraClient.py.only-ice.in @@ -33,7 +33,7 @@ def __getListenerCamera(jdrc, prefix): @return Camera ROS Subscriber ''' - + print(prefix + ": ROS msg are diabled") return None @@ -66,8 +66,13 @@ def getCameraClient (jdrc, prefix): @return None if Camera is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](jdrc, name) \ No newline at end of file + return cons[server](jdrc, name) diff --git a/src/libs/comm_py/comm/cameraClient.py.ros.in b/src/libs/comm_py/comm/cameraClient.py.ros.in index fda8a419a..e64c18e92 100644 --- a/src/libs/comm_py/comm/cameraClient.py.ros.in +++ b/src/libs/comm_py/comm/cameraClient.py.ros.in @@ -75,8 +75,13 @@ def getCameraClient (jdrc, prefix): @return None if Camera is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 e56668c21..2c6317100 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherCMDVel(jdrc, prefix): @return CMDVel ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/cmdvelClient.py.ros.in b/src/libs/comm_py/comm/cmdvelClient.py.ros.in index 5e9be18c9..6a9af146a 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.ros.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherCMDVel(jdrc, prefix): @return CMDVel ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 d1e51662e..7f179cd34 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -24,7 +24,7 @@ class Communicator: Communicator constructor @param config: configuration of communicator - + @type config: dict ''' @@ -37,7 +37,7 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": iceserver = True if iceserver: @@ -60,7 +60,7 @@ class Communicator: return self.__ic def getConfig(self): - return self.config + return self.config def getCameraClient(self, name): @@ -68,7 +68,7 @@ class Communicator: Returns a Camera client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -79,7 +79,7 @@ class Communicator: Returns a Motors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -90,7 +90,7 @@ class Communicator: Returns a PTMotors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -101,7 +101,7 @@ class Communicator: Returns a Pose3D client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -112,7 +112,7 @@ class Communicator: Returns a Laser client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -123,7 +123,7 @@ class Communicator: Returns a RGBD client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -134,7 +134,7 @@ class Communicator: Returns a CMDVel client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -145,7 +145,7 @@ class Communicator: Returns a Navdata client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -156,7 +156,7 @@ class Communicator: Returns a ArDroneExtra client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -168,7 +168,7 @@ class Communicator: Returns a Bumper client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -179,9 +179,8 @@ class Communicator: Returns a Sonar client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' return getSonarClient(self, name) - diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in index 34d24af5f..91cb80062 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -25,7 +25,7 @@ class Communicator: Communicator constructor @param config: configuration of communicator - + @type config: dict ''' @@ -38,9 +38,9 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": iceserver = True - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 2: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ros": rosserver = True if rosserver: @@ -66,8 +66,8 @@ class Communicator: def getIc(self): return self.__ic - def getConfig(self): - return self.config + def getConfig(self): + return self.config def getCameraClient(self, name): @@ -75,7 +75,7 @@ class Communicator: Returns a Camera client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -86,7 +86,7 @@ class Communicator: Returns a Motors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -97,7 +97,7 @@ class Communicator: Returns a Pose3D client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -108,7 +108,7 @@ class Communicator: Returns a Laser client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -120,7 +120,7 @@ class Communicator: Returns a RGBD client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -131,7 +131,7 @@ class Communicator: Returns a CMDVel client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -142,7 +142,7 @@ class Communicator: Returns a Navdata client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -153,7 +153,7 @@ class Communicator: Returns a ArDroneExtra client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -164,7 +164,7 @@ class Communicator: Returns a PTMotors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -175,7 +175,7 @@ class Communicator: Returns a Bumper client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -186,9 +186,8 @@ class Communicator: Returns a Sonar client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' return getSonarClient(self, name) - 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 83c5e3212..b0dd3fb77 100644 --- a/src/libs/comm_py/comm/laserClient.py.only-ice.in +++ b/src/libs/comm_py/comm/laserClient.py.only-ice.in @@ -66,8 +66,13 @@ def getLaserClient (jdrc, prefix): @return None if Laser is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/laserClient.py.ros.in b/src/libs/comm_py/comm/laserClient.py.ros.in index ef6afbbd3..fcc73bd66 100644 --- a/src/libs/comm_py/comm/laserClient.py.ros.in +++ b/src/libs/comm_py/comm/laserClient.py.ros.in @@ -75,8 +75,13 @@ def getLaserClient (jdrc, prefix): @return None if Laser is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 10c5ece30..e6a33857a 100644 --- a/src/libs/comm_py/comm/motorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/motorsClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherMotors(jdrc, prefix): @return Motors ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -70,8 +70,13 @@ def getMotorsClient (jdrc, prefix): @return None if Motors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/motorsClient.py.ros.in b/src/libs/comm_py/comm/motorsClient.py.ros.in index 022f338bd..7765a4829 100644 --- a/src/libs/comm_py/comm/motorsClient.py.ros.in +++ b/src/libs/comm_py/comm/motorsClient.py.ros.in @@ -45,14 +45,14 @@ def __getPublisherMotors(jdrc, prefix): if not maxW: maxW = 0.5 print (prefix+".maxW not provided, the default value is used: "+ repr(maxW)) - + maxV = jdrc.getConfig().getPropertyWithDefault(prefix+".maxV", 5) if not maxV: maxV = 5 print (prefix+".maxV not provided, the default value is used: "+ repr(maxV)) - + client = PublisherMotors(topic, maxV, maxW) return client else: @@ -90,8 +90,13 @@ def getMotorsClient (jdrc, prefix): @return None if Motors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 f1be2d4c3..a5d59d803 100644 --- a/src/libs/comm_py/comm/navdataClient.py.only-ice.in +++ b/src/libs/comm_py/comm/navdataClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherNavdata(jdrc, prefix): @return Navdata ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/navdataClient.py.ros.in b/src/libs/comm_py/comm/navdataClient.py.ros.in index 1501e5661..1ecd6601c 100644 --- a/src/libs/comm_py/comm/navdataClient.py.ros.in +++ b/src/libs/comm_py/comm/navdataClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherNavdata(jdrc, prefix): @return Navdata ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 eb3f657dc..39627f385 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.only-ice.in +++ b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in @@ -67,8 +67,13 @@ def getPose3dClient (jdrc, prefix): @return None if pose3d is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/pose3dClient.py.ros.in b/src/libs/comm_py/comm/pose3dClient.py.ros.in index 35dbc9bd4..71586b449 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.ros.in +++ b/src/libs/comm_py/comm/pose3dClient.py.ros.in @@ -78,8 +78,13 @@ def getPose3dClient (jdrc, prefix): @return None if pose3d is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 c06a304b5..f59a419b4 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherPTMotors(jdrc, prefix): @return PTMotors ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -70,8 +70,13 @@ def getPTMotorsClient (jdrc, prefix): @return None if PTMotors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in index 8ac285629..aa05b3fb4 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherPTMotors(jdrc, prefix): @return PTMotors ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -70,8 +70,13 @@ def getPTMotorsClient (jdrc, prefix): @return None if PTMotors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 b9b34c950..ffaa70d50 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.only-ice.in +++ b/src/libs/comm_py/comm/rgbdClient.py.only-ice.in @@ -69,7 +69,12 @@ def getRgbdClient (jdrc, prefix): @return None if Rgbd is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 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 06a049efc..0ad12e22f 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.ros.in +++ b/src/libs/comm_py/comm/rgbdClient.py.ros.in @@ -78,7 +78,12 @@ def getRgbdClient (jdrc, prefix): @return None if Rgbd is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 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 ed3e42dd8..ba7172253 100644 --- a/src/libs/comm_py/comm/sonarClient.py.only-ice.in +++ b/src/libs/comm_py/comm/sonarClient.py.only-ice.in @@ -67,9 +67,12 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/sonarClient.py.ros.in b/src/libs/comm_py/comm/sonarClient.py.ros.in index 0f2364749..f5f2226f2 100644 --- a/src/libs/comm_py/comm/sonarClient.py.ros.in +++ b/src/libs/comm_py/comm/sonarClient.py.ros.in @@ -34,7 +34,7 @@ def __getListenerSonar(jdrc, prefix): @return Laser ROS Subscriber - ''' + ''' print(prefix + ": This Interface doesn't support ROS msg") return None @@ -68,9 +68,12 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Sonardisabled, __getSonarIceClient, __getListenerSonar] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/tests/test.yml b/src/libs/comm_py/tests/test.yml index 8c679a42e..5b4b34225 100644 --- a/src/libs/comm_py/tests/test.yml +++ b/src/libs/comm_py/tests/test.yml @@ -1,6 +1,6 @@ Test: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: testMotors @@ -8,55 +8,55 @@ Test: maxW: 0.7 Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" Name: testCamera1 Camera2: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraR:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraR/image_raw" Name: testCamera2 Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "/turtlebotROS/odom" Name: testPose3d Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9001" Topic: "/turtlebotROS/laser/scan" Name: testLaser Sonar: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Sonar:default -h localhost -p 8993" Topic: "/turtlebotROS/laser/scan" Name: testSonar CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:default -h 0.0.0.0 -p 9000" Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Navdata:default -h 0.0.0.0 -p 9000 Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Extra:default -h 0.0.0.0 -p 9000 Bumper: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Extra:default -h 0.0.0.0 -p 9000 Topic: "/turtlebotROS/mobile_base/events/bumper" Name: testPose3d Vmax: 3 Wmax: 0.7 - NodeName: JdeRobotCommTest \ No newline at end of file + NodeName: JdeRobotCommTest diff --git a/src/libs/config_cpp/src/demo/demo.yml b/src/libs/config_cpp/src/demo/demo.yml index 04a17ada4..725ebb2ec 100644 --- a/src/libs/config_cpp/src/demo/demo.yml +++ b/src/libs/config_cpp/src/demo/demo.yml @@ -1,18 +1,17 @@ Demo: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , 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 + Server: ROS # Deactivate, Ice , 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_py/demo/demo.yml b/src/libs/config_py/demo/demo.yml index abc5b1518..3600abea2 100644 --- a/src/libs/config_py/demo/demo.yml +++ b/src/libs/config_py/demo/demo.yml @@ -1,18 +1,17 @@ Demo: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , 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 + Server: ROS # Deactivate, Ice , 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/tools/cameraview/cameraview.yml b/src/tools/cameraview/cameraview.yml index 794c83f25..9b936ec35 100644 --- a/src/tools/cameraview/cameraview.yml +++ b/src/tools/cameraview/cameraview.yml @@ -1,10 +1,10 @@ Cameraview: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , 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 + NodeName: cameraview diff --git a/src/tools/colorTuner_py/colorTuner_py.yml b/src/tools/colorTuner_py/colorTuner_py.yml index f1167d452..f4a8176c0 100644 --- a/src/tools/colorTuner_py/colorTuner_py.yml +++ b/src/tools/colorTuner_py/colorTuner_py.yml @@ -1,6 +1,6 @@ ColorTuner: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" diff --git a/src/tools/kobukiViewer/kobukiViewer.yml b/src/tools/kobukiViewer/kobukiViewer.yml index 6240df791..95dd26dbb 100644 --- a/src/tools/kobukiViewer/kobukiViewer.yml +++ b/src/tools/kobukiViewer/kobukiViewer.yml @@ -1,6 +1,6 @@ kobukiViewer: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: kobukiViewerMotors @@ -8,27 +8,27 @@ kobukiViewer: maxW: 0.7 Camera1: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" Name: kobukiViewerCamera1 Camera2: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "CameraR:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraR/image_raw" Name: kobukiViewerCamera2 Pose3D: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "//turtlebotROS/odom" Name: kobukiViewerPose3d Laser: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9001" Topic: "/turtlebotROS/laser/scan" Name: kobukiViewerLaser diff --git a/src/tools/pantilt_teleop_py/pantilt_teleop.yml b/src/tools/pantilt_teleop_py/pantilt_teleop.yml index 2fd472ba6..42312070e 100644 --- a/src/tools/pantilt_teleop_py/pantilt_teleop.yml +++ b/src/tools/pantilt_teleop_py/pantilt_teleop.yml @@ -1,15 +1,14 @@ pantilt_teleop: PTMotors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS (Not supported) + Server: Ice # Deactivate, Ice , ROS (Not supported) Proxy: "PanTilt:default -h localhost -p 9977" Name: basic_component_pyPTMotors - + Camera: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "cameraA:default -h localhost -p 9999" Format: RGB8 Topic: "/usb_cam/image_raw" Name: pantilt_teleop_pyCamera NodeName: pantilt_teleop_py - diff --git a/src/tools/rgbdViewer/rgbdViewer.yml b/src/tools/rgbdViewer/rgbdViewer.yml index 0d982c075..166c889f4 100644 --- a/src/tools/rgbdViewer/rgbdViewer.yml +++ b/src/tools/rgbdViewer/rgbdViewer.yml @@ -1,6 +1,6 @@ rgbdViewer: CameraRGB: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 Topic: "/camera/rgb/image_raw" @@ -8,7 +8,7 @@ rgbdViewer: Fps: 30 CameraDEPTH: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice, ROS Proxy: "cameraB:tcp -h localhost -p 9999" Format: RGB8 Topic: "/camera/depth_registered/image_raw" @@ -16,27 +16,27 @@ rgbdViewer: Fps: 30 PointCloud: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "pointcloud1:tcp -h localhost -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: pointcloud Fps: 30 RGBD: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "rgbd1:tcp -h localhost -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: RGBD Fps: 30 Pose3DMotors: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "Pose3DMotors1:tcp -h 193.147.14.20 -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: Pose3DMotors KinectLeds: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "kinectleds1:tcp -h 193.147.14.20 -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: KinectLeds diff --git a/src/tools/scratch2jderobot/cfg/drone.yml b/src/tools/scratch2jderobot/cfg/drone.yml index 57fc7906c..6784a6743 100644 --- a/src/tools/scratch2jderobot/cfg/drone.yml +++ b/src/tools/scratch2jderobot/cfg/drone.yml @@ -1,6 +1,6 @@ drone: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9000" Topic: "/cmd_vel_mux/input/teleop" Name: Motors @@ -8,36 +8,36 @@ drone: maxW: 0.7 Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Camera:default -h localhost -p 9000" Format: RGB8 Topic: "/camera/rgb/image_raw" Name: Camera1 Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9000" Topic: "/odom" Name: Pose3d Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9000" Topic: "/scan" Name: Laser CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:default -h localhost -p 9000" Name: CMDVel Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Navdata:default -h localhost -p 9000" Name: Navdata Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Extra:default -h localhost -p 9000" Name: Extra diff --git a/src/tools/scratch2jderobot/cfg/robot.yml b/src/tools/scratch2jderobot/cfg/robot.yml index 9d13e29a9..28f8730e4 100644 --- a/src/tools/scratch2jderobot/cfg/robot.yml +++ b/src/tools/scratch2jderobot/cfg/robot.yml @@ -1,6 +1,6 @@ robot: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/mobile_base/commands/velocity" Name: robotMotors @@ -8,19 +8,19 @@ robot: maxV: 4 Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:tcp -h localhost -p 9001" Topic: "/scan" Name: robotLaser Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "/odom" Name: robotPose3d Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/cameraL/image_raw" diff --git a/src/tools/uav_viewer_py/uav_viewer_py.yml b/src/tools/uav_viewer_py/uav_viewer_py.yml index 43512936a..b39b9ac44 100644 --- a/src/tools/uav_viewer_py/uav_viewer_py.yml +++ b/src/tools/uav_viewer_py/uav_viewer_py.yml @@ -1,31 +1,31 @@ UAVViewer: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Camera:default -h localhost -p 9000" Format: RGB8 Topic: "/IntrorobROS/image_raw" Name: UAVViewerCamera Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9000" Topic: "/IntrorobROS/Pose3D" Name: UAVViewerPose3d CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:tcp -h localhost -p 9000" Topic: "/IntrorobROS/CMDVel" Name: UAVViewerCMDVel Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Navdata:tcp -h localhost -p 9000" Topic: "/IntrorobROS/Navdata" Name: UAVViewerNavdata Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Extra:tcp -h localhost -p 9000" Topic: "/IntrorobROS/Extra" Name: UAVViewerExtra @@ -33,6 +33,6 @@ UAVViewer: Xmax: 10 Ymax: 10 Zmax: 5 - Yawmax: 1 + Yawmax: 1 NodeName: UAVViewer From d036f08edcda9d24ca3253772b9d2ef6899a91b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20P=C3=A9rez?= Date: Thu, 22 Feb 2018 18:26:48 +0100 Subject: [PATCH 02/10] Update communicator.py.ros.in --- src/libs/comm_py/comm/communicator.py.ros.in | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in index 91cb80062..41bec377e 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -38,9 +38,9 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": + if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": iceserver = True - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ros": + if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ros": rosserver = True if rosserver: From 119f7bbc92e1ca685dfb3d7ccccb04fe64727f1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20P=C3=A9rez?= Date: Thu, 22 Feb 2018 18:27:35 +0100 Subject: [PATCH 03/10] Update communicator.py.only-ice.in --- src/libs/comm_py/comm/communicator.py.only-ice.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7f179cd34..54ab093fb 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -37,7 +37,7 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": + if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": iceserver = True if iceserver: From baa4bd6e0b37c958507c708189caf805b0f2537f Mon Sep 17 00:00:00 2001 From: Pushkal Katara Date: Tue, 20 Feb 2018 16:08:33 +0530 Subject: [PATCH 04/10] [Libs] Makes YML config file description more readable Instead of ``` Server: 1 or Server: 2 ``` Changed to ``` Server: "Ice" or Server: "ROS" ``` Solves issue https://github.com/JdeRobot/JdeRobot/issues/1060 --- .../MAVLinkServer/MAVProxy/uav_viewer_py.yml | 30 ----------------- src/drivers/YoutubeServer/youtubeserver.yml | 3 +- .../basic_component/basic_component.yml | 7 ++-- .../basic_component_py/basic_component_py.yml | 6 ++-- src/libs/comm_cpp/src/ardroneextraClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/bumperClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/cameraClient.cpp | 15 +++++++-- src/libs/comm_cpp/src/cmdvelClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/laserClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/motorsClient.cpp | 13 ++++++-- src/libs/comm_cpp/src/navdataClient.cpp | 14 ++++++-- src/libs/comm_cpp/src/pose3dClient.cpp | 15 +++++++-- src/libs/comm_cpp/src/rgbdClient.cpp | 15 +++++++-- .../comm/ardroneextraClient.py.only-ice.in | 11 ++++--- .../comm_py/comm/ardroneextraClient.py.ros.in | 11 ++++--- .../comm_py/comm/bumperClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/bumperClient.py.ros.in | 9 +++-- .../comm_py/comm/cameraClient.py.only-ice.in | 11 +++++-- src/libs/comm_py/comm/cameraClient.py.ros.in | 9 +++-- .../comm_py/comm/cmdvelClient.py.only-ice.in | 11 ++++--- src/libs/comm_py/comm/cmdvelClient.py.ros.in | 11 ++++--- .../comm_py/comm/communicator.py.only-ice.in | 29 ++++++++-------- src/libs/comm_py/comm/communicator.py.ros.in | 33 +++++++++---------- .../comm_py/comm/laserClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/laserClient.py.ros.in | 9 +++-- .../comm_py/comm/motorsClient.py.only-ice.in | 11 +++++-- src/libs/comm_py/comm/motorsClient.py.ros.in | 13 +++++--- .../comm_py/comm/navdataClient.py.only-ice.in | 11 ++++--- src/libs/comm_py/comm/navdataClient.py.ros.in | 11 ++++--- .../comm_py/comm/pose3dClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/pose3dClient.py.ros.in | 9 +++-- .../comm/ptMotorsClient.py.only-ice.in | 11 +++++-- .../comm_py/comm/ptMotorsClient.py.ros.in | 11 +++++-- .../comm_py/comm/rgbdClient.py.only-ice.in | 7 +++- src/libs/comm_py/comm/rgbdClient.py.ros.in | 7 +++- .../comm_py/comm/sonarClient.py.only-ice.in | 9 +++-- src/libs/comm_py/comm/sonarClient.py.ros.in | 11 ++++--- src/libs/comm_py/tests/test.yml | 22 ++++++------- src/libs/config_cpp/src/demo/demo.yml | 7 ++-- src/libs/config_py/demo/demo.yml | 7 ++-- src/tools/cameraview/cameraview.yml | 4 +-- src/tools/colorTuner_py/colorTuner_py.yml | 2 +- src/tools/kobukiViewer/kobukiViewer.yml | 10 +++--- .../pantilt_teleop_py/pantilt_teleop.yml | 7 ++-- src/tools/rgbdViewer/rgbdViewer.yml | 12 +++---- src/tools/scratch2jderobot/cfg/drone.yml | 14 ++++---- src/tools/scratch2jderobot/cfg/robot.yml | 8 ++--- src/tools/uav_viewer_py/uav_viewer_py.yml | 12 +++---- 48 files changed, 350 insertions(+), 209 deletions(-) delete mode 100644 src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml diff --git a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml deleted file mode 100644 index d2e72d586..000000000 --- a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml +++ /dev/null @@ -1,30 +0,0 @@ -Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "default -h 0.0.0.0 -p 9999" - Format: RGB8 - Topic: "/MavLink/image_raw" - Name: MavLinkCamera - -Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "default -h 0.0.0.0 -p 9998" - Topic: "/MavLink/Pose3D" - Name: MavLinkPose3d - -CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "default -h 0.0.0.0 -p 9997" - Topic: "/MavLink/CMDVel" - Name: MavLinkCMDVel - -Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "default -h 0.0.0.0 -p 9996" - Topic: "/MavLink/Navdata" - Name: MavLinkNavdata - -Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "default -h 0.0.0.0 -p 9995" - Topic: "/MavLink/Extra" - Name: MavLinkExtra diff --git a/src/drivers/YoutubeServer/youtubeserver.yml b/src/drivers/YoutubeServer/youtubeserver.yml index 9f0a4692b..3138e66bc 100644 --- a/src/drivers/YoutubeServer/youtubeserver.yml +++ b/src/drivers/YoutubeServer/youtubeserver.yml @@ -1,6 +1,6 @@ youtubeServer: ImageSrv: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS (Not supported) + Server: Ice #Deactivate, Ice , ROS (Not supported) Proxy: "default -h 0.0.0.0 -p 9999" URL: "https://www.youtube.com/watch?v=zw47_q9wbBE" LiveBroadcast: False #True for youtube live events @@ -10,4 +10,3 @@ youtubeServer: Name: youtubeServer_py NodeName: youtubeServerCfg - diff --git a/src/examples/basic_component/basic_component.yml b/src/examples/basic_component/basic_component.yml index 0c473255d..b0570ee77 100644 --- a/src/examples/basic_component/basic_component.yml +++ b/src/examples/basic_component/basic_component.yml @@ -1,14 +1,14 @@ basic_component: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: basic_componentCamera maxW: 0.7 maxV: 4 - + Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" @@ -17,4 +17,3 @@ basic_component: NodeName: basic_component Vmax: 5 Wmax: 0.5 - diff --git a/src/examples/basic_component_py/basic_component_py.yml b/src/examples/basic_component_py/basic_component_py.yml index 561135419..94369b82c 100644 --- a/src/examples/basic_component_py/basic_component_py.yml +++ b/src/examples/basic_component_py/basic_component_py.yml @@ -1,14 +1,14 @@ basic_component: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , 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: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" diff --git a/src/libs/comm_cpp/src/ardroneextraClient.cpp b/src/libs/comm_cpp/src/ardroneextraClient.cpp index 330c70389..0bdfddc92 100644 --- a/src/libs/comm_cpp/src/ardroneextraClient.cpp +++ b/src/libs/comm_cpp/src/ardroneextraClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -ArDroneExtraClient* +ArDroneExtraClient* getArDroneExtraClient(Comm::Communicator* jdrc, std::string prefix){ ArDroneExtraClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/bumperClient.cpp b/src/libs/comm_cpp/src/bumperClient.cpp index 090a6034d..f6baee909 100644 --- a/src/libs/comm_cpp/src/bumperClient.cpp +++ b/src/libs/comm_cpp/src/bumperClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -BumperClient* +BumperClient* getBumperClient(Comm::Communicator* jdrc, std::string prefix){ BumperClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/cameraClient.cpp b/src/libs/comm_cpp/src/cameraClient.cpp index f52c57de8..3466cbd26 100644 --- a/src/libs/comm_cpp/src/cameraClient.cpp +++ b/src/libs/comm_cpp/src/cameraClient.cpp @@ -24,12 +24,21 @@ namespace Comm { -CameraClient* +CameraClient* getCameraClient(Comm::Communicator* jdrc, std::string prefix){ CameraClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -76,4 +85,4 @@ getCameraClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS diff --git a/src/libs/comm_cpp/src/cmdvelClient.cpp b/src/libs/comm_cpp/src/cmdvelClient.cpp index 43943e5d6..19a916ecc 100644 --- a/src/libs/comm_cpp/src/cmdvelClient.cpp +++ b/src/libs/comm_cpp/src/cmdvelClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -CMDVelClient* +CMDVelClient* getCMDVelClient(Comm::Communicator* jdrc, std::string prefix){ CMDVelClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/laserClient.cpp b/src/libs/comm_cpp/src/laserClient.cpp index 71de12e20..7ea71475c 100644 --- a/src/libs/comm_cpp/src/laserClient.cpp +++ b/src/libs/comm_cpp/src/laserClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -LaserClient* +LaserClient* getLaserClient(Comm::Communicator* jdrc, std::string prefix){ LaserClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/motorsClient.cpp b/src/libs/comm_cpp/src/motorsClient.cpp index 8fc10f160..6081ef874 100644 --- a/src/libs/comm_cpp/src/motorsClient.cpp +++ b/src/libs/comm_cpp/src/motorsClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -MotorsClient* +MotorsClient* getMotorsClient(Comm::Communicator* jdrc, std::string prefix){ MotorsClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/navdataClient.cpp b/src/libs/comm_cpp/src/navdataClient.cpp index 65a1a6029..7ee90d81e 100644 --- a/src/libs/comm_cpp/src/navdataClient.cpp +++ b/src/libs/comm_cpp/src/navdataClient.cpp @@ -6,11 +6,21 @@ namespace Comm { -NavdataClient* +NavdataClient* getNavdataClient(Comm::Communicator* jdrc, std::string prefix){ NavdataClient* client = 0; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + 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; + switch (server){ case 0: { diff --git a/src/libs/comm_cpp/src/pose3dClient.cpp b/src/libs/comm_cpp/src/pose3dClient.cpp index 80c8d2f9b..0e9bd93bb 100644 --- a/src/libs/comm_cpp/src/pose3dClient.cpp +++ b/src/libs/comm_cpp/src/pose3dClient.cpp @@ -6,12 +6,21 @@ namespace Comm { -Pose3dClient* +Pose3dClient* getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ Pose3dClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -57,4 +66,4 @@ getPose3dClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS diff --git a/src/libs/comm_cpp/src/rgbdClient.cpp b/src/libs/comm_cpp/src/rgbdClient.cpp index 9c961809e..6db445d31 100644 --- a/src/libs/comm_cpp/src/rgbdClient.cpp +++ b/src/libs/comm_cpp/src/rgbdClient.cpp @@ -24,12 +24,21 @@ namespace Comm { -RgbdClient* +RgbdClient* getRgbdClient(Comm::Communicator* jdrc, std::string prefix){ RgbdClient* client = 0; + 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; - int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); switch (server){ case 0: { @@ -77,4 +86,4 @@ getRgbdClient(Comm::Communicator* jdrc, std::string prefix){ } -}//NS \ No newline at end of file +}//NS 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 f5b06fa96..71b1073ee 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherArDroneExtra(jdrc, prefix): @return ArDroneExtra ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in index b29238ffb..63b562d8d 100644 --- a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in +++ b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherArDroneExtra(jdrc, prefix): @return ArDroneExtra ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getArDroneExtraClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 9947e1070..dfad02341 100644 --- a/src/libs/comm_py/comm/bumperClient.py.only-ice.in +++ b/src/libs/comm_py/comm/bumperClient.py.only-ice.in @@ -66,8 +66,13 @@ def getBumperClient (jdrc, prefix): @return None if Bumper is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/bumperClient.py.ros.in b/src/libs/comm_py/comm/bumperClient.py.ros.in index e15e7a875..3025b9401 100644 --- a/src/libs/comm_py/comm/bumperClient.py.ros.in +++ b/src/libs/comm_py/comm/bumperClient.py.ros.in @@ -75,8 +75,13 @@ def getBumperClient (jdrc, prefix): @return None if Bumper is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 ba3082568..b245b3e2b 100644 --- a/src/libs/comm_py/comm/cameraClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cameraClient.py.only-ice.in @@ -33,7 +33,7 @@ def __getListenerCamera(jdrc, prefix): @return Camera ROS Subscriber ''' - + print(prefix + ": ROS msg are diabled") return None @@ -66,8 +66,13 @@ def getCameraClient (jdrc, prefix): @return None if Camera is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](jdrc, name) \ No newline at end of file + return cons[server](jdrc, name) diff --git a/src/libs/comm_py/comm/cameraClient.py.ros.in b/src/libs/comm_py/comm/cameraClient.py.ros.in index fda8a419a..e64c18e92 100644 --- a/src/libs/comm_py/comm/cameraClient.py.ros.in +++ b/src/libs/comm_py/comm/cameraClient.py.ros.in @@ -75,8 +75,13 @@ def getCameraClient (jdrc, prefix): @return None if Camera is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 e56668c21..2c6317100 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherCMDVel(jdrc, prefix): @return CMDVel ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/cmdvelClient.py.ros.in b/src/libs/comm_py/comm/cmdvelClient.py.ros.in index 5e9be18c9..6a9af146a 100644 --- a/src/libs/comm_py/comm/cmdvelClient.py.ros.in +++ b/src/libs/comm_py/comm/cmdvelClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherCMDVel(jdrc, prefix): @return CMDVel ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getCMDVelClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 d1e51662e..7f179cd34 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -24,7 +24,7 @@ class Communicator: Communicator constructor @param config: configuration of communicator - + @type config: dict ''' @@ -37,7 +37,7 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": iceserver = True if iceserver: @@ -60,7 +60,7 @@ class Communicator: return self.__ic def getConfig(self): - return self.config + return self.config def getCameraClient(self, name): @@ -68,7 +68,7 @@ class Communicator: Returns a Camera client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -79,7 +79,7 @@ class Communicator: Returns a Motors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -90,7 +90,7 @@ class Communicator: Returns a PTMotors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -101,7 +101,7 @@ class Communicator: Returns a Pose3D client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -112,7 +112,7 @@ class Communicator: Returns a Laser client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -123,7 +123,7 @@ class Communicator: Returns a RGBD client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -134,7 +134,7 @@ class Communicator: Returns a CMDVel client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -145,7 +145,7 @@ class Communicator: Returns a Navdata client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -156,7 +156,7 @@ class Communicator: Returns a ArDroneExtra client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -168,7 +168,7 @@ class Communicator: Returns a Bumper client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -179,9 +179,8 @@ class Communicator: Returns a Sonar client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' return getSonarClient(self, name) - diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in index 34d24af5f..91cb80062 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -25,7 +25,7 @@ class Communicator: Communicator constructor @param config: configuration of communicator - + @type config: dict ''' @@ -38,9 +38,9 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 1: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": iceserver = True - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == 2: + if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ros": rosserver = True if rosserver: @@ -66,8 +66,8 @@ class Communicator: def getIc(self): return self.__ic - def getConfig(self): - return self.config + def getConfig(self): + return self.config def getCameraClient(self, name): @@ -75,7 +75,7 @@ class Communicator: Returns a Camera client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -86,7 +86,7 @@ class Communicator: Returns a Motors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -97,7 +97,7 @@ class Communicator: Returns a Pose3D client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -108,7 +108,7 @@ class Communicator: Returns a Laser client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -120,7 +120,7 @@ class Communicator: Returns a RGBD client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -131,7 +131,7 @@ class Communicator: Returns a CMDVel client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -142,7 +142,7 @@ class Communicator: Returns a Navdata client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -153,7 +153,7 @@ class Communicator: Returns a ArDroneExtra client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -164,7 +164,7 @@ class Communicator: Returns a PTMotors client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -175,7 +175,7 @@ class Communicator: Returns a Bumper client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' @@ -186,9 +186,8 @@ class Communicator: Returns a Sonar client with the configration indicated by the name @param name: name of the client in the config - + @type name: String ''' return getSonarClient(self, name) - 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 83c5e3212..b0dd3fb77 100644 --- a/src/libs/comm_py/comm/laserClient.py.only-ice.in +++ b/src/libs/comm_py/comm/laserClient.py.only-ice.in @@ -66,8 +66,13 @@ def getLaserClient (jdrc, prefix): @return None if Laser is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/laserClient.py.ros.in b/src/libs/comm_py/comm/laserClient.py.ros.in index ef6afbbd3..fcc73bd66 100644 --- a/src/libs/comm_py/comm/laserClient.py.ros.in +++ b/src/libs/comm_py/comm/laserClient.py.ros.in @@ -75,8 +75,13 @@ def getLaserClient (jdrc, prefix): @return None if Laser is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 10c5ece30..e6a33857a 100644 --- a/src/libs/comm_py/comm/motorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/motorsClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherMotors(jdrc, prefix): @return Motors ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -70,8 +70,13 @@ def getMotorsClient (jdrc, prefix): @return None if Motors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/motorsClient.py.ros.in b/src/libs/comm_py/comm/motorsClient.py.ros.in index 022f338bd..7765a4829 100644 --- a/src/libs/comm_py/comm/motorsClient.py.ros.in +++ b/src/libs/comm_py/comm/motorsClient.py.ros.in @@ -45,14 +45,14 @@ def __getPublisherMotors(jdrc, prefix): if not maxW: maxW = 0.5 print (prefix+".maxW not provided, the default value is used: "+ repr(maxW)) - + maxV = jdrc.getConfig().getPropertyWithDefault(prefix+".maxV", 5) if not maxV: maxV = 5 print (prefix+".maxV not provided, the default value is used: "+ repr(maxV)) - + client = PublisherMotors(topic, maxV, maxW) return client else: @@ -90,8 +90,13 @@ def getMotorsClient (jdrc, prefix): @return None if Motors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 f1be2d4c3..a5d59d803 100644 --- a/src/libs/comm_py/comm/navdataClient.py.only-ice.in +++ b/src/libs/comm_py/comm/navdataClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherNavdata(jdrc, prefix): @return Navdata ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -71,9 +71,12 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/navdataClient.py.ros.in b/src/libs/comm_py/comm/navdataClient.py.ros.in index 1501e5661..1ecd6601c 100644 --- a/src/libs/comm_py/comm/navdataClient.py.ros.in +++ b/src/libs/comm_py/comm/navdataClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherNavdata(jdrc, prefix): @return Navdata ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -71,9 +71,12 @@ def getNavdataClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 eb3f657dc..39627f385 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.only-ice.in +++ b/src/libs/comm_py/comm/pose3dClient.py.only-ice.in @@ -67,8 +67,13 @@ def getPose3dClient (jdrc, prefix): @return None if pose3d is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/pose3dClient.py.ros.in b/src/libs/comm_py/comm/pose3dClient.py.ros.in index 35dbc9bd4..71586b449 100644 --- a/src/libs/comm_py/comm/pose3dClient.py.ros.in +++ b/src/libs/comm_py/comm/pose3dClient.py.ros.in @@ -78,8 +78,13 @@ def getPose3dClient (jdrc, prefix): @return None if pose3d is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 c06a304b5..f59a419b4 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.only-ice.in @@ -35,7 +35,7 @@ def __getPublisherPTMotors(jdrc, prefix): @return PTMotors ROS Publisher ''' - + print(prefix + ": ROS msg are diabled") return None @@ -70,8 +70,13 @@ def getPTMotorsClient (jdrc, prefix): @return None if PTMotors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in index 8ac285629..aa05b3fb4 100644 --- a/src/libs/comm_py/comm/ptMotorsClient.py.ros.in +++ b/src/libs/comm_py/comm/ptMotorsClient.py.ros.in @@ -35,7 +35,7 @@ def __getPublisherPTMotors(jdrc, prefix): @return PTMotors ROS Publisher ''' - + print(prefix + ": This Interface doesn't support ROS msg") return None @@ -70,8 +70,13 @@ def getPTMotorsClient (jdrc, prefix): @return None if PTMotors is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__PTMotorsdisabled, __getPTMotorsIceClient, __getPublisherPTMotors] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) 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 b9b34c950..ffaa70d50 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.only-ice.in +++ b/src/libs/comm_py/comm/rgbdClient.py.only-ice.in @@ -69,7 +69,12 @@ def getRgbdClient (jdrc, prefix): @return None if Rgbd is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 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 06a049efc..0ad12e22f 100644 --- a/src/libs/comm_py/comm/rgbdClient.py.ros.in +++ b/src/libs/comm_py/comm/rgbdClient.py.ros.in @@ -78,7 +78,12 @@ def getRgbdClient (jdrc, prefix): @return None if Rgbd is disabled ''' - server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + server = jdrc.getConfig().getProperty(prefix+".Server") + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 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 ed3e42dd8..ba7172253 100644 --- a/src/libs/comm_py/comm/sonarClient.py.only-ice.in +++ b/src/libs/comm_py/comm/sonarClient.py.only-ice.in @@ -67,9 +67,12 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/comm/sonarClient.py.ros.in b/src/libs/comm_py/comm/sonarClient.py.ros.in index 0f2364749..f5f2226f2 100644 --- a/src/libs/comm_py/comm/sonarClient.py.ros.in +++ b/src/libs/comm_py/comm/sonarClient.py.ros.in @@ -34,7 +34,7 @@ def __getListenerSonar(jdrc, prefix): @return Laser ROS Subscriber - ''' + ''' print(prefix + ": This Interface doesn't support ROS msg") return None @@ -68,9 +68,12 @@ def getSonarClient (jdrc, prefix): ''' server = jdrc.getConfig().getProperty(prefix+".Server") - if not server: - server=0 + if server.lower() == "ice": + server=1 + elif server.lower() == "ros": + server=2 + else : server = 0 cons = [__Sonardisabled, __getSonarIceClient, __getListenerSonar] - return cons[server](jdrc, prefix) \ No newline at end of file + return cons[server](jdrc, prefix) diff --git a/src/libs/comm_py/tests/test.yml b/src/libs/comm_py/tests/test.yml index 8c679a42e..5b4b34225 100644 --- a/src/libs/comm_py/tests/test.yml +++ b/src/libs/comm_py/tests/test.yml @@ -1,6 +1,6 @@ Test: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: testMotors @@ -8,55 +8,55 @@ Test: maxW: 0.7 Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" Name: testCamera1 Camera2: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraR:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraR/image_raw" Name: testCamera2 Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "/turtlebotROS/odom" Name: testPose3d Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9001" Topic: "/turtlebotROS/laser/scan" Name: testLaser Sonar: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Sonar:default -h localhost -p 8993" Topic: "/turtlebotROS/laser/scan" Name: testSonar CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:default -h 0.0.0.0 -p 9000" Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Navdata:default -h 0.0.0.0 -p 9000 Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Extra:default -h 0.0.0.0 -p 9000 Bumper: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: Extra:default -h 0.0.0.0 -p 9000 Topic: "/turtlebotROS/mobile_base/events/bumper" Name: testPose3d Vmax: 3 Wmax: 0.7 - NodeName: JdeRobotCommTest \ No newline at end of file + NodeName: JdeRobotCommTest diff --git a/src/libs/config_cpp/src/demo/demo.yml b/src/libs/config_cpp/src/demo/demo.yml index 04a17ada4..725ebb2ec 100644 --- a/src/libs/config_cpp/src/demo/demo.yml +++ b/src/libs/config_cpp/src/demo/demo.yml @@ -1,18 +1,17 @@ Demo: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , 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 + Server: ROS # Deactivate, Ice , 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_py/demo/demo.yml b/src/libs/config_py/demo/demo.yml index abc5b1518..3600abea2 100644 --- a/src/libs/config_py/demo/demo.yml +++ b/src/libs/config_py/demo/demo.yml @@ -1,18 +1,17 @@ Demo: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , 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 + Server: ROS # Deactivate, Ice , 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/tools/cameraview/cameraview.yml b/src/tools/cameraview/cameraview.yml index 794c83f25..9b936ec35 100644 --- a/src/tools/cameraview/cameraview.yml +++ b/src/tools/cameraview/cameraview.yml @@ -1,10 +1,10 @@ Cameraview: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , 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 + NodeName: cameraview diff --git a/src/tools/colorTuner_py/colorTuner_py.yml b/src/tools/colorTuner_py/colorTuner_py.yml index f1167d452..f4a8176c0 100644 --- a/src/tools/colorTuner_py/colorTuner_py.yml +++ b/src/tools/colorTuner_py/colorTuner_py.yml @@ -1,6 +1,6 @@ ColorTuner: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" diff --git a/src/tools/kobukiViewer/kobukiViewer.yml b/src/tools/kobukiViewer/kobukiViewer.yml index 6240df791..95dd26dbb 100644 --- a/src/tools/kobukiViewer/kobukiViewer.yml +++ b/src/tools/kobukiViewer/kobukiViewer.yml @@ -1,6 +1,6 @@ kobukiViewer: Motors: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: kobukiViewerMotors @@ -8,27 +8,27 @@ kobukiViewer: maxW: 0.7 Camera1: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraL/image_raw" Name: kobukiViewerCamera1 Camera2: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "CameraR:default -h localhost -p 9001" Format: RGB8 Topic: "/TurtlebotROS/cameraR/image_raw" Name: kobukiViewerCamera2 Pose3D: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "//turtlebotROS/odom" Name: kobukiViewerPose3d Laser: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9001" Topic: "/turtlebotROS/laser/scan" Name: kobukiViewerLaser diff --git a/src/tools/pantilt_teleop_py/pantilt_teleop.yml b/src/tools/pantilt_teleop_py/pantilt_teleop.yml index 2fd472ba6..42312070e 100644 --- a/src/tools/pantilt_teleop_py/pantilt_teleop.yml +++ b/src/tools/pantilt_teleop_py/pantilt_teleop.yml @@ -1,15 +1,14 @@ pantilt_teleop: PTMotors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS (Not supported) + Server: Ice # Deactivate, Ice , ROS (Not supported) Proxy: "PanTilt:default -h localhost -p 9977" Name: basic_component_pyPTMotors - + Camera: - Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: ROS # Deactivate, Ice , ROS Proxy: "cameraA:default -h localhost -p 9999" Format: RGB8 Topic: "/usb_cam/image_raw" Name: pantilt_teleop_pyCamera NodeName: pantilt_teleop_py - diff --git a/src/tools/rgbdViewer/rgbdViewer.yml b/src/tools/rgbdViewer/rgbdViewer.yml index 0d982c075..166c889f4 100644 --- a/src/tools/rgbdViewer/rgbdViewer.yml +++ b/src/tools/rgbdViewer/rgbdViewer.yml @@ -1,6 +1,6 @@ rgbdViewer: CameraRGB: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 Topic: "/camera/rgb/image_raw" @@ -8,7 +8,7 @@ rgbdViewer: Fps: 30 CameraDEPTH: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice, ROS Proxy: "cameraB:tcp -h localhost -p 9999" Format: RGB8 Topic: "/camera/depth_registered/image_raw" @@ -16,27 +16,27 @@ rgbdViewer: Fps: 30 PointCloud: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "pointcloud1:tcp -h localhost -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: pointcloud Fps: 30 RGBD: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "rgbd1:tcp -h localhost -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: RGBD Fps: 30 Pose3DMotors: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "Pose3DMotors1:tcp -h 193.147.14.20 -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: Pose3DMotors KinectLeds: - Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Deactivate # Deactivate, Ice, ROS Proxy: "kinectleds1:tcp -h 193.147.14.20 -p 9999" Topic: "/TurtlebotROS/cameraL/image_raw" Name: KinectLeds diff --git a/src/tools/scratch2jderobot/cfg/drone.yml b/src/tools/scratch2jderobot/cfg/drone.yml index 57fc7906c..6784a6743 100644 --- a/src/tools/scratch2jderobot/cfg/drone.yml +++ b/src/tools/scratch2jderobot/cfg/drone.yml @@ -1,6 +1,6 @@ drone: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9000" Topic: "/cmd_vel_mux/input/teleop" Name: Motors @@ -8,36 +8,36 @@ drone: maxW: 0.7 Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Camera:default -h localhost -p 9000" Format: RGB8 Topic: "/camera/rgb/image_raw" Name: Camera1 Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9000" Topic: "/odom" Name: Pose3d Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:default -h localhost -p 9000" Topic: "/scan" Name: Laser CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:default -h localhost -p 9000" Name: CMDVel Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Navdata:default -h localhost -p 9000" Name: Navdata Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Extra:default -h localhost -p 9000" Name: Extra diff --git a/src/tools/scratch2jderobot/cfg/robot.yml b/src/tools/scratch2jderobot/cfg/robot.yml index 9d13e29a9..28f8730e4 100644 --- a/src/tools/scratch2jderobot/cfg/robot.yml +++ b/src/tools/scratch2jderobot/cfg/robot.yml @@ -1,6 +1,6 @@ robot: Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9001" Topic: "/mobile_base/commands/velocity" Name: robotMotors @@ -8,19 +8,19 @@ robot: maxV: 4 Laser: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Laser:tcp -h localhost -p 9001" Topic: "/scan" Name: robotLaser Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9001" Topic: "/odom" Name: robotPose3d Camera1: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 Topic: "/cameraL/image_raw" diff --git a/src/tools/uav_viewer_py/uav_viewer_py.yml b/src/tools/uav_viewer_py/uav_viewer_py.yml index 43512936a..b39b9ac44 100644 --- a/src/tools/uav_viewer_py/uav_viewer_py.yml +++ b/src/tools/uav_viewer_py/uav_viewer_py.yml @@ -1,31 +1,31 @@ UAVViewer: Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Camera:default -h localhost -p 9000" Format: RGB8 Topic: "/IntrorobROS/image_raw" Name: UAVViewerCamera Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Pose3D:default -h localhost -p 9000" Topic: "/IntrorobROS/Pose3D" Name: UAVViewerPose3d CMDVel: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "CMDVel:tcp -h localhost -p 9000" Topic: "/IntrorobROS/CMDVel" Name: UAVViewerCMDVel Navdata: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Navdata:tcp -h localhost -p 9000" Topic: "/IntrorobROS/Navdata" Name: UAVViewerNavdata Extra: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Extra:tcp -h localhost -p 9000" Topic: "/IntrorobROS/Extra" Name: UAVViewerExtra @@ -33,6 +33,6 @@ UAVViewer: Xmax: 10 Ymax: 10 Zmax: 5 - Yawmax: 1 + Yawmax: 1 NodeName: UAVViewer From cd45c2a5c164b9f756dba6782ec232a496dc49f9 Mon Sep 17 00:00:00 2001 From: Pushkal Katara Date: Thu, 22 Feb 2018 21:58:29 +0000 Subject: [PATCH 05/10] Update communicator.py.ros.in --- src/libs/comm_py/comm/communicator.py.ros.in | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/libs/comm_py/comm/communicator.py.ros.in b/src/libs/comm_py/comm/communicator.py.ros.in index ec1f464c5..41bec377e 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -38,15 +38,9 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: -<<<<<<< HEAD - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": - iceserver = True - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ros": -======= if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": iceserver = True if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ros": ->>>>>>> 119f7bbc92e1ca685dfb3d7ccccb04fe64727f1c rosserver = True if rosserver: From f902dca7c93be77936c478dcfcd778e09e692540 Mon Sep 17 00:00:00 2001 From: Pushkal Katara Date: Thu, 22 Feb 2018 22:01:34 +0000 Subject: [PATCH 06/10] Update communicator.py.only-ice.in --- src/libs/comm_py/comm/communicator.py.only-ice.in | 4 ---- 1 file changed, 4 deletions(-) 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 48fdc52d0..54ab093fb 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -37,11 +37,7 @@ class Communicator: ymlNode = self.config.getProperty(prefix) for i in ymlNode: -<<<<<<< HEAD - if type(ymlNode[i]) is dict and ymlNode[i]["Server"] == "ice": -======= if type(ymlNode[i]) is dict and ymlNode[i]["Server"].lower() == "ice": ->>>>>>> 119f7bbc92e1ca685dfb3d7ccccb04fe64727f1c iceserver = True if iceserver: From ef270158aaa571c3dff4f225edbaa557c30748ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aitor=20Mart=C3=ADnez=20Fern=C3=A1ndez?= Date: Fri, 23 Feb 2018 19:50:24 +0100 Subject: [PATCH 07/10] Create freenect_ros.yml --- src/tools/rgbdViewer/freenect_ros.yml | 49 +++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 src/tools/rgbdViewer/freenect_ros.yml diff --git a/src/tools/rgbdViewer/freenect_ros.yml b/src/tools/rgbdViewer/freenect_ros.yml new file mode 100644 index 000000000..cc3dfbd08 --- /dev/null +++ b/src/tools/rgbdViewer/freenect_ros.yml @@ -0,0 +1,49 @@ +rgbdViewer: + CameraRGB: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraA:tcp -h localhost -p 9998" + Format: RGB8 + Topic: "/camera/rgb/image_raw" + Name: cameraA + Fps: 30 + + CameraDEPTH: + Server: 2 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraB:tcp -h localhost -p 9998" + Format: RGB8 + Topic: "/camera/depth_registered/sw_registered/image_rect" + Name: cameraB + Fps: 30 + + PointCloud: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "pointcloud1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: pointcloud + Fps: 30 + + RGBD: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "rgbd1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: RGBD + Fps: 30 + + Pose3DMotors: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Pose3DMotors1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: Pose3DMotors + + KinectLeds: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "kinectleds1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: KinectLeds + + + NodeName: rgbdViewer + Width: 640 + Height: 480 + Fps: 15 + Debug: 1 From db683210c1d80bb76b5029ef0bc6b5f2653f59e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aitor=20Mart=C3=ADnez=20Fern=C3=A1ndez?= Date: Mon, 26 Feb 2018 12:07:23 +0100 Subject: [PATCH 08/10] [#1083] fix installations of opel plugin fix #1083 --- src/drivers/gazeboserver/plugins/opel/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gazeboserver/plugins/opel/CMakeLists.txt b/src/drivers/gazeboserver/plugins/opel/CMakeLists.txt index db19c27d6..5c5d09bf1 100644 --- a/src/drivers/gazeboserver/plugins/opel/CMakeLists.txt +++ b/src/drivers/gazeboserver/plugins/opel/CMakeLists.txt @@ -42,7 +42,7 @@ target_link_libraries(camera_dumpopel JderobotInterfaces ) -INSTALL (TARGETS opelMotors DESTINATION share/jderobot/gazebo/plugins/opel/ COMPONENT gazeboserver) +INSTALL (TARGETS opelMotors camera_dumpopel opelPose3D DESTINATION share/jderobot/gazebo/plugins/opel/ COMPONENT gazeboserver) FILE(GLOB_RECURSE CFG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cfg) INSTALL (FILES ${CFG_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf/ COMPONENT gazeboserver) From deb6f1b7e6daadd6d38ddbd4d5b239b04360d40c Mon Sep 17 00:00:00 2001 From: Your Name Date: Mon, 26 Feb 2018 14:02:22 +0100 Subject: [PATCH 09/10] [#1084] solved bug in comm and RGBDViewer related with server selection --- src/libs/comm_cpp/CMakeLists.txt | 2 ++ .../jderobot/comm/ardroneextraClient.hpp | 5 +++ .../include/jderobot/comm/bumperClient.hpp | 5 +++ .../include/jderobot/comm/cameraClient.hpp | 5 +++ .../include/jderobot/comm/cmdvelClient.hpp | 5 +++ .../include/jderobot/comm/communicator.hpp | 1 + .../include/jderobot/comm/laserClient.hpp | 5 +++ .../include/jderobot/comm/motorsClient.hpp | 6 ++++ .../include/jderobot/comm/navdataClient.hpp | 5 +++ .../include/jderobot/comm/pose3dClient.hpp | 5 +++ .../include/jderobot/comm/rgbdClient.hpp | 5 +++ .../comm_cpp/include/jderobot/comm/tools.hpp | 33 +++++++++++++++++++ src/libs/comm_cpp/src/ardroneextraClient.cpp | 14 ++------ src/libs/comm_cpp/src/bumperClient.cpp | 16 ++------- src/libs/comm_cpp/src/cameraClient.cpp | 14 ++------ src/libs/comm_cpp/src/cmdvelClient.cpp | 14 ++------ src/libs/comm_cpp/src/laserClient.cpp | 16 ++------- src/libs/comm_cpp/src/motorsClient.cpp | 13 +------- src/libs/comm_cpp/src/navdataClient.cpp | 15 ++------- src/libs/comm_cpp/src/pose3dClient.cpp | 13 ++------ src/libs/comm_cpp/src/rgbdClient.cpp | 14 ++------ src/libs/comm_cpp/src/tools.cpp | 33 +++++++++++++++++++ src/libs/comm_py/comm/__init__.py | 8 +++-- .../comm/ardroneextraClient.py.only-ice.in | 7 ++-- .../comm_py/comm/ardroneextraClient.py.ros.in | 7 ++-- .../comm_py/comm/bumperClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/bumperClient.py.ros.in | 7 ++-- .../comm_py/comm/cameraClient.py.only-ice.in | 9 ++--- src/libs/comm_py/comm/cameraClient.py.ros.in | 7 ++-- .../comm_py/comm/cmdvelClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/cmdvelClient.py.ros.in | 7 ++-- .../comm_py/comm/communicator.py.only-ice.in | 3 +- src/libs/comm_py/comm/communicator.py.ros.in | 5 +-- .../comm_py/comm/laserClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/laserClient.py.ros.in | 7 ++-- .../comm_py/comm/motorsClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/motorsClient.py.ros.in | 7 ++-- .../comm_py/comm/navdataClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/navdataClient.py.ros.in | 7 ++-- .../comm_py/comm/pose3dClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/pose3dClient.py.ros.in | 7 ++-- .../comm/ptMotorsClient.py.only-ice.in | 7 ++-- .../comm_py/comm/ptMotorsClient.py.ros.in | 7 ++-- .../comm_py/comm/rgbdClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/rgbdClient.py.ros.in | 7 ++-- .../comm_py/comm/sonarClient.py.only-ice.in | 7 ++-- src/libs/comm_py/comm/sonarClient.py.ros.in | 7 ++-- src/libs/comm_py/comm/tools.py | 9 +++++ src/tools/rgbdViewer/rgbdViewer.cpp | 9 +++-- 49 files changed, 205 insertions(+), 229 deletions(-) create mode 100644 src/libs/comm_cpp/include/jderobot/comm/tools.hpp create mode 100644 src/libs/comm_cpp/src/tools.cpp create mode 100644 src/libs/comm_py/comm/tools.py 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"; From 165f7ba450c8daa9a04209af5afb730c8574ad4e Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Tue, 27 Feb 2018 11:42:23 +0100 Subject: [PATCH 10/10] updated install script --- scripts/install_deps.sh | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh index 9c9e92a58..9da8baae7 100755 --- a/scripts/install_deps.sh +++ b/scripts/install_deps.sh @@ -7,7 +7,7 @@ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C sudo apt-get update -sudo apt install -y ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ros-kinetic-opencv3 ros-kinetic-kobuki-gazebo +sudo apt install -y ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ros-kinetic-opencv3 ros-kinetic-kobuki-gazebo ros-kinetic-usb_cam sudo apt-get install -y libpcl-dev @@ -41,7 +41,7 @@ sudo apt-get install -y libdc1394-22 libdc1394-22-dev sudo apt-get install -y libusb-1.0-0 libusb-1.0-0-dev -sudo apt-get install -y python-matplotlib python-pyqt5 python-pip python-numpy python-pyqt5.qtsvg +sudo apt-get install -y python-matplotlib python-pyqt5 python-pip python-numpy python-pyqt5.qtsvg python-pyqt5.qsci sudo apt-get install -y qfi @@ -55,7 +55,7 @@ sudo apt install -y libzeroc-ice3.6 zeroc-ice-utils libzeroc-icestorm3.6 zeroc-i sudo apt-get install -y libssl-dev libbz2-dev sudo pip2 install --upgrade pip -sudo pip2 install zeroc-ice +sudo pip2 install zeroc-ice==3.6.4 sudo apt-get install -y libopenni2-dev libopenni-dev @@ -63,8 +63,6 @@ sudo apt-get install -y gazebo7 libgazebo7-dev sudo apt-get install -y nodejs -sudo apt install -y sophus - sudo apt-get install -y ardronelib libgoogle-glog-dev