From 5bd225485ecba3bb5200aec2144fb910971fd07b Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Thu, 19 Oct 2017 11:12:24 +0200 Subject: [PATCH 1/3] added drone interfaces to comm_cpp --- src/libs/comm_cpp/CMakeLists.txt | 15 ++ .../jderobot/comm/ardroneextraClient.hpp | 49 +++++ .../include/jderobot/comm/cmdvelClient.hpp | 49 +++++ .../comm/ice/ardroneextraIceClient.hpp | 59 ++++++ .../jderobot/comm/ice/cmdvelIceClient.hpp | 53 ++++++ .../jderobot/comm/ice/navdataIceClient.hpp | 70 ++++++++ .../comm/interfaces/ardroneextraClient.hpp | 49 +++++ .../jderobot/comm/interfaces/cmdvelClient.hpp | 44 +++++ .../comm/interfaces/navdataClient.hpp | 44 +++++ .../include/jderobot/comm/navdataClient.hpp | 48 +++++ src/libs/comm_cpp/src/ardroneextraClient.cpp | 61 +++++++ src/libs/comm_cpp/src/cmdvelClient.cpp | 61 +++++++ .../src/ice/ardroneextraIceClient.cpp | 114 ++++++++++++ src/libs/comm_cpp/src/ice/cmdvelIceClient.cpp | 81 +++++++++ .../comm_cpp/src/ice/navdataIceClient.cpp | 168 ++++++++++++++++++ src/libs/comm_cpp/src/navdataClient.cpp | 62 +++++++ src/types/cpp/CMakeLists.txt | 18 +- .../cpp/include/jderobot/types/navdataData.h | 83 +++++++++ 18 files changed, 1113 insertions(+), 15 deletions(-) create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ice/ardroneextraIceClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ice/cmdvelIceClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/ice/navdataIceClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/interfaces/ardroneextraClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/interfaces/cmdvelClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/interfaces/navdataClient.hpp create mode 100644 src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp create mode 100644 src/libs/comm_cpp/src/ardroneextraClient.cpp create mode 100644 src/libs/comm_cpp/src/cmdvelClient.cpp create mode 100644 src/libs/comm_cpp/src/ice/ardroneextraIceClient.cpp create mode 100644 src/libs/comm_cpp/src/ice/cmdvelIceClient.cpp create mode 100644 src/libs/comm_cpp/src/ice/navdataIceClient.cpp create mode 100644 src/libs/comm_cpp/src/navdataClient.cpp create mode 100644 src/types/cpp/include/jderobot/types/navdataData.h diff --git a/src/libs/comm_cpp/CMakeLists.txt b/src/libs/comm_cpp/CMakeLists.txt index c2304e74d..72fd2898e 100644 --- a/src/libs/comm_cpp/CMakeLists.txt +++ b/src/libs/comm_cpp/CMakeLists.txt @@ -49,6 +49,15 @@ set(HEADERS include/jderobot/comm/rgbdClient.hpp include/jderobot/comm/interfaces/rgbdClient.hpp include/jderobot/comm/ice/rgbdIceClient.hpp + include/jderobot/comm/cmdvelClient.hpp + include/jderobot/comm/interfaces/cmdvelClient.hpp + include/jderobot/comm/ice/cmdvelIceClient.hpp + include/jderobot/comm/navdataClient.hpp + include/jderobot/comm/interfaces/navdataClient.hpp + include/jderobot/comm/ice/navdataIceClient.hpp + include/jderobot/comm/ardroneextraClient.hpp + include/jderobot/comm/interfaces/ardroneextraClient.hpp + include/jderobot/comm/ice/ardroneextraIceClient.hpp ) set(SOURCES @@ -63,6 +72,12 @@ set(SOURCES src/ice/motorsIceClient.cpp src/rgbdClient.cpp src/ice/rgbdIceClient.cpp + src/cmdvelClient.cpp + src/ice/cmdvelIceClient.cpp + src/ardroneextraClient.cpp + src/ice/ardroneextraIceClient.cpp + src/navdataClient.cpp + src/ice/navdataIceClient.cpp ) diff --git a/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp new file mode 100644 index 000000000..46a7a786e --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ardroneextraClient.hpp @@ -0,0 +1,49 @@ +/* + * 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_ARDRONEEXTRACLIENT_H +#define JDEROBOTCOMM_ARDRONEEXTRACLIENT_H + + +#include +#include +#include + + + + + +namespace Comm { + + /** + * @brief make a ArDroneExtraClient using propierties + * + * + * @param communicator that contains properties + * @param prefix of client Propierties (example: "Uav_viewer.ArDroneExtra") + * + * + * @return null if propierties are wrong + */ + ArDroneExtraClient* getArDroneExtraClient(Comm::Communicator* jdrc, std::string prefix); + + +} //NS + +#endif // JDEROBOTCOMM_ARDRONEEXTRACLIENT_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp new file mode 100644 index 000000000..fd9d078dc --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/cmdvelClient.hpp @@ -0,0 +1,49 @@ +/* + * 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_CMDVELCLIENT_H +#define JDEROBOTCOMM_CMDVELCLIENT_H + +#include +#include +#include +#include + + + + + +namespace Comm { + + /** + * @brief make a CMDVelClient using propierties + * + * + * @param communicator that contains properties + * @param prefix of client Propierties (example: "Uav_viewer.CMDVel") + * + * + * @return null if propierties are wrong + */ + CMDVelClient* getCMDVelClient(Comm::Communicator* jdrc, std::string prefix); + + +} //NS + +#endif // JDEROBOTCOMM_CMDVELCLIENT_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ice/ardroneextraIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/ardroneextraIceClient.hpp new file mode 100644 index 000000000..be996cf8a --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/ardroneextraIceClient.hpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 1997-2017 JDE Developers + * + * 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 : Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_ARDRONEEXTRAICECLIENT_H_ +#define JDEROBOTCOMM_ARDRONEEXTRAICECLIENT_H_ + +#include +#include +#include +#include +#include +#include + +namespace Comm { + + +class ArDroneExtraIceClient: public Comm::ArDroneExtraClient { +public: + ArDroneExtraIceClient(Comm::Communicator* jdrc, std::string prefix); + virtual ~ArDroneExtraIceClient(); + + virtual void toggleCam(); + virtual void land(); + virtual void takeoff(); + virtual void reset(); + virtual void recordOnUsb(bool record); + virtual void ledAnimation(int type,float duration, float req); + virtual void flightAnimation(int type, float duration); + virtual void flatTrim(); + + + +private: + std::string prefix; + jderobot::ArDroneExtraPrx prx; + IceUtil::Mutex controlMutex; + + +}; + + +} /* namespace jderobotcomm */ +#endif /* JDEROBOTCOMM_ARDRONEEXTRAICECLIENT_H_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ice/cmdvelIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/cmdvelIceClient.hpp new file mode 100644 index 000000000..3afdb4280 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/cmdvelIceClient.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 1997-2017 JDE Developers + * + * 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 : Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_CMDVELSICECLIENT_H_ +#define JDEROBOTCOMM_CMDVELSICECLIENT_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace Comm { + + +class CMDVelIceClient: public Comm::CMDVelClient { +public: + CMDVelIceClient(Comm::Communicator* jdrc, std::string prefix); + virtual ~CMDVelIceClient(); + + virtual void sendVelocities(JdeRobotTypes::CMDVel vel); + + + +private: + std::string prefix; + jderobot::CMDVelPrx prx; + IceUtil::Mutex controlMutex; + + +}; + + +} /* namespace jderobotcomm */ +#endif /* JDEROBOTCOMM_CMDVELSICECLIENT_H_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/ice/navdataIceClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/ice/navdataIceClient.hpp new file mode 100644 index 000000000..281ca86ff --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/ice/navdataIceClient.hpp @@ -0,0 +1,70 @@ +/* + * 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_NAVDATAICECLIENT_H_ +#define JDEROBOTCOMM_NAVDATAICECLIENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Comm { + + +class NavdataIceClient: public IceUtil::Thread, public Comm::NavdataClient { +public: + NavdataIceClient(Comm::Communicator* jdrc, std::string prefix); + virtual ~NavdataIceClient(); + virtual void run(); + + virtual JdeRobotTypes::NavdataData getNavdataData(); + int getRefreshRate(){return refreshRate;}; + void pause(); + void resume(); + bool getPause(){return pauseStatus;}; + + + +private: + std::string prefix; + jderobot::NavdataPrx prx; + long long int cycle; + IceUtil::Mutex controlMutex; + bool debug; + bool _done; + int refreshRate; + bool pauseStatus; + + IceUtil::Cond sem; + +}; + + +} /* namespace jderobot */ +#endif /* JDEROBOTCOMM_NAVDATAICECLIENT_H_ */ \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/interfaces/ardroneextraClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/ardroneextraClient.hpp new file mode 100644 index 000000000..de73cc0d4 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/ardroneextraClient.hpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H +#define JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H + + +namespace Comm { + + /** + * @brief MotorsClient 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 ArDroneExtraClient { + public: + bool on = false; + + virtual void toggleCam()=0; + virtual void land()=0; + virtual void takeoff()=0; + virtual void reset()=0; + virtual void recordOnUsb(bool record)=0; + virtual void ledAnimation(int type,float duration, float req)=0; + virtual void flightAnimation(int type, float duration)=0; + virtual void flatTrim()=0; + + }; + +} //NS + +#endif // JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/interfaces/cmdvelClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/cmdvelClient.hpp new file mode 100644 index 000000000..ebd5b4b88 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/cmdvelClient.hpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 1997-2017 JDE Developers Team + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * Authors : + * Aitor Martinez Fernandez + */ + +#ifndef JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H +#define JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H + +#include + + +namespace Comm { + + /** + * @brief MotorsClient 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 CMDVelClient { + public: + virtual void sendVelocities(JdeRobotTypes::CMDVel vel ) = 0; + bool on = false; + protected: + JdeRobotTypes::CMDVel cmdvel; + }; + +} //NS + +#endif // JDEROBOTCOMM_CMDVELCLIENT_INTERFACE_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/interfaces/navdataClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/interfaces/navdataClient.hpp new file mode 100644 index 000000000..8af83b216 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/interfaces/navdataClient.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_NAVDATACLIENT_INTERFACE_H +#define JDEROBOTCOMM_NAVDATACLIENT_INTERFACE_H + +#include + + +namespace Comm { + + /** + * @brief NavdataClient 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 NavdataClient { + public: + virtual JdeRobotTypes::NavdataData getNavdataData() = 0; + bool on = false; + protected: + JdeRobotTypes::NavdataData navdataData; + }; + +} //NS + +#endif // JDEROBOTCOMM_NAVDATACLIENT_INTERFACE_H \ No newline at end of file diff --git a/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp b/src/libs/comm_cpp/include/jderobot/comm/navdataClient.hpp new file mode 100644 index 000000000..ad9771f36 --- /dev/null +++ b/src/libs/comm_cpp/include/jderobot/comm/navdataClient.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_NAVDATACLIENT_H +#define JDEROBOTCOMM_NAVDATACLIENT_H + +#include +#include +#include +#include + + + + +namespace Comm { + + /** + * @brief make a NavdataClient using propierties + * + * + * @param communicator that contains properties + * @param prefix of client Propierties (example: "kobukiViewer.Navdata") + * + * + * @return null if propierties are wrong + */ + NavdataClient* getNavdataClient(Comm::Communicator* jdrc, std::string prefix); + + +} //NS + +#endif // JDEROBOTCOMM_NAVDATACLIENT_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 new file mode 100644 index 000000000..330c70389 --- /dev/null +++ b/src/libs/comm_cpp/src/ardroneextraClient.cpp @@ -0,0 +1,61 @@ +#include +#include +#ifdef JDERROS +//#include +#endif + +namespace Comm { + +ArDroneExtraClient* +getArDroneExtraClient(Comm::Communicator* jdrc, std::string prefix){ + ArDroneExtraClient* client = 0; + + + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + switch (server){ + case 0: + { + std::cout << "ArDroneExtra disabled" << std::endl; + break; + } + case 1: + { + std::cout << "Sending ArDroneExtra by ICE interfaces" << std::endl; + ArDroneExtraIceClient* cl; + cl = new ArDroneExtraIceClient(jdrc, prefix); + client = (Comm::ArDroneExtraClient*) cl; + break; + } + case 2: + { + #ifdef JDERROS + /*std::cout << "Sending Velocities by ROS messages" << std::endl; + std::string nodeName; + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "ArDroneExtraNode"); + std::string topic; + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); + PublisherArDroneExtra* pm; + pm = new PublisherArDroneExtra(0, nullptr, nodeName, topic); + pm->start(); + client = (Comm::ArDroneExtraClient*) pm; + */ + throw "ERROR: ArDroneExtra is not supported with ROS yet"; + #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/cmdvelClient.cpp b/src/libs/comm_cpp/src/cmdvelClient.cpp new file mode 100644 index 000000000..43943e5d6 --- /dev/null +++ b/src/libs/comm_cpp/src/cmdvelClient.cpp @@ -0,0 +1,61 @@ +#include +#include +#ifdef JDERROS +//#include +#endif + +namespace Comm { + +CMDVelClient* +getCMDVelClient(Comm::Communicator* jdrc, std::string prefix){ + CMDVelClient* client = 0; + + + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + switch (server){ + case 0: + { + std::cout << "CMDVel disabled" << std::endl; + break; + } + case 1: + { + std::cout << "Sending CMDVel by ICE interfaces" << std::endl; + CMDVelIceClient* cl; + cl = new CMDVelIceClient(jdrc, prefix); + client = (Comm::CMDVelClient*) cl; + break; + } + case 2: + { + #ifdef JDERROS + /*std::cout << "Sending Velocities by ROS messages" << std::endl; + std::string nodeName; + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "CMDVelNode"); + std::string topic; + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); + PublisherCMDVel* pm; + pm = new PublisherCMDVel(0, nullptr, nodeName, topic); + pm->start(); + client = (Comm::CMDVelClient*) pm; + */ + throw "ERROR: CMDVel is not supported with ROS yet"; + #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/ardroneextraIceClient.cpp b/src/libs/comm_cpp/src/ice/ardroneextraIceClient.cpp new file mode 100644 index 000000000..502781fc4 --- /dev/null +++ b/src/libs/comm_cpp/src/ice/ardroneextraIceClient.cpp @@ -0,0 +1,114 @@ +/* + * Copyright (C) 1997-2017 JDE Developers + * + * 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 : Aitor Martinez Fernandez + */ + +#include "jderobot/comm/ice/ardroneextraIceClient.hpp" + +namespace Comm { + +ArDroneExtraIceClient::ArDroneExtraIceClient(Comm::Communicator* jdrc, std::string prefix) { + + this->prefix=prefix; + + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx baseArDroneExtra = jdrc->getIceComm()->stringToProxy(proxy); + + if (0==baseArDroneExtra){ + this->on = false; + std::cout << prefix + ".Proxy configuration not specified" <prx = jderobot::ArDroneExtraPrx::checkedCast(baseArDroneExtra); + + 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; + } + } + +} + +ArDroneExtraIceClient::~ArDroneExtraIceClient() { + this->on=false; +} + +void +ArDroneExtraIceClient::toggleCam(){ + this->controlMutex.lock(); + this->prx->toggleCam(); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::land(){ + this->controlMutex.lock(); + this->prx->land(); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::takeoff(){ + this->controlMutex.lock(); + this->prx->takeoff(); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::reset(){ + this->controlMutex.lock(); + this->prx->reset(); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::recordOnUsb(bool record){ + this->controlMutex.lock(); + this->prx->recordOnUsb(record); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::ledAnimation(int type,float duration, float req){ + this->controlMutex.lock(); + this->prx->ledAnimation(type, duration, req); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::flightAnimation(int type, float duration){ + this->controlMutex.lock(); + this->prx->flightAnimation(type, duration); + this->controlMutex.unlock(); +} +void +ArDroneExtraIceClient::flatTrim(){ + this->controlMutex.lock(); + this->prx->flatTrim(); + this->controlMutex.unlock(); +} + + +} /* NS */ \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ice/cmdvelIceClient.cpp b/src/libs/comm_cpp/src/ice/cmdvelIceClient.cpp new file mode 100644 index 000000000..040f67227 --- /dev/null +++ b/src/libs/comm_cpp/src/ice/cmdvelIceClient.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 1997-2017 JDE Developers + * + * 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 : Aitor Martinez Fernandez + */ + +#include "jderobot/comm/ice/cmdvelIceClient.hpp" + +namespace Comm { + +CMDVelIceClient::CMDVelIceClient(Comm::Communicator* jdrc, std::string prefix) { + + this->prefix=prefix; + + std::string proxy = jdrc->getConfig().asString(prefix+".Proxy"); + Ice::ObjectPrx baseCMDVel = jdrc->getIceComm()->stringToProxy(proxy); + + if (0==baseCMDVel){ + this->on = false; + std::cout << prefix + ".Proxy configuration not specified" <prx = jderobot::CMDVelPrx::checkedCast(baseCMDVel); + + 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; + } + } + +} + +CMDVelIceClient::~CMDVelIceClient() { + this->on=false; +} + +void +CMDVelIceClient::sendVelocities(JdeRobotTypes::CMDVel vel){ + this->controlMutex.lock(); + jderobot::CMDVelDataPtr icevel=new jderobot::CMDVelData(); + + icevel->linearX=vel.vx; + icevel->linearY=vel.vy; + icevel->linearZ=vel.vz; + icevel->angularZ=vel.az; + icevel->angularX=vel.ax; + icevel->angularY=vel.ay; + + this->prx->setCMDVelData(icevel); + this->controlMutex.unlock(); +} + + +} /* NS */ \ No newline at end of file diff --git a/src/libs/comm_cpp/src/ice/navdataIceClient.cpp b/src/libs/comm_cpp/src/ice/navdataIceClient.cpp new file mode 100644 index 000000000..0a7b2ee22 --- /dev/null +++ b/src/libs/comm_cpp/src/ice/navdataIceClient.cpp @@ -0,0 +1,168 @@ +/* + * 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/navdataIceClient.hpp" + +namespace Comm { + +NavdataIceClient::NavdataIceClient(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 baseNavdata = jdrc->getIceComm()->stringToProxy(proxy); + + if (0==baseNavdata){ + this->on = false; + std::cout << prefix + ".Proxy configuration not specified" <prx = jderobot::NavdataPrx::checkedCast(baseNavdata); + + 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; + +} + +NavdataIceClient::~NavdataIceClient() { + this->on=false; +} + + +void NavdataIceClient::pause(){ + std::cout << "Paused" << std::endl; + this->pauseStatus=true; +} + +void NavdataIceClient::resume(){ + std::cout << "resumed" << std::endl; + this->controlMutex.lock(); + this->pauseStatus=false; + this->sem.broadcast(); + this->controlMutex.unlock(); +} + +void NavdataIceClient::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::NavdataDataPtr localNavdata=this->prx->getNavdata(); + + + this->controlMutex.lock(); + this->navdataData.vehicle = localNavdata->vehicle; + this->navdataData.state = localNavdata->state; + + this->navdataData.batteryPercent = localNavdata->batteryPercent; + this->navdataData.magX = localNavdata->magX; + this->navdataData.magY = localNavdata->magY; + this->navdataData.magZ = localNavdata->magZ; + this->navdataData.pressure = localNavdata->pressure; + this->navdataData.temp = localNavdata->temp; + + this->navdataData.windSpeed = localNavdata->windSpeed; + this->navdataData.windAngle = localNavdata->windAngle; + this->navdataData.windCompAngle = localNavdata->windCompAngle; + + this->navdataData.rotX = localNavdata->rotX; + this->navdataData.rotY = localNavdata->rotY; + this->navdataData.rotZ = localNavdata->rotZ; + this->navdataData.altd = localNavdata->altd; + + this->navdataData.vx = localNavdata->vx; + this->navdataData.vy = localNavdata->vy; + this->navdataData.vz = localNavdata->vz; + this->navdataData.ax = localNavdata->ax; + this->navdataData.ay = localNavdata->ay; + this->navdataData.az = localNavdata->az; + + this->navdataData.tagsCount = localNavdata->tagsCount; + this->navdataData.tagsType = localNavdata->tagsType; + this->navdataData.tagsXc = localNavdata->tagsXc; + this->navdataData.tagsYc = localNavdata->tagsYc; + this->navdataData.tagsWidth = localNavdata->tagsWidth; + this->navdataData.tagsHeight = localNavdata->tagsHeight; + this->navdataData.tagsOrientation = localNavdata->tagsOrientation; + this->navdataData.tagsDistance = localNavdata->tagsDistance; + + this->navdataData.timeStamp = localNavdata->tm; + + + 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::NavdataData NavdataIceClient::getNavdataData(){ + JdeRobotTypes::NavdataData navdata; + this->controlMutex.lock(); + navdata = this->navdataData; + this->controlMutex.unlock(); + return navdata; +} + +} /* NS */ \ No newline at end of file diff --git a/src/libs/comm_cpp/src/navdataClient.cpp b/src/libs/comm_cpp/src/navdataClient.cpp new file mode 100644 index 000000000..65a1a6029 --- /dev/null +++ b/src/libs/comm_cpp/src/navdataClient.cpp @@ -0,0 +1,62 @@ +#include +#include +#ifdef JDERROS +//#include +#endif + +namespace Comm { + +NavdataClient* +getNavdataClient(Comm::Communicator* jdrc, std::string prefix){ + NavdataClient* client = 0; + + int server = jdrc->getConfig().asIntWithDefault(prefix+".Server", 0); + switch (server){ + case 0: + { + std::cout << "Navdata disabled" << std::endl; + break; + } + case 1: + { + std::cout << "Receiving NavdataData from ICE interfaces" << std::endl; + NavdataIceClient* cl; + cl = new NavdataIceClient(jdrc, prefix); + cl->start(); + client = (Comm::NavdataClient*) cl; + break; + } + case 2: + { + #ifdef JDERROS + /*std::cout << "Receiving NavdataData from ROS messages" << std::endl; + std::string nodeName; + nodeName = jdrc->getConfig().asStringWithDefault(prefix+".Name", "NavdataNode"); + std::string topic; + topic = jdrc->getConfig().asStringWithDefault(prefix+".Topic", ""); + ListenerNavdata* lc; + lc = new ListenerNavdata(0, nullptr, nodeName, topic); + lc->start(); + client = (Comm::NavdataClient*) lc; + */ + throw "ERROR: Navdata is not supported with ROS yet"; + #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/types/cpp/CMakeLists.txt b/src/types/cpp/CMakeLists.txt index 154945a88..c6fe0ae10 100644 --- a/src/types/cpp/CMakeLists.txt +++ b/src/types/cpp/CMakeLists.txt @@ -11,23 +11,11 @@ set(HEADERS include/jderobot/types/laserData.h include/jderobot/types/image.h include/jderobot/types/pose3d.h + include/jderobot/types/rgbd.h + include/jderobot/types/navdataData.h + include/jderobot/types/cmdvel.h ) -#set(SOURCES -# src/loader.cpp -# src/initializer.cpp -#) -#set_property(SOURCE src/loader.cpp PROPERTY COMPILE_FLAGS "-std=c++0x") - -## Adding shared library for common usage -#add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) -#target_link_libraries(${PROJECT_NAME} ${Ice_LIBRARIES}) - -## Adding static library for single .so configurations -# since target is a shared library, -fPIC must be provided -#add_library(${PROJECT_NAME}-embedded STATIC ${SOURCES} ${HEADERS}) -#target_link_libraries(${PROJECT_NAME}-embedded ${Ice_LIBRARIES}) -#set_property(TARGET ${PROJECT_NAME}-embedded PROPERTY POSITION_INDEPENDENT_CODE 1) ## Export library variables (like find_package) set(${PROJECT_NAME}_FOUND 1 CACHE BOOL "Find(${PROJECT_NAME})") diff --git a/src/types/cpp/include/jderobot/types/navdataData.h b/src/types/cpp/include/jderobot/types/navdataData.h new file mode 100644 index 000000000..48a9ec03c --- /dev/null +++ b/src/types/cpp/include/jderobot/types/navdataData.h @@ -0,0 +1,83 @@ +/* + * 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= 0 ; 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_NAVDATADATA_H +#define JDEROBOTTYPES_NAVDATADATA_H + +#include + +namespace JdeRobotTypes { + + class NavdataData { + public: + //0-> ArDrone1, 1-> ArDrone2 + int vehicle = 1; + int state = 0; + //The remaing charge of baterry % + float batteryPercent = 0; + //Magnetometer ArDrone 2.0 + int magX= 0 ; + int magY= 0 ; + int magZ= 0 ; + //Barometer ArDrone 2.0 + int pressure= 0 ; + //Temperature sensor ArDrone 2.0 + int temp= 0 ; + //Estimated wind speed ArDrone 2.0 + float windSpeed= 0 ; + float windAngle= 0 ; + float windCompAngle= 0 ; + //rotation about the X axis + float rotX= 0 ; + //rotation about the Y axis + float rotY= 0 ; + //rotation about the Z axis + float rotZ= 0 ; + //Estimated altitude (mm) + int altd= 0 ; + //linear velocity (mm/sec) + float vx= 0 ; + //linear velocity (mm/sec) + float vy= 0 ; + //linear velocity (mm/sec) + float vz= 0 ; + //linear accelerations (unit: g) ¿ArDrone 2.0? + float ax= 0 ; + float ay= 0 ; + float az= 0 ; + //Tags in Vision Detectoion + //Should be unsigned + int tagsCount= 0 ; + + std::vector tagsType; + std::vector tagsXc; + std::vector tagsYc; + std::vector tagsWidth; + std::vector tagsHeight; + std::vector tagsOrientation; + std::vector tagsDistance; + double timeStamp = 0; //seconds + }; + + +} //NS + +#endif // JDEROBOTTYPES_NAVDATADATA_H + + From 32af0805879993d01630f3eeb588862380d1a1c7 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Fri, 20 Oct 2017 10:24:00 +0200 Subject: [PATCH 2/3] added drone support for comm_py --- .../comm/ardroneextraClient.py.only-ice.in | 79 +++++++++ .../comm_py/comm/ardroneextraClient.py.ros.in | 79 +++++++++ .../comm_py/comm/cmdvelClient.py.only-ice.in | 79 +++++++++ src/libs/comm_py/comm/cmdvelClient.py.ros.in | 79 +++++++++ .../comm_py/comm/communicator.py.only-ice.in | 48 ++++++ src/libs/comm_py/comm/communicator.py.ros.in | 61 +++++++ .../comm_py/comm/ice/ardroneextraIceClient.py | 92 +++++++++++ src/libs/comm_py/comm/ice/cmdvelIceClient.py | 138 ++++++++++++++++ src/libs/comm_py/comm/ice/navdataIceClient.py | 154 ++++++++++++++++++ .../comm_py/comm/ice/ptMotorsIceClient.py | 7 +- .../comm_py/comm/navdataClient.py.only-ice.in | 79 +++++++++ src/libs/comm_py/comm/navdataClient.py.ros.in | 79 +++++++++ src/libs/comm_py/test.yml | 12 ++ src/libs/comm_py/testArDroneExtra.py | 24 +++ src/libs/comm_py/testCMDVel.py | 28 ++++ src/libs/comm_py/testNavdata.py | 23 +++ src/types/python/jderobotTypes/__init__.py | 3 +- src/types/python/jderobotTypes/navdataData.py | 79 +++++++++ 18 files changed, 1139 insertions(+), 4 deletions(-) create mode 100644 src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in create mode 100644 src/libs/comm_py/comm/ardroneextraClient.py.ros.in create mode 100644 src/libs/comm_py/comm/cmdvelClient.py.only-ice.in create mode 100644 src/libs/comm_py/comm/cmdvelClient.py.ros.in create mode 100644 src/libs/comm_py/comm/ice/ardroneextraIceClient.py create mode 100644 src/libs/comm_py/comm/ice/cmdvelIceClient.py create mode 100644 src/libs/comm_py/comm/ice/navdataIceClient.py create mode 100644 src/libs/comm_py/comm/navdataClient.py.only-ice.in create mode 100644 src/libs/comm_py/comm/navdataClient.py.ros.in create mode 100644 src/libs/comm_py/testArDroneExtra.py create mode 100644 src/libs/comm_py/testCMDVel.py create mode 100644 src/libs/comm_py/testNavdata.py create mode 100644 src/types/python/jderobotTypes/navdataData.py diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in new file mode 100644 index 000000000..f5b06fa96 --- /dev/null +++ b/src/libs/comm_py/comm/ardroneextraClient.py.only-ice.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.ardroneextraIceClient import ArDroneExtraIceClient + + + +def __getArDroneExtraIceClient(jdrc, prefix): + ''' + Returns a ArDroneExtra Ice Client. This function should never be used. Use getArDroneExtraClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return ArDroneExtra Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = ArDroneExtraIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherArDroneExtra(jdrc, prefix): + ''' + Returns a ArDroneExtra ROS Publisher. This function should never be used. Use getArDroneExtraClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return ArDroneExtra ROS Publisher + + ''' + + print(prefix + ": ROS msg are diabled") + return None + +def __ArDroneExtradisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getArDroneExtraClient 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 getArDroneExtraClient (jdrc, prefix): + ''' + Returns a ArDroneExtra Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if ArDroneExtra is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/ardroneextraClient.py.ros.in b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in new file mode 100644 index 000000000..b29238ffb --- /dev/null +++ b/src/libs/comm_py/comm/ardroneextraClient.py.ros.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.ardroneextraIceClient import ArDroneExtraIceClient + + + +def __getArDroneExtraIceClient(jdrc, prefix): + ''' + Returns a ArDroneExtra Ice Client. This function should never be used. Use getArDroneExtraClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return ArDroneExtra Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = ArDroneExtraIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherArDroneExtra(jdrc, prefix): + ''' + Returns a ArDroneExtra ROS Publisher. This function should never be used. Use getArDroneExtraClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return ArDroneExtra ROS Publisher + + ''' + + print(prefix + ": This Interface doesn't support ROS msg") + return None + +def __ArDroneExtradisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getArDroneExtraClient 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 getArDroneExtraClient (jdrc, prefix): + ''' + Returns a ArDroneExtra Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if ArDroneExtra is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__ArDroneExtradisabled, __getArDroneExtraIceClient, __getPublisherArDroneExtra] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in new file mode 100644 index 000000000..e56668c21 --- /dev/null +++ b/src/libs/comm_py/comm/cmdvelClient.py.only-ice.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.cmdvelIceClient import CMDVelIceClient + + + +def __getCMDVelIceClient(jdrc, prefix): + ''' + Returns a CMDVel Ice Client. This function should never be used. Use getCMDVelClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return CMDVel Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = CMDVelIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherCMDVel(jdrc, prefix): + ''' + Returns a CMDVel ROS Publisher. This function should never be used. Use getCMDVelClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return CMDVel ROS Publisher + + ''' + + print(prefix + ": ROS msg are diabled") + return None + +def __CMDVeldisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getCMDVelClient 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 getCMDVelClient (jdrc, prefix): + ''' + Returns a CMDVel Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if CMDVel is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/cmdvelClient.py.ros.in b/src/libs/comm_py/comm/cmdvelClient.py.ros.in new file mode 100644 index 000000000..5e9be18c9 --- /dev/null +++ b/src/libs/comm_py/comm/cmdvelClient.py.ros.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.cmdvelIceClient import CMDVelIceClient + + + +def __getCMDVelIceClient(jdrc, prefix): + ''' + Returns a CMDVel Ice Client. This function should never be used. Use getCMDVelClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return CMDVel Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = CMDVelIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherCMDVel(jdrc, prefix): + ''' + Returns a CMDVel ROS Publisher. This function should never be used. Use getCMDVelClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return CMDVel ROS Publisher + + ''' + + print(prefix + ": This Interface doesn't support ROS msg") + return None + +def __CMDVeldisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getCMDVelClient 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 getCMDVelClient (jdrc, prefix): + ''' + Returns a CMDVel Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if CMDVel is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__CMDVeldisabled, __getCMDVelIceClient, __getPublisherCMDVel] + + 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 8fe745cd0..b0f83a488 100644 --- a/src/libs/comm_py/comm/communicator.py.only-ice.in +++ b/src/libs/comm_py/comm/communicator.py.only-ice.in @@ -5,6 +5,10 @@ from .cameraClient import getCameraClient from .pose3dClient import getPose3dClient from .motorsClient import getMotorsClient from .ptMotorsClient import getPTMotorsClient +from .rgbdClient import getRgbdClient +from .ardroneextraClient import getArDroneExtraClient +from .navdataClient import getNavdataClient +from .cmdvelClient import getCMDVelClient class Communicator: @@ -111,3 +115,47 @@ class Communicator: ''' return getLaserClient(self, name) + def getRgbdClient(self, name): + ''' + Returns a RGBD client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getRgbdClient(self, name) + + def getCMDVelClient(self, name): + ''' + Returns a CMDVel client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getCMDVelClient(self, name) + + def getNavdataClient(self, name): + ''' + Returns a Navdata client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getNavdataClient(self, name) + + def getArDroneExtraClient(self, name): + ''' + Returns a ArDroneExtra client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getArDroneExtraClient(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 53a65dd03..17eb0dfab 100644 --- a/src/libs/comm_py/comm/communicator.py.ros.in +++ b/src/libs/comm_py/comm/communicator.py.ros.in @@ -5,6 +5,11 @@ from .laserClient import getLaserClient from .cameraClient import getCameraClient from .pose3dClient import getPose3dClient from .motorsClient import getMotorsClient +from .rgbdClient import getRgbdClient +from .ardroneextraClient import getArDroneExtraClient +from .navdataClient import getNavdataClient +from .cmdvelClient import getCMDVelClient +from .ptMotorsClient import getPTMotorsClient class Communicator: @@ -106,3 +111,59 @@ class Communicator: ''' return getLaserClient(self, name) + + def getRgbdClient(self, name): + ''' + Returns a RGBD client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getRgbdClient(self, name) + + def getCMDVelClient(self, name): + ''' + Returns a CMDVel client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getCMDVelClient(self, name) + + def getNavdataClient(self, name): + ''' + Returns a Navdata client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getNavdataClient(self, name) + + def getArDroneExtraClient(self, name): + ''' + Returns a ArDroneExtra client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getArDroneExtraClient(self, name) + + def getPTMotorsClient(self, name): + ''' + Returns a PTMotors client with the configration indicated by the name + + @param name: name of the client in the config + + @type name: String + + ''' + return getPTMotorsClient(self, name) + diff --git a/src/libs/comm_py/comm/ice/ardroneextraIceClient.py b/src/libs/comm_py/comm/ice/ardroneextraIceClient.py new file mode 100644 index 000000000..eb6a07ca1 --- /dev/null +++ b/src/libs/comm_py/comm/ice/ardroneextraIceClient.py @@ -0,0 +1,92 @@ +# +# Copyright (C) 1997-2016 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# + +import traceback +import jderobot +import threading +import Ice + + +class ArDroneExtraIceClient: + ''' + ArDroneExtra Connector. Recives image from Ice interface when you run update method. + ''' + + def __init__(self, jdrc, prefix): + ''' + ArDroneExtra 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.ArDroneExtraPrx.checkedCast(base) + prop = ic.getProperties() + + if not self.proxy: + print ('Interface ' + prefix + ' not configured') + + except Ice.ConnectionRefusedException: + print(prefix + ': connection refused') + + except: + traceback.print_exc() + exit(-1) + + + def takeoff(self): + if self.hasproxy(): + self.lock.acquire() + self.proxy.takeoff() + self.lock.release() + + def land(self): + if self.hasproxy(): + self.lock.acquire() + self.proxy.land() + self.lock.release() + + def toggleCam(self): + if self.hasproxy(): + self.lock.acquire() + self.proxy.toggleCam() + self.lock.release() + + def reset(self): + if self.hasproxy(): + self.lock.acquire() + self.proxy.reset() + self.lock.release() + + def record(self,record): + if self.hasproxy(): + self.lock.acquire() + self.proxy.recordOnUsb(record) + self.lock.release() + + def hasproxy (self): + return hasattr(self,"proxy") and self.proxy \ No newline at end of file diff --git a/src/libs/comm_py/comm/ice/cmdvelIceClient.py b/src/libs/comm_py/comm/ice/cmdvelIceClient.py new file mode 100644 index 000000000..7da34d8fd --- /dev/null +++ b/src/libs/comm_py/comm/ice/cmdvelIceClient.py @@ -0,0 +1,138 @@ +# +# 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 CMDVel + + +class CMDVelIceClient: + + + ''' + CMDVel Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param jdrc: Comm Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + ''' + def __init__(self, jdrc, prefix): + self.lock = threading.Lock() + ic = jdrc.getIc() + + self.vel = CMDVel() + + + try: + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) + self.proxy = jderobot.CMDVelPrx.checkedCast(base) + + + if not self.proxy: + print ('Interface ' + prefix + ' not configured') + + except Ice.ConnectionRefusedException: + print(prefix + ': connection refused') + + except: + traceback.print_exc() + exit(-1) + + + def start(self): + pass + def stop(self): + pass + + def setVX(self,vx): + self.lock.acquire() + self.vel.vx=vx + self.lock.release() + + def setVY(self,vy): + self.lock.acquire() + self.vel.vy=vy + self.lock.release() + + def setVZ(self,vz): + self.lock.acquire() + self.vel.vz=vz + self.lock.release() + + def setAngularZ(self,az): + self.lock.acquire() + self.vel.az=az + self.lock.release() + + def setAngularX(self,ax): + self.lock.acquire() + self.vel.ax=ax + self.lock.release() + + def setAngularY(self,ay): + self.lock.acquire() + self.vel.ay=ay + self.lock.release() + + def setYaw(self,yaw): + self.setAngularZ(yaw) + + def setRoll(self,roll): + self.angularX(roll) + + def setPitch(self,pitch): + self.angularY(pitch) + + def setCMD (self, cmd): + self.lock.acquire() + self.vel = cmd + self.lock.release() + + def sendVelocities(self): + if self.hasproxy(): + self.lock.acquire() + tmp = self.vel + self.lock.release() + self.sendCMDVel(tmp.vx,tmp.vy,tmp.vz,tmp.ax, tmp.ay, tmp.az) + + def sendCMDVel(self,vx,vy,vz,ax,ay,az): + cmd=jderobot.CMDVelData() + + cmd.linearX = vx + cmd.linearY = vy + cmd.linearZ = vz + cmd.angularZ = az + cmd.angularX = ax + cmd.angularY = ay + + if self.hasproxy(): + self.lock.acquire() + self.proxy.setCMDVelData(cmd) + self.lock.release() + + + def hasproxy (self): + return hasattr(self,"proxy") and self.proxy \ No newline at end of file diff --git a/src/libs/comm_py/comm/ice/navdataIceClient.py b/src/libs/comm_py/comm/ice/navdataIceClient.py new file mode 100644 index 000000000..95a65bb9a --- /dev/null +++ b/src/libs/comm_py/comm/ice/navdataIceClient.py @@ -0,0 +1,154 @@ +# +# Copyright (C) 1997-2016 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Aitor Martinez Fernandez +# + +import traceback +import jderobot +import threading +import Ice +from .threadSensor import ThreadSensor +from jderobotTypes import NavdataData + + +class NavData: + + def __init__(self, jdrc, prefix): + self.lock = threading.Lock() + + try: + ic = jdrc.getIc() + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) + self.proxy = jderobot.NavdataPrx.checkedCast(base) + + self.navData = NavdataData() + + 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 self.hasproxy(): + localNavdata = self.proxy.getNavdata() + navdataData = NavdataData() + + navdataData.vehicle = localNavdata.vehicle + navdataData.state = localNavdata.state + + navdataData.batteryPercent = localNavdata.batteryPercent + navdataData.magX = localNavdata.magX + navdataData.magY = localNavdata.magY + navdataData.magZ = localNavdata.magZ + navdataData.pressure = localNavdata.pressure + navdataData.temp = localNavdata.temp + + navdataData.windSpeed = localNavdata.windSpeed + navdataData.windAngle = localNavdata.windAngle + navdataData.windCompAngle = localNavdata.windCompAngle + + navdataData.rotX = localNavdata.rotX + navdataData.rotY = localNavdata.rotY + navdataData.rotZ = localNavdata.rotZ + navdataData.altd = localNavdata.altd + + navdataData.vx = localNavdata.vx + navdataData.vy = localNavdata.vy + navdataData.vz = localNavdata.vz + navdataData.ax = localNavdata.ax + navdataData.ay = localNavdata.ay + navdataData.az = localNavdata.az + + navdataData.tagsCount = localNavdata.tagsCount + navdataData.tagsType = localNavdata.tagsType + navdataData.tagsXc = localNavdata.tagsXc + navdataData.tagsYc = localNavdata.tagsYc + navdataData.tagsWidth = localNavdata.tagsWidth + navdataData.tagsHeight = localNavdata.tagsHeight + navdataData.tagsOrientation = localNavdata.tagsOrientation + navdataData.tagsDistance = localNavdata.tagsDistance + + navdataData.timeStamp = localNavdata.tm + + self.lock.acquire() + self.navData = navdataData + 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 getNavdata(self): + if self.hasproxy(): + self.lock.acquire() + navData = self.navData + self.lock.release() + return navData + + return None + + + + +class NavdataIceClient: + def __init__(self,ic,prefix, start = False): + self.navdata = NavData(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.navdata, self.kill_event) + self.thread.daemon = True + + if start: + self.start() + + def start(self): + ''' + Starts the client. If client is stopped you can not start again, Threading.Thread raised error + ''' + self.kill_event.clear() + self.thread.start() + + def stop(self): + ''' + Stops the client. If client is stopped you can not start again, Threading.Thread raised error + ''' + self.kill_event.set() + + def getNavdata(self): + return self.navdata.getNavdata() + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return self.navdata.hasproxy() \ No newline at end of file diff --git a/src/libs/comm_py/comm/ice/ptMotorsIceClient.py b/src/libs/comm_py/comm/ice/ptMotorsIceClient.py index 18f972ef7..ee7cbf0e0 100644 --- a/src/libs/comm_py/comm/ice/ptMotorsIceClient.py +++ b/src/libs/comm_py/comm/ice/ptMotorsIceClient.py @@ -27,16 +27,17 @@ class PTMotors: - def __init__(self, ic, prefix): + def __init__(self, jdrc, prefix): self.lock = threading.Lock() self.data=jderobot.PTMotorsData() self.params=jderobot.PTMotorsParams() - prop = ic.getProperties() + ic = jdrc.getIc() try: - base = ic.propertyToProxy(prefix+".Proxy") + proxyStr = jdrc.getConfig().getProperty(prefix+".Proxy") + base = ic.stringToProxy(proxyStr) self.proxy = jderobot.PTMotorsPrx.checkedCast(base) if not self.proxy: diff --git a/src/libs/comm_py/comm/navdataClient.py.only-ice.in b/src/libs/comm_py/comm/navdataClient.py.only-ice.in new file mode 100644 index 000000000..f1be2d4c3 --- /dev/null +++ b/src/libs/comm_py/comm/navdataClient.py.only-ice.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.navdataIceClient import NavdataIceClient + + + +def __getNavdataIceClient(jdrc, prefix): + ''' + Returns a Navdata Ice Client. This function should never be used. Use getNavdataClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Navdata Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = NavdataIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherNavdata(jdrc, prefix): + ''' + Returns a Navdata ROS Publisher. This function should never be used. Use getNavdataClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Navdata ROS Publisher + + ''' + + print(prefix + ": ROS msg are diabled") + return None + +def __Navdatadisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getNavdataClient 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 getNavdataClient (jdrc, prefix): + ''' + Returns a Navdata Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if Navdata is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/comm/navdataClient.py.ros.in b/src/libs/comm_py/comm/navdataClient.py.ros.in new file mode 100644 index 000000000..1501e5661 --- /dev/null +++ b/src/libs/comm_py/comm/navdataClient.py.ros.in @@ -0,0 +1,79 @@ +import sys +import Ice +from .ice.navdataIceClient import NavdataIceClient + + + +def __getNavdataIceClient(jdrc, prefix): + ''' + Returns a Navdata Ice Client. This function should never be used. Use getNavdataClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Navdata Ice Client + + ''' + print("Publishing "+ prefix +" with ICE interfaces") + client = NavdataIceClient(jdrc, prefix) + client.start() + return client + +def __getPublisherNavdata(jdrc, prefix): + ''' + Returns a Navdata ROS Publisher. This function should never be used. Use getNavdataClient instead of this + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + + @type jdrc: Comm Communicator + @type prefix: String + + @return Navdata ROS Publisher + + ''' + + print(prefix + ": This Interface doesn't support ROS msg") + return None + +def __Navdatadisabled(jdrc, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getNavdataClient 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 getNavdataClient (jdrc, prefix): + ''' + Returns a Navdata Client. + + @param jdrc: Comm Communicator + @param prefix: Name of client in config file + @param node: ROS node + + @type jdrc: Comm Communicator + @type prefix: String + @type node: ROS node + + @return None if Navdata is disabled + + ''' + server = jdrc.getConfig().getProperty(prefix+".Server") + if not server: + server=0 + + cons = [__Navdatadisabled, __getNavdataIceClient, __getPublisherNavdata] + + return cons[server](jdrc, prefix) \ No newline at end of file diff --git a/src/libs/comm_py/test.yml b/src/libs/comm_py/test.yml index e22b1cb39..548bb8c6d 100644 --- a/src/libs/comm_py/test.yml +++ b/src/libs/comm_py/test.yml @@ -33,6 +33,18 @@ Test: Topic: "/scan" Name: testLaser + CMDVel: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "CMDVel:default -h 0.0.0.0 -p 9000" + + Navdata: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: Navdata:default -h 0.0.0.0 -p 9000 + + Extra: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: Extra:default -h 0.0.0.0 -p 9000 + 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/testArDroneExtra.py new file mode 100644 index 000000000..59948f716 --- /dev/null +++ b/src/libs/comm_py/testArDroneExtra.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import config +import comm +import sys +import time +import signal + + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + + client = jdrc.getArDroneExtraClient("Test.Extra") + for i in range (10): + client.takeoff() + time.sleep(4) + client.land() + + jdrc.destroy() \ No newline at end of file diff --git a/src/libs/comm_py/testCMDVel.py b/src/libs/comm_py/testCMDVel.py new file mode 100644 index 000000000..df5e440a2 --- /dev/null +++ b/src/libs/comm_py/testCMDVel.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +import config +import comm +import sys +import time +import signal + +from jderobotTypes import CMDVel + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + vel = CMDVel() + vel.vx = 1 + vel.az = 0.1 + + client = jdrc.getCMDVelClient("Test.CMDVel") + for i in range (10): + client.setCMD(vel) + client.sendVelocities() + time.sleep(1) + + jdrc.destroy() \ No newline at end of file diff --git a/src/libs/comm_py/testNavdata.py b/src/libs/comm_py/testNavdata.py new file mode 100644 index 000000000..65105eeff --- /dev/null +++ b/src/libs/comm_py/testNavdata.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python2 +import config +import comm +import sys +import time +import signal + +from jderobotTypes import NavdataData + + + + +if __name__ == '__main__': + + cfg = config.load(sys.argv[1]) + jdrc= comm.init(cfg, "Test") + + client = jdrc.getLaserClient("Test.Navdata") + + data = client.getNavdataData() + print (data) + + jdrc.destroy() \ No newline at end of file diff --git a/src/types/python/jderobotTypes/__init__.py b/src/types/python/jderobotTypes/__init__.py index 816a7d5e7..3bc41ac81 100644 --- a/src/types/python/jderobotTypes/__init__.py +++ b/src/types/python/jderobotTypes/__init__.py @@ -2,4 +2,5 @@ from .image import Image from .pose3d import Pose3d from .cmdvel import CMDVel -from .rgbd import Rgbd \ No newline at end of file +from .rgbd import Rgbd +from .navdataData import NavdataData \ No newline at end of file diff --git a/src/types/python/jderobotTypes/navdataData.py b/src/types/python/jderobotTypes/navdataData.py new file mode 100644 index 000000000..23acff3d3 --- /dev/null +++ b/src/types/python/jderobotTypes/navdataData.py @@ -0,0 +1,79 @@ +# -*- coding: utf-8 -*- +# +# 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 NavdataData (): + + def __init__(self): + + self.vehicle = 1 # 0-> ArDrone1, 1-> ArDrone2 + self.state = 0 + self.batteryPercent = 0 # The remaing charge of baterry % + + #Magnetometer ArDrone 2.0 + self.magX = 0 ; + self.magY = 0 ; + self.magZ = 0 ; + + + self.pressure = 0 ; #Barometer ArDrone 2.0 + self.temp = 0 ; #Temperature sensor ArDrone 2.0 + + self.windSpeed= 0 ; + self.windAngle= 0 ; + self.windCompAngle= 0 ; + + self.rotX= 0 ; #rotation about the X axis + self.rotY= 0 ; #rotation about the Y axis + self.rotZ= 0 ; #rotation about the Z axis + self.altd= 0 ; #Estimated altitude (mm) + self.vx= 0 ; #linear velocity (mm/sec) + self.vy= 0 ; #linear velocity (mm/sec) + self.vz= 0 ; #linear velocity (mm/sec) + #linear accelerations (unit: g) ¿ArDrone 2.0? + self.ax= 0 ; + self.ay= 0 ; + self.az= 0 ; + + #Tags in Vision Detectoion + #Should be unsigned + self.tagsCount= 0 ; + + self.tagsType = []; + self.tagsXc = []; + self.tagsYc = []; + self.tagsWidth = []; + self.tagsHeight = []; + self.tagsOrientation = []; + self.tagsDistance = []; + + self.timeStamp = 0 # seconds + + + + + def __str__(self): + s = "NavdataData: {\n vehicle: " + str(self.vehicle) + "\n state: " + str(self.state) + s = s + "\n batteryPercent: " + str(self.batteryPercent) + "\n rotX: " + str(self.rotX) + s = s + "\n rotY: " + str(self.rotY) + "\n rotZ: " + str(self.rotZ) + s = s + "\n altd: " + str(self.altd) + "\n vx: " + str(self.vx) + s = s + "\n vy: " + str(self.vy) + "\n vz: " + str(self.vz) + s = s + "\n timeStamp: " + str(self.timeStamp) + "\n...}" + return s \ No newline at end of file From 0d39f3e8f20951737a7e5ee7fdab586c463a1f06 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Fri, 20 Oct 2017 15:47:56 +0200 Subject: [PATCH 3/3] solved bugs with drone support of comm --- src/libs/comm_py/comm/ice/ardroneextraIceClient.py | 5 +++++ src/libs/comm_py/comm/ice/navdataIceClient.py | 4 ++-- src/libs/comm_py/testArDroneExtra.py | 7 +++---- src/libs/comm_py/testNavdata.py | 4 ++-- 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/libs/comm_py/comm/ice/ardroneextraIceClient.py b/src/libs/comm_py/comm/ice/ardroneextraIceClient.py index eb6a07ca1..f7b985786 100644 --- a/src/libs/comm_py/comm/ice/ardroneextraIceClient.py +++ b/src/libs/comm_py/comm/ice/ardroneextraIceClient.py @@ -58,6 +58,11 @@ def __init__(self, jdrc, prefix): exit(-1) + def start(self): + pass + def stop(self): + pass + def takeoff(self): if self.hasproxy(): self.lock.acquire() diff --git a/src/libs/comm_py/comm/ice/navdataIceClient.py b/src/libs/comm_py/comm/ice/navdataIceClient.py index 95a65bb9a..a1a2283e1 100644 --- a/src/libs/comm_py/comm/ice/navdataIceClient.py +++ b/src/libs/comm_py/comm/ice/navdataIceClient.py @@ -136,12 +136,12 @@ def start(self): self.thread.start() def stop(self): - ''' + ''' Stops the client. If client is stopped you can not start again, Threading.Thread raised error ''' self.kill_event.set() - def getNavdata(self): + def getNavData(self): return self.navdata.getNavdata() def hasproxy (self): diff --git a/src/libs/comm_py/testArDroneExtra.py b/src/libs/comm_py/testArDroneExtra.py index 59948f716..1773db1e0 100644 --- a/src/libs/comm_py/testArDroneExtra.py +++ b/src/libs/comm_py/testArDroneExtra.py @@ -16,9 +16,8 @@ client = jdrc.getArDroneExtraClient("Test.Extra") - for i in range (10): - client.takeoff() - time.sleep(4) - client.land() + client.takeoff() + time.sleep(4) + client.land() jdrc.destroy() \ No newline at end of file diff --git a/src/libs/comm_py/testNavdata.py b/src/libs/comm_py/testNavdata.py index 65105eeff..815fcd133 100644 --- a/src/libs/comm_py/testNavdata.py +++ b/src/libs/comm_py/testNavdata.py @@ -15,9 +15,9 @@ cfg = config.load(sys.argv[1]) jdrc= comm.init(cfg, "Test") - client = jdrc.getLaserClient("Test.Navdata") + client = jdrc.getNavdataClient("Test.Navdata") - data = client.getNavdataData() + data = client.getNavData() print (data) jdrc.destroy() \ No newline at end of file