From 4e27aa160c9c5ed5de431148d111b45fdd33790f Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Mon, 20 Nov 2017 10:35:33 +0100 Subject: [PATCH] [issue #956] added support for bumper to comm --- src/libs/comm_cpp/CMakeLists.txt | 7 + .../include/jderobot/comm/bumperClient.hpp | 48 +++++++ .../jderobot/comm/ice/bumperIceClient.hpp | 69 +++++++++ .../jderobot/comm/interfaces/bumperClient.hpp | 44 ++++++ .../jderobot/comm/ros/listenerBumper.hpp | 60 ++++++++ .../include/jderobot/comm/ros/translators.hpp | 14 ++ src/libs/comm_cpp/package.xml | 2 + src/libs/comm_cpp/src/bumperClient.cpp | 60 ++++++++ src/libs/comm_cpp/src/ice/bumperIceClient.cpp | 133 ++++++++++++++++++ src/libs/comm_cpp/src/ros/listenerBumper.cpp | 65 +++++++++ src/libs/comm_cpp/src/ros/translators.cpp | 16 +++ .../comm_py/comm/bumperClient.py.only-ice.in | 73 ++++++++++ src/libs/comm_py/comm/bumperClient.py.ros.in | 82 +++++++++++ .../comm_py/comm/communicator.py.only-ice.in | 13 ++ src/libs/comm_py/comm/communicator.py.ros.in | 12 ++ src/libs/comm_py/comm/ice/bumperIceClient.py | 123 ++++++++++++++++ src/libs/comm_py/comm/ros/listenerBumper.py | 96 +++++++++++++ src/libs/comm_py/{ => tests}/test.yml | 16 ++- .../comm_py/{ => tests}/testArDroneExtra.py | 0 src/libs/comm_py/tests/testBumper.py | 29 ++++ src/libs/comm_py/{ => tests}/testCMDVel.py | 0 src/libs/comm_py/{ => tests}/testCamera.py | 0 src/libs/comm_py/{ => tests}/testLaser.py | 0 src/libs/comm_py/{ => tests}/testMotors.py | 0 src/libs/comm_py/{ => tests}/testNavdata.py | 0 src/libs/comm_py/{ => tests}/testPose.py | 0 src/types/cpp/CMakeLists.txt | 1 + .../cpp/include/jderobot/types/bumperData.h | 35 +++++ src/types/python/jderobotTypes/__init__.py | 3 +- src/types/python/jderobotTypes/bumper.py | 32 +++++ 30 files changed, 1027 insertions(+), 6 deletions(-) create mode 100644 src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ice/bumperIceClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/interfaces/bumperClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp create mode 100644 src/libs/comm_cpp/src/bumperClient.cpp create mode 100644 src/libs/comm_cpp/src/ice/bumperIceClient.cpp create mode 100644 src/libs/comm_cpp/src/ros/listenerBumper.cpp create mode 100644 src/libs/comm_py/comm/bumperClient.py.only-ice.in create mode 100644 src/libs/comm_py/comm/bumperClient.py.ros.in create mode 100644 src/libs/comm_py/comm/ice/bumperIceClient.py create mode 100644 src/libs/comm_py/comm/ros/listenerBumper.py rename src/libs/comm_py/{ => tests}/test.yml (73%) rename src/libs/comm_py/{ => tests}/testArDroneExtra.py (100%) create mode 100644 src/libs/comm_py/tests/testBumper.py rename src/libs/comm_py/{ => tests}/testCMDVel.py (100%) rename src/libs/comm_py/{ => tests}/testCamera.py (100%) rename src/libs/comm_py/{ => tests}/testLaser.py (100%) rename src/libs/comm_py/{ => tests}/testMotors.py (100%) rename src/libs/comm_py/{ => tests}/testNavdata.py (100%) rename src/libs/comm_py/{ => tests}/testPose.py (100%) create mode 100644 src/types/cpp/include/jderobot/types/bumperData.h create mode 100644 src/types/python/jderobotTypes/bumper.py diff --git a/src/libs/comm_cpp/CMakeLists.txt b/src/libs/comm_cpp/CMakeLists.txt index 72fd2898e..cea3a46ca 100644 --- a/src/libs/comm_cpp/CMakeLists.txt +++ b/src/libs/comm_cpp/CMakeLists.txt @@ -58,6 +58,9 @@ set(HEADERS include/jderobot/comm/ardroneextraClient.hpp include/jderobot/comm/interfaces/ardroneextraClient.hpp include/jderobot/comm/ice/ardroneextraIceClient.hpp + include/jderobot/comm/bumperClient.hpp + include/jderobot/comm/interfaces/bumperClient.hpp + include/jderobot/comm/ice/bumperIceClient.hpp ) set(SOURCES @@ -78,6 +81,8 @@ set(SOURCES src/ice/ardroneextraIceClient.cpp src/navdataClient.cpp src/ice/navdataIceClient.cpp + src/bumperClient.cpp + src/ice/bumperIceClient.cpp ) @@ -90,6 +95,7 @@ IF(JDEROBOTCOM_ROS) include/jderobot/comm/ros/listenerCamera.hpp include/jderobot/comm/ros/listenerPose.hpp include/jderobot/comm/ros/publisherMotors.hpp + include/jderobot/comm/ros/listenerBumper.hpp ) set(SOURCES @@ -99,6 +105,7 @@ IF(JDEROBOTCOM_ROS) src/ros/listenerCamera.cpp src/ros/listenerPose.cpp src/ros/publisherMotors.cpp + src/ros/listenerBumper.cpp ) ENDIF() diff --git a/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp new file mode 100644 index 000000000..c9c3227a9 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/bumperClient.hpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 1997-2016 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_BUMPERCLIENT_H +#define JDEROBOTCOMM_BUMPERCLIENT_H + +#include +#include +#include +#include + + + + +namespace Comm { + + /** + * @brief make a BumperClient using propierties + * + * + * @param communicator that contains properties + * @param prefix of client Propierties (example: "kobukiViewer.Bumper") + * + * + * @return null if propierties are wrong + */ + BumperClient* getBumperClient(Comm::Communicator* jdrc, std::string prefix); + + +} //NS + +#endif // JDEROBOTCOMM_BUMPERCLIENT_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ice/bumperIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/bumperIceClient.hpp new file mode 100644 index 000000000..ed511a708 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/bumperIceClient.hpp @@ -0,0 +1,69 @@ +/* + * Copyright (C) 1997-2013 JDE Developers TeamkinectViewer.camRGB + * + * 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/. + * + * Author : Jose María Cañas + Francisco Miguel Rivas Montero + + */ + +#ifndef JDEROBOTCOMM_BUMPERICECLIENT_H_ +#define JDEROBOTCOMM_BUMPERICECLIENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Comm { + + +class BumperIceClient: public IceUtil::Thread, public Comm::BumperClient { +public: + BumperIceClient(Comm::Communicator* jdrc, std::string prefix); + virtual ~BumperIceClient(); + virtual void run(); + + virtual JdeRobotTypes::BumperData getBumperData(); + int getRefreshRate(){return refreshRate;}; + void pause(); + void resume(); + bool getPause(){return pauseStatus;}; + + + +private: + std::string prefix; + jderobot::BumperPrx prx; + long long int cycle; + IceUtil::Mutex controlMutex; + bool debug; + bool _done; + int refreshRate; + bool pauseStatus; + + IceUtil::Cond sem; + +}; + + +} /* namespace jderobot */ +#endif /* JDEROBOTCOMM_BUMPERICECLIENT_H_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/interfaces/bumperClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/bumperClient.hpp new file mode 100644 index 000000000..41d2c01a0 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/bumperClient.hpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 1997-2016 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_BUMPERCLIENT_INTERFACE_H +#define JDEROBOTCOMM_BUMPERCLIENT_INTERFACE_H + +#include + + +namespace Comm { + + /** + * @brief BumperClient class. + * This class is a Interface to seprate communications from tools. + * With this, the tools don't need know which communicator (ROS or ICE) are using because both use the same interface. + * + */ + class BumperClient { + public: + virtual JdeRobotTypes::BumperData getBumperData() = 0; + bool on = false; + protected: + JdeRobotTypes::BumperData bumperData; + }; + +} //NS + +#endif // JDEROBOTCOMM_BUMPERCLIENT_INTERFACE_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp new file mode 100644 index 000000000..4c0cb32d8 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/listenerBumper.hpp @@ -0,0 +1,60 @@ +/* + * Copyright (C) 1997-2016 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_LISTENERBUMPER_H_ +#define JDEROBOTCOMM_LISTENERBUMPER_H_ + + +#include +#include +#include +#include +#include +#include + +namespace Comm { + class ListenerBumper: public Comm::BumperClient { + + public: + ListenerBumper(int argc, char** argv, std::string nodeName, std::string topic); + ~ListenerBumper(); + + void start(); + void stop(); + virtual JdeRobotTypes::BumperData getBumperData(); + + + private: + pthread_mutex_t mutex; + ros::Subscriber sub; + std::string topic; + std::string nodeName; + + ros::AsyncSpinner* spinner; + + void bumpercallback (const kobuki_msgs::BumperEventConstPtr& bumper_msg); + + + + + + };//class + +} //NS +#endif /* JDEROBOTCOMM_LISTENERBUMPER_H_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp b/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp index 1342a451b..19b96d09b 100644 --- a/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp +++ b/src/libs/comm_cpp/include/jderobot/comm/ros/translators.hpp @@ -41,6 +41,9 @@ #include #include +#include +#include + namespace Comm { @@ -55,6 +58,17 @@ namespace Comm { */ JdeRobotTypes::LaserData translate_laser_messages(const sensor_msgs::LaserScanConstPtr& scan); + /** + * @brief translate ROS BumperEvent messages to JdeRobot BumperData + * + * + * @param ROS kobuki Bumper Event Message + * + * + * @return BumperData translated from ROS Message + */ + JdeRobotTypes::BumperData translate_bumper_messages(const kobuki_msgs::BumperEventConstPtr& bump); + /** * @brief translate ROS Image messages to JdeRobot Image * diff --git a/src/libs/comm_cpp/package.xml b/src/libs/comm_cpp/package.xml index 0bdcef583..bc4f52fa3 100644 --- a/src/libs/comm_cpp/package.xml +++ b/src/libs/comm_cpp/package.xml @@ -46,6 +46,7 @@ image_transport nav_msgs geometry_msgs + kobuki_msgs message_runtime roscpp @@ -54,6 +55,7 @@ image_transport nav_msgs geometry_msgs + kobuki_msgs diff --git a/src/libs/comm_cpp/src/bumperClient.cpp b/src/libs/comm_cpp/src/bumperClient.cpp new file mode 100644 index 000000000..090a6034d --- /dev/null +++ b/src/libs/comm_cpp/src/bumperClient.cpp @@ -0,0 +1,60 @@ +#include +#include +#ifdef JDERROS +#include +#endif + +namespace Comm { + +BumperClient* +getBumperClient(Comm::Communicator* jdrc, std::string prefix){ + BumperClient* client = 0; + + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + switch (server){ + case 0: + { + std::cout << "Bumper disabled" << std::endl; + break; + } + case 1: + { + std::cout << "Receiving BumperData from ICE interfaces" << std::endl; + BumperIceClient* cl; + cl = new BumperIceClient(jdrc, prefix); + cl->start(); + client = (Comm::BumperClient*) cl; + break; + } + case 2: + { + #ifdef JDERROS + std::cout << "Receiving BumperData from ROS messages" << std::endl; + std::string nodeName; + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "BumperNode"); + std::string topic; + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); + ListenerBumper* lc; + lc = new ListenerBumper(0, nullptr, nodeName, topic); + lc->start(); + client = (Comm::BumperClient*) lc; + #else + throw "ERROR: ROS is not available"; + #endif + + break; + } + default: + { + std::cerr << "Wrong " + prefix+".Server property" << std::endl; + break; + } + + } + + return client; + + +} + +}//NS diff --git a/src/libs/comm_cpp/src/ice/bumperIceClient.cpp b/src/libs/comm_cpp/src/ice/bumperIceClient.cpp new file mode 100644 index 000000000..fadd6929f --- /dev/null +++ b/src/libs/comm_cpp/src/ice/bumperIceClient.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (C) 1997-2013 JDE Developers TeamkinectViewer.camRGB + * + * 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/. + * + * Author : Jose María Cañas + Francisco Miguel Rivas Montero + + */ +#include "jderobot/comm/ice/bumperIceClient.hpp" + +namespace Comm { + +BumperIceClient::BumperIceClient(Comm::Communicator* jdrc, std::string prefix) { + + this->prefix=prefix; + + this->refreshRate=0; + float fps=jdrc->getConfig().asFloatWithDefault(prefix+".Fps",10); + this->cycle=(1/fps)*1000000; + + + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx baseBumper = jdrc->getIceComm()->stringToProxy(proxy); + + if (0==baseBumper){ + this->on = false; + std::cout << prefix + ".Proxy configuration not specified" <prx = jderobot::BumperPrx::checkedCast(baseBumper); + + if (0 == this->prx){ + this->on = false; + std::cout <<"Invalid proxy "+ prefix + ".Proxy" <on = true; + std::cout << prefix + " connected" << std::endl; + } + + + }catch (const Ice::ConnectionRefusedException& e) { + std::cout << prefix +" inactive" << std::endl; + } + catch (const Ice::Exception& ex) { + std::cerr << ex << std::endl; + } + } + + this->pauseStatus=false; + +} + +BumperIceClient::~BumperIceClient() { + this->on=false; +} + + +void BumperIceClient::pause(){ + std::cout << "Paused" << std::endl; + this->pauseStatus=true; +} + +void BumperIceClient::resume(){ + std::cout << "resumed" << std::endl; + this->controlMutex.lock(); + this->pauseStatus=false; + this->sem.broadcast(); + this->controlMutex.unlock(); +} + +void BumperIceClient::run(){ + + IceUtil::Time last; + + last=IceUtil::Time::now(); + while (this->on){ + if (pauseStatus){ + IceUtil::Mutex::Lock sync(this->controlMutex); + this->sem.wait(sync); + } + + try{ + jderobot::BumperDataPtr localBumper=this->prx->getBumperData(); + + this->controlMutex.lock(); + this->bumperData.state = localBumper->state; + this->bumperData.bumper = localBumper->bumper; + this->controlMutex.unlock(); + + + } + catch(...){ + LOG(WARNING) < this->cycle ){ + DLOG(WARNING) <cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds())); + } + this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds())); + last=IceUtil::Time::now(); + } +} + +JdeRobotTypes::BumperData BumperIceClient::getBumperData(){ + JdeRobotTypes::BumperData bumper; + this->controlMutex.lock(); + bumper = this->bumperData; + this->controlMutex.unlock(); + return bumper; +} + +} /* NS */ \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ros/listenerBumper.cpp b/src/libs/comm_cpp/src/ros/listenerBumper.cpp new file mode 100644 index 000000000..bdf21b001 --- /dev/null +++ b/src/libs/comm_cpp/src/ros/listenerBumper.cpp @@ -0,0 +1,65 @@ +#include + +namespace Comm { + + ListenerBumper::ListenerBumper(int argc, char** argv, std::string nodeName, std::string topic){ + pthread_mutex_init(&mutex, NULL); + if ("" == topic){ + this->on = false; + std::cerr <<"Invalid bumper topic" <on = true; + this->topic = topic; + this->nodeName = nodeName; + + const std::string name = std::string(this->nodeName); + int a = 0; + ros::init(a, nullptr, name); + ros::NodeHandle nh; + this->sub = nh.subscribe(this->topic, 1001, &ListenerBumper::bumpercallback, this); + std::cout << "listen from "+ this->topic << std::endl; + + this->spinner = new ros::AsyncSpinner(1); + + + } + } + + + + ListenerBumper::~ListenerBumper(){ + this->stop(); + } + + void + ListenerBumper::start(){ + this->spinner->start(); + + } + + void + ListenerBumper::stop(){ + this->spinner->stop(); + ros::shutdown(); + } + + void + ListenerBumper::bumpercallback(const kobuki_msgs::BumperEventConstPtr& bumper_msg){ + pthread_mutex_lock(&mutex); + this->bumperData = Comm::translate_bumper_messages(bumper_msg); + pthread_mutex_unlock(&mutex); + + } + + JdeRobotTypes::BumperData + ListenerBumper::getBumperData(){ + JdeRobotTypes::BumperData ld; + pthread_mutex_lock(&mutex); + ld = this->bumperData; + pthread_mutex_unlock(&mutex); + return ld; + } + + + +}//NS \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ros/translators.cpp b/src/libs/comm_cpp/src/ros/translators.cpp index 2767f863e..672058fbf 100644 --- a/src/libs/comm_cpp/src/ros/translators.cpp +++ b/src/libs/comm_cpp/src/ros/translators.cpp @@ -47,6 +47,22 @@ namespace Comm { } + JdeRobotTypes::BumperData + translate_bumper_messages(const kobuki_msgs::BumperEventConstPtr& bump) + { + JdeRobotTypes::BumperData data; + + data.state = bump->state; + data.bumper = bump->bumper; + + //data.timeStamp = scan->header.stamp.sec + (scan->header.stamp.nsec *1e-9); + + + return data; + + } + + JdeRobotTypes::Image diff --git a/src/libs/comm_py/comm/bumperClient.py.only-ice.in b/src/libs/comm_py/comm/bumperClient.py.only-ice.in new file mode 100644 index 000000000..9947e1070 --- /dev/null +++ b/src/libs/comm_py/comm/bumperClient.py.only-ice.in @@ -0,0 +1,73 @@ +import sys +import Ice +from .ice.bumperIceClient import BumperIceClient + + +def __getBumperIceClient(jdrc, prefix): + ''' + Returns a Bumper Ice Client. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Bumper Ice Client + + ''' + print("Receiving " + prefix + " BumperData from ICE interfaces") + client = BumperIceClient(jdrc, prefix) + client.start() + return client + +def __getListenerBumper(jdrc, prefix): + ''' + Returns a Bumper ROS Subscriber. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Bumper ROS Subscriber + + ''' + print(prefix + ": ROS msg are diabled") + return None + +def __Bumperdisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return None + + ''' + print( prefix + " Disabled") + return None + +def getBumperClient (jdrc, prefix): + ''' + Returns a Bumper Client. + + @param jdrc: Comm Communicator + @param name: name of client in config file + + @type jdrc: Comm Communicator + @type name: String + + @return None if Bumper is disabled + + ''' + server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + + cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/bumperClient.py.ros.in b/src/libs/comm_py/comm/bumperClient.py.ros.in new file mode 100644 index 000000000..e15e7a875 --- /dev/null +++ b/src/libs/comm_py/comm/bumperClient.py.ros.in @@ -0,0 +1,82 @@ +import sys +import Ice +import rospy +from .ice.bumperIceClient import BumperIceClient + +if (sys.version_info[0] == 2): + from .ros.listenerBumper import ListenerBumper + +def __getBumperIceClient(jdrc, prefix): + ''' + Returns a Bumper Ice Client. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Bumper Ice Client + + ''' + print("Receiving " + prefix + " BumperData from ICE interfaces") + client = BumperIceClient(jdrc, prefix) + client.start() + return client + +def __getListenerBumper(jdrc, prefix): + ''' + Returns a Bumper ROS Subscriber. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Bumper ROS Subscriber + + ''' + if (sys.version_info[0] == 2): + print("Receiving " + prefix + " BumperData from ROS messages") + topic = jdrc.getConfig().getProperty(prefix+".Topic") + client = ListenerBumper(topic) + return client + else: + print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) + return None + +def __Bumperdisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getBumperClient instead of this + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return None + + ''' + print( prefix + " Disabled") + return None + +def getBumperClient (jdrc, prefix): + ''' + Returns a Bumper Client. + + @param jdrc: Comm Communicator + @param prefix: name of client in config file + + @type jdrc: Comm Communicator + @type name: String + + @return None if Bumper is disabled + + ''' + server = jdrc.getConfig().getPropertyWithDefault(prefix+".Server", 0) + + cons = [__Bumperdisabled, __getBumperIceClient, __getListenerBumper] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/communicator.py.only-ice.in b/src/libs/comm_py/comm/communicator.py.only-ice.in index b0f83a488..4d5ecc608 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -9,6 +9,7 @@ from .rgbdClient import getRgbdClient from .ardroneextraClient import getArDroneExtraClient from .navdataClient import getNavdataClient from .cmdvelClient import getCMDVelClient +from .bumperClient import getBumperClient class Communicator: @@ -159,3 +160,15 @@ class Communicator: ''' return getArDroneExtraClient(self, name) + + def getBumperClient(self, name): + ''' + Returns a Bumper client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getBumperClient(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 17eb0dfab..21b18a4a5 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -10,6 +10,7 @@ from .ardroneextraClient import getArDroneExtraClient from .navdataClient import getNavdataClient from .cmdvelClient import getCMDVelClient from .ptMotorsClient import getPTMotorsClient +from .bumperClient import getBumperClient class Communicator: @@ -167,3 +168,14 @@ class Communicator: ''' return getPTMotorsClient(self, name) + def getBumperClient(self, name): + ''' + Returns a Bumper client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getBumperClient(self, name) + diff --git a/src/libs/comm_py/comm/ice/bumperIceClient.py b/src/libs/comm_py/comm/ice/bumperIceClient.py new file mode 100644 index 000000000..cb63fcdee --- /dev/null +++ b/src/libs/comm_py/comm/ice/bumperIceClient.py @@ -0,0 +1,123 @@ +# +# 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 +# + +import traceback +import jderobot +import threading +import Ice +from .threadSensor import ThreadSensor +from jderobotTypes import BumperData + +class Bumper: + + def __init__(self, jdrc, prefix): + ''' + Bumper Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + ''' + + self.lock = threading.Lock() + try: + + ic = jdrc.getIc() + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) + self.proxy = jderobot.BumperPrx.checkedCast(base) + + self.update() + + if not self.proxy: + print ('Interface ' + prefix + ' not configured') + + except Ice.ConnectionRefusedException: + print(prefix + ': connection refused') + + except: + traceback.print_exc() + exit(-1) + + def update(self): + if hasattr(self,"proxy") and self.proxy: + bumper = self.proxy.getBumperData() + + self.lock.acquire() + self.bumper = bumper + self.lock.release() + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return hasattr(self,"proxy") and self.proxy + + def getBumper(self): + ''' + Returns last Bumper. + + @return last JdeRobotTypes Bumper saved + + ''' + if self.hasproxy(): + self.lock.acquire() + bumper = self.bumper + self.lock.release() + return bumper + + return None + +class BumperIceClient: + def __init__(self,jdrc,prefix, start = False): + self.bumper = Bumper(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.bumper, self.kill_event) + self.thread.daemon = True + + if start: + self.start() + + # if client is stopped you can not start again, Threading.Thread raised error + def start(self): + self.kill_event.clear() + self.thread.start() + + # if client is stopped you can not start again + def stop(self): + self.kill_event.set() + + def getBumperData(self): + return self.bumper.getBumper() + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return self.bumper.hasproxy() diff --git a/src/libs/comm_py/comm/ros/listenerBumper.py b/src/libs/comm_py/comm/ros/listenerBumper.py new file mode 100644 index 000000000..b8bdc90ac --- /dev/null +++ b/src/libs/comm_py/comm/ros/listenerBumper.py @@ -0,0 +1,96 @@ +import rospy +from kobuki_msgs.msg import BumperEvent +import threading +from jderobotTypes import BumperData + + + +def bumperEvent2BumperData(event): + ''' + Translates from ROS BumperScan to JderobotTypes BumperData. + + @param event: ROS BumperScan to translate + + @type event: BumperScan + + @return a BumperData translated from event + + # bumper + LEFT = 0 + CENTER = 1 + RIGHT = 2 + + # state + RELEASED = 0 + PRESSED = 1 + + ''' + bump = BumperData() + bump.state = event.state + bump.bumper = event.bumper + + #bump.timeStamp = event.header.stamp.secs + (event.header.stamp.nsecs *1e-9) + return bump + +class ListenerBumper: + ''' + ROS Bumper Subscriber. Bumper Client to Receive Bumper Scans from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerBumper Constructor. + + @param topic: ROS topic to subscribe + + @type topic: String + + ''' + self.topic = topic + self.data = BumperData() + self.sub = None + self.lock = threading.Lock() + self.start() + + def __callback (self, event): + ''' + Callback function to receive and save Bumper Scans. + + @param event: ROS BumperScan received + + @type event: BumperScan + + ''' + bump = bumperEvent2BumperData(event) + + self.lock.acquire() + self.data = bump + self.lock.release() + + def stop(self): + ''' + Stops (Unregisters) the client. + + ''' + self.sub.unregister() + + def start (self): + ''' + Starts (Subscribes) the client. + + ''' + self.sub = rospy.Subscriber(self.topic, BumperEvent, self.__callback) + + def getBumperData(self): + ''' + Returns last BumperData. + + @return last JdeRobotTypes BumperData saved + + ''' + self.lock.acquire() + bump = self.data + self.lock.release() + + return bump + + diff --git a/src/libs/comm_py/test.yml b/src/libs/comm_py/tests/test.yml similarity index 73% rename from src/libs/comm_py/test.yml rename to src/libs/comm_py/tests/test.yml index 548bb8c6d..b16362264 100644 --- a/src/libs/comm_py/test.yml +++ b/src/libs/comm_py/tests/test.yml @@ -2,7 +2,7 @@ Test: Motors: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "Motors:default -h localhost -p 9001" - Topic: "/cmd_vel_mux/input/teleop" + Topic: "/turtlebotROS/mobile_base/commands/velocity" Name: testMotors maxV: 3 maxW: 0.7 @@ -11,26 +11,26 @@ Test: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "CameraL:default -h localhost -p 9001" Format: RGB8 - Topic: "/camera/rgb/image_raw" + Topic: "/TurtlebotROS/cameraL/image_raw" Name: testCamera1 Camera2: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "CameraR:default -h localhost -p 9001" Format: RGB8 - Topic: "/camera/rgb/image_raw" + Topic: "/TurtlebotROS/cameraR/image_raw" Name: testCamera2 Pose3D: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "Pose3D:default -h localhost -p 9001" - Topic: "/odom" + Topic: "/turtlebotROS/odom" Name: testPose3d Laser: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "Laser:default -h localhost -p 9001" - Topic: "/scan" + Topic: "/turtlebotROS/laser/scan" Name: testLaser CMDVel: @@ -45,6 +45,12 @@ Test: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: Extra:default -h 0.0.0.0 -p 9000 + Bumper: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> 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 diff --git a/src/libs/comm_py/testArDroneExtra.py b/src/libs/comm_py/tests/testArDroneExtra.py similarity index 100% rename from src/libs/comm_py/testArDroneExtra.py rename to src/libs/comm_py/tests/testArDroneExtra.py diff --git a/src/libs/comm_py/tests/testBumper.py b/src/libs/comm_py/tests/testBumper.py new file mode 100644 index 000000000..0866ea706 --- /dev/null +++ b/src/libs/comm_py/tests/testBumper.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python2 +import config +import comm +import sys +import time +import signal + +from jderobotTypes import BumperData + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + client = jdrc.getBumperClient("Test.Bumper") + + while True: + #print("client1", end=":") + bumper = client.getBumperData() + print bumper + if bumper.state == 1: + print bumper + break + time.sleep(1) + + jdrc.destroy() \ No newline at end of file diff --git a/src/libs/comm_py/testCMDVel.py b/src/libs/comm_py/tests/testCMDVel.py similarity index 100% rename from src/libs/comm_py/testCMDVel.py rename to src/libs/comm_py/tests/testCMDVel.py diff --git a/src/libs/comm_py/testCamera.py b/src/libs/comm_py/tests/testCamera.py similarity index 100% rename from src/libs/comm_py/testCamera.py rename to src/libs/comm_py/tests/testCamera.py diff --git a/src/libs/comm_py/testLaser.py b/src/libs/comm_py/tests/testLaser.py similarity index 100% rename from src/libs/comm_py/testLaser.py rename to src/libs/comm_py/tests/testLaser.py diff --git a/src/libs/comm_py/testMotors.py b/src/libs/comm_py/tests/testMotors.py similarity index 100% rename from src/libs/comm_py/testMotors.py rename to src/libs/comm_py/tests/testMotors.py diff --git a/src/libs/comm_py/testNavdata.py b/src/libs/comm_py/tests/testNavdata.py similarity index 100% rename from src/libs/comm_py/testNavdata.py rename to src/libs/comm_py/tests/testNavdata.py diff --git a/src/libs/comm_py/testPose.py b/src/libs/comm_py/tests/testPose.py similarity index 100% rename from src/libs/comm_py/testPose.py rename to src/libs/comm_py/tests/testPose.py diff --git a/src/types/cpp/CMakeLists.txt b/src/types/cpp/CMakeLists.txt index c6fe0ae10..6a48fc5d6 100644 --- a/src/types/cpp/CMakeLists.txt +++ b/src/types/cpp/CMakeLists.txt @@ -14,6 +14,7 @@ set(HEADERS include/jderobot/types/rgbd.h include/jderobot/types/navdataData.h include/jderobot/types/cmdvel.h + include/jderobot/types/bumperData.h ) diff --git a/src/types/cpp/include/jderobot/types/bumperData.h b/src/types/cpp/include/jderobot/types/bumperData.h new file mode 100644 index 000000000..ae9edfbd6 --- /dev/null +++ b/src/types/cpp/include/jderobot/types/bumperData.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 1997-2016 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTTYPES_BUMPERDATA_H +#define JDEROBOTTYPES_BUMPERDATA_H + +namespace JdeRobotTypes { + + class BumperData { + public: + int state = 0; // pressed or no (1,0) + int bumper = 0; // Indicates that bumper is + double timeStamp = 0; //seconds + }; + + +} //NS + +#endif // JDEROBOTTYPES_BUMPERDATA_H \ No newline at end of file diff --git a/src/types/python/jderobotTypes/__init__.py b/src/types/python/jderobotTypes/__init__.py index 3bc41ac81..485bb014a 100644 --- a/src/types/python/jderobotTypes/__init__.py +++ b/src/types/python/jderobotTypes/__init__.py @@ -3,4 +3,5 @@ from .pose3d import Pose3d from .cmdvel import CMDVel from .rgbd import Rgbd -from .navdataData import NavdataData \ No newline at end of file +from .navdataData import NavdataData +from .bumper import BumperData \ No newline at end of file diff --git a/src/types/python/jderobotTypes/bumper.py b/src/types/python/jderobotTypes/bumper.py new file mode 100644 index 000000000..e4480ae55 --- /dev/null +++ b/src/types/python/jderobotTypes/bumper.py @@ -0,0 +1,32 @@ +# +# 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 +# + +class BumperData (): + + def __init__(self): + + self.bumper = 0 # Indicates that bumper is + self.state = 0 # pressed or no (1,0) + self.timeStamp = 0 # Time stamp [s] + + + def __str__(self): + s = "BumperData: {\n bumper: " + str(self.bumper) + "\n state: " + str(self.state) + s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" + return s \ No newline at end of file