diff --git a/src/libs/jderobotcomm_py/CMakeLists.txt b/src/libs/jderobotcomm_py/CMakeLists.txt new file mode 100644 index 000000000..505d93ba1 --- /dev/null +++ b/src/libs/jderobotcomm_py/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8) + +configure_module_python(jderobotComm) + +add_custom_target(jderobotcomm_py ALL) +copy_to_binary_python(jderobotcomm_py jderobotComm) + +install_python(jderobotComm core) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/__init__.py b/src/libs/jderobotcomm_py/jderobotComm/__init__.py new file mode 100644 index 000000000..fce5fb55b --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/__init__.py @@ -0,0 +1,56 @@ +import Ice +import rospy + +from .laserClient import getLaserClient +from .cameraClient import getCameraClient +from .pose3dClient import getPose3dClient +from .motorsClient import getMotorsClient + + +def init (ic): + ''' + Starts ROS Node if it is necessary. + + @param ic: Ice Communicator + + @type ic: Ice Communicator + ''' + node = None + rosserver = False + + prop = ic.getProperties() + nodeName = prop.getPropertyWithDefault("NodeName", "JdeRobot") + + l = prop.getPropertiesForPrefix("") + keys = l.keys() + for i in keys: + if (i.endswith(".Server") and l[i] == "2"): + rosserver = True + + if (rosserver): + node = rospy.init_node(nodeName, anonymous=True) + + return ic,node + +def destroy(ic=None, node=None): + ''' + Destroys ROS Node and Ice Communicator if it is necessary. + + @param ic: Ice Communicator + @param node: ROS Node + + @type ic: Ice Communicator + @type node: ROS Node + ''' + if node: + rospy.signal_shutdown("Node Closed") + if ic: + ic.destroy() + + + + + + + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py b/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py new file mode 100644 index 000000000..a98a8ca46 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/cameraClient.py @@ -0,0 +1,86 @@ +import sys +import Ice +import rospy +from .ice.cameraIceClient import CameraIceClient + +if (sys.version_info[0] == 2): + from .ros.listenerCamera import ListenerCamera + +def __getCameraIceClient(ic, prefix): + ''' + Returns a Camera Ice Client. This function should never be used. Use getCameraClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Camera Ice Client + + ''' + print("Receiving " + prefix + " Image from ICE interfaces") + client = CameraIceClient(ic, prefix) + client.start() + return client + +def __getListenerCamera(ic, prefix): + ''' + Returns a Camera ROS Subscriber. This function should never be used. Use getCameraClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Camera ROS Subscriber + + ''' + if (sys.version_info[0] == 2): + print("Receiving " + prefix + " Image from ROS messages") + prop = prop = ic.getProperties() + topic = prop.getPropertyWithDefault(prefix+".Topic",""); + client = ListenerCamera(topic) + return client + else: + print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) + return None + +def __Cameradisabled(ic, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getCameraClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return None + + ''' + print( prefix + " Disabled") + return None + +def getCameraClient (ic, prefix, node=None): + ''' + Returns a Camera Client. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param node: ROS node + + @type ic: Ice Communicator + @type prefix: String + @type node: ROS node + + @return None if Camera is disabled + + ''' + prop = prop = ic.getProperties() + server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + + cons = [__Cameradisabled, __getCameraIceClient, __getListenerCamera] + + return cons[server](ic, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/__init__.py b/src/libs/jderobotcomm_py/jderobotComm/ice/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py b/src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py new file mode 100644 index 000000000..ef78c2658 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ice/cameraIceClient.py @@ -0,0 +1,176 @@ +# +# 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 numpy as np +import threading +import Ice +from .threadSensor import ThreadSensor +from jderobotTypes import Image + + +class Camera: + ''' + Camera Connector. Recives image from Ice interface when you run update method. + ''' + + def __init__(self, ic, prefix): + ''' + Camera Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + ''' + + self.lock = threading.Lock() + self.image = Image() + + try: + basecamera = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.CameraPrx.checkedCast(basecamera) + prop = ic.getProperties() + self.imgFormat = prop.getProperty(prefix+".Format") + if not self.imgFormat: + self.imgFormat = "RGB8" + + 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): + ''' + Updates Image. + ''' + if self.hasproxy(): + img = Image() + imageData = self.proxy.getImageData(self.imgFormat) + img.height = imageData.description.height + img.width = imageData.description.width + img.format = imageData.description.format + + img.data = np.frombuffer(imageData.pixelData, dtype=np.uint8) + img.data.shape = img.height, img.width, 3 + + img.timeStamp = imageData.timeStamp.seconds + imageData.timeStamp.useconds * 1e-9 + + + self.lock.acquire() + self.image = img + self.lock.release() + + def getImage(self): + ''' + Returns last Image. + + @return last JdeRobotTypes Image saved + + ''' + img = Image() + if self.hasproxy(): + self.lock.acquire() + img = self.image + self.lock.release() + return img + + 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 + + + + +class CameraIceClient: + ''' + Camera Ice Client. Recives image from Ice interface running camera update method in a thread. + ''' + def __init__(self,ic,prefix, start = False): + ''' + CameraIceClient Contructor. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param start: indicates if start automatically the client + + @type ic: Ice Communicator + @type prefix: String + @type start: Boolean + ''' + self.camera = Camera(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.camera, self.kill_event) + #self.thread = ThreadSensor(self.camera) + 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 getImage(self): + ''' + Returns last Image. + + @return last JdeRobotTypes Image saved + + ''' + return self.camera.getImage() + + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return self.camera.hasproxy() diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py b/src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py new file mode 100644 index 000000000..3a5d22f9c --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ice/laserIceClient.py @@ -0,0 +1,173 @@ +# +# 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 LaserData + + +class Laser: + ''' + Laser Connector. Recives LaserData from Ice interface when you run update method. + ''' + + def __init__(self, ic, prefix): + ''' + Laser Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + ''' + self.lock = threading.Lock() + self.laser = LaserData() + + try: + base = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.LaserPrx.checkedCast(base) + prop = ic.getProperties() + + 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): + ''' + Updates LaserData. + ''' + if self.hasproxy(): + laserD = LaserData() + values = [] + data = self.proxy.getLaserData() + + #laserD.values = laser.distanceData + for i in range (data.numLaser): + values.append(data.distanceData[i] / 1000) + + laserD.maxAngle = data.maxAngle + laserD.minAngle = data.minAngle + laserD.maxRange = data.maxRange + laserD.minRange = data.minRange + laserD.values = values + + + self.lock.acquire() + self.laser = laserD + 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 getLaserData(self): + ''' + Returns last LaserData. + + @return last JdeRobotTypes LaserData saved + + ''' + if self.hasproxy(): + self.lock.acquire() + laser = self.laser + self.lock.release() + return laser + + return None + + + + +class LaserIceClient: + ''' + Laser Ice Client. Recives LaserData from Ice interface running Laser update method in a thread. + ''' + def __init__(self,ic,prefix, start = False): + ''' + LaserIceClient Contructor. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param start: indicates if start automatically the client + + @type ic: Ice Communicator + @type prefix: String + @type start: Boolean + ''' + self.laser = Laser(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.laser, 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 getLaserData(self): + ''' + Returns last LaserData. + + @return last JdeRobotTypes LaserData saved + + ''' + return self.laser.getLaserData() + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return self.laser.hasproxy() \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py b/src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py new file mode 100644 index 000000000..53a269609 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ice/motorsIceClient.py @@ -0,0 +1,112 @@ +# +# 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 MotorsIceClient: + + def __init__(self, ic, prefix): + self.lock = threading.Lock() + prop = ic.getProperties() + + maxWstr = prop.getProperty(prefix+".maxW") + if maxWstr: + self.maxW = float(maxWstr) + else: + self.maxW = 0.5 + print (prefix+".maxW not provided, the default value is used: "+ repr(self.maxW)) + + + maxVstr = prop.getProperty(prefix+".maxV") + if maxWstr: + self.maxV = float(maxVstr) + else: + self.maxV = 5 + print (prefix+".maxV not provided, the default value is used: "+ repr(self.maxV)) + + try: + base = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.MotorsPrx.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 sendVelocities(self, vel): + if self.hasproxy(): + self.lock.acquire() + self.proxy.setV(vel.vx) + self.proxy.setW(vel.az) + self.proxy.setL(vel.vy) + self.lock.release() + + def getMaxW(self): + return self.maxW + + def getMaxV(self): + return self.maxV + + def sendV(self, v): + self.sendVX(v) + + def sendL(self, l): + self.sendVY(l) + + def sendW(self, w): + self.sendAZ(w) + + def sendVX(self, vx): + if self.hasproxy(): + self.lock.acquire() + self.proxy.setV(vx) + self.lock.release() + + def sendVY(self, vy): + if self.hasproxy(): + self.lock.acquire() + self.proxy.setL(vy) + self.lock.release() + + def sendAZ(self, az): + if self.hasproxy(): + self.lock.acquire() + self.proxy.setW(az) + self.lock.release() + + def hasproxy (self): + return hasattr(self,"proxy") and self.proxy \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py b/src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py new file mode 100644 index 000000000..1da49c129 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ice/pose3dIceClient.py @@ -0,0 +1,229 @@ +# +# 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 math import asin, atan2, pi +from jderobotTypes import Pose3d + + +class Pose3D: + ''' + Pose3d Connector. Recives Pose3d from Ice interface when you run update method. + ''' + + def __init__(self, ic, prefix): + ''' + Pose3d Contructor. + Exits When it receives a Exception diferent to Ice.ConnectionRefusedException + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + ''' + self.lock = threading.Lock() + + self.pose = Pose3d() + + try: + base = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.Pose3DPrx.checkedCast(base) + prop = ic.getProperties() + + 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): + ''' + Updates Pose3d. + ''' + pos = Pose3d() + if self.hasproxy(): + pose = self.proxy.getPose3DData() + pos.yaw = self.quat2Yaw(pose.q0, pose.q1, pose.q2, pose.q3) + pos.pitch = self.quat2Pitch(pose.q0, pose.q1, pose.q2, pose.q3) + pos.roll = self.quat2Roll(pose.q0, pose.q1, pose.q2, pose.q3) + pos.x = pose.x + pos.y = pose.y + pos.z = pose.z + pos.h = pose.h + pos.q = [pose.q0, pose.q1, pose.q2, pose.q3] + + self.lock.acquire() + self.pose = pos + 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 getPose3d(self): + ''' + Returns last Pose3d. + + @return last JdeRobotTypes Pose3d saved + + ''' + self.lock.acquire() + pose = self.pose + self.lock.release() + return pose + + + def quat2Yaw(self, qw, qx, qy, qz): + ''' + Translates from Quaternion to Yaw. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Yaw value translated from Quaternion + + ''' + rotateZa0=2.0*(qx*qy + qw*qz) + rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz + rotateZ=0.0 + if(rotateZa0 != 0.0 and rotateZa1 != 0.0): + rotateZ=atan2(rotateZa0,rotateZa1) + return rotateZ + + def quat2Pitch(self, qw, qx, qy, qz): + ''' + Translates from Quaternion to Pitch. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Pitch value translated from Quaternion + + ''' + rotateYa0=-2.0*(qx*qz - qw*qy) + rotateY=0.0 + if(rotateYa0 >= 1.0): + rotateY = pi/2.0 + elif(rotateYa0 <= -1.0): + rotateY = -pi/2.0 + else: + rotateY = asin(rotateYa0) + + return rotateY + + def quat2Roll (self, qw, qx, qy, qz): + ''' + Translates from Quaternion to Roll. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Roll value translated from Quaternion + + ''' + rotateXa0=2.0*(qy*qz + qw*qx) + rotateXa1=qw*qw - qx*qx - qy*qy + qz*qz + rotateX=0.0 + + if(rotateXa0 != 0.0 and rotateXa1 != 0.0): + rotateX=atan2(rotateXa0, rotateXa1) + return rotateX + + + + + +class Pose3dIceClient: + ''' + Pose3d Ice Client. Recives Pose3d from Ice interface running Pose3d update method in a thread. + ''' + def __init__(self,ic,prefix, start = False): + ''' + Pose3dIceClient Contructor. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param start: indicates if start automatically the client + + @type ic: Ice Communicator + @type prefix: String + @type start: Boolean + ''' + self.pose3d = Pose3D(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.pose3d, 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 getPose3d(self): + ''' + Returns last Pose3d. + + @return last JdeRobotTypes Pose3d saved + + ''' + return self.pose3d.getPose3d() + + def hasproxy (self): + ''' + Returns if proxy has ben created or not. + + @return if proxy has ben created or not (Boolean) + + ''' + return self.pose3d.hasproxy() diff --git a/src/libs/jderobotcomm_py/jderobotComm/ice/threadSensor.py b/src/libs/jderobotcomm_py/jderobotComm/ice/threadSensor.py new file mode 100644 index 000000000..e30ece981 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ice/threadSensor.py @@ -0,0 +1,46 @@ +# +# 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 : +# Alberto Martin Florido +# Aitor Martinez Fernandez +# +import threading +import time +from datetime import datetime + +time_cycle = 80 + + +class ThreadSensor(threading.Thread): + + def __init__(self, sensor, kill_event): + self.sensor = sensor + self.kill_event = kill_event + threading.Thread.__init__(self, args=kill_event) + + def run(self): + while (not self.kill_event.is_set()): + start_time = datetime.now() + + self.sensor.update() + + finish_Time = datetime.now() + + dt = finish_Time - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + #print (ms) + if (ms < time_cycle): + time.sleep((time_cycle - ms) / 1000.0) diff --git a/src/libs/jderobotcomm_py/jderobotComm/laserClient.py b/src/libs/jderobotcomm_py/jderobotComm/laserClient.py new file mode 100644 index 000000000..934cd025e --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/laserClient.py @@ -0,0 +1,86 @@ +import sys +import Ice +import rospy +from .ice.laserIceClient import LaserIceClient + +if (sys.version_info[0] == 2): + from .ros.listenerLaser import ListenerLaser + +def __getLaserIceClient(ic, prefix): + ''' + Returns a Laser Ice Client. This function should never be used. Use getLaserClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Laser Ice Client + + ''' + print("Receiving " + prefix + " LaserData from ICE interfaces") + client = LaserIceClient(ic, prefix) + client.start() + return client + +def __getListenerLaser(ic, prefix): + ''' + Returns a Laser ROS Subscriber. This function should never be used. Use getLaserClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Laser ROS Subscriber + + ''' + if (sys.version_info[0] == 2): + print("Receiving " + prefix + " LaserData from ROS messages") + prop = prop = ic.getProperties() + topic = prop.getPropertyWithDefault(prefix+".Topic",""); + client = ListenerLaser(topic) + return client + else: + print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) + return None + +def __Laserdisabled(ic, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getLaserClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return None + + ''' + print( prefix + " Disabled") + return None + +def getLaserClient (ic, prefix, node=None): + ''' + Returns a Laser Client. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param node: ROS node + + @type ic: Ice Communicator + @type prefix: String + @type node: ROS node + + @return None if Laser is disabled + + ''' + prop = prop = ic.getProperties() + server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + + cons = [__Laserdisabled, __getLaserIceClient, __getListenerLaser] + + return cons[server](ic, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py b/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py new file mode 100644 index 000000000..787d5f9c9 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/motorsClient.py @@ -0,0 +1,103 @@ +import sys +import Ice +import rospy +from .ice.motorsIceClient import MotorsIceClient + +if (sys.version_info[0] == 2): + from .ros.publisherMotors import PublisherMotors + +def __getMotorsIceClient(ic, prefix): + ''' + Returns a Motors Ice Client. This function should never be used. Use getMotorsClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Motors Ice Client + + ''' + print("Publishing Motors with ICE interfaces") + client = MotorsIceClient(ic, prefix) + client.start() + return client + +def __getPublisherMotors(ic, prefix): + ''' + Returns a Motors ROS Publisher. This function should never be used. Use getMotorsClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Motors ROS Publisher + + ''' + if (sys.version_info[0] == 2): + print("Publishing Motors with ROS messages") + prop = prop = ic.getProperties() + topic = prop.getPropertyWithDefault(prefix+".Topic","") + + maxWstr = prop.getProperty(prefix+".maxW") + if maxWstr: + maxW = float(maxWstr) + else: + maxW = 0.5 + print (prefix+".maxW not provided, the default value is used: "+ repr(maxW)) + + + maxVstr = prop.getProperty(prefix+".maxV") + if maxWstr: + maxV = float(maxVstr) + else: + maxV = 5 + print (prefix+".maxV not provided, the default value is used: "+ repr(maxV)) + + + client = PublisherMotors(topic, maxV, maxW) + return client + else: + print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) + return None + +def __Motorsdisabled(ic, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getMotorsClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return None + + ''' + print(prefix + " Disabled") + return None + +def getMotorsClient (ic, prefix, node=None): + ''' + Returns a Motors Client. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param node: ROS node + + @type ic: Ice Communicator + @type prefix: String + @type node: ROS node + + @return None if Motors is disabled + + ''' + prop = prop = ic.getProperties() + server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + + cons = [__Motorsdisabled, __getMotorsIceClient, __getPublisherMotors] + + return cons[server](ic, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py b/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py new file mode 100644 index 000000000..f052ce976 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/pose3dClient.py @@ -0,0 +1,89 @@ +import sys +import Ice +import rospy +from .ice.pose3dIceClient import Pose3dIceClient + +if (sys.version_info[0] == 2): + from .ros.listenerPose3d import ListenerPose3d + + + +def __getPoseIceClient(ic, prefix): + ''' + Returns a Pose3D Ice Client. This function should never be used. Use getPose3dClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Pose3D Ice Client + + ''' + print("Receiving " + prefix + " from ICE interfaces") + client = Pose3dIceClient(ic, prefix) + client.start() + return client + +def __getListenerPose(ic, prefix): + ''' + Returns a Pose3D ROS Subscriber. This function should never be used. Use getPose3dClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return Pose3D ROS Subscriber + + ''' + if (sys.version_info[0] == 2): + print("Receiving " + prefix + " from ROS messages") + prop = prop = ic.getProperties() + topic = prop.getPropertyWithDefault(prefix+".Topic",""); + client = ListenerPose3d(topic) + return client + else: + print(prefix + ": ROS msg are diabled for python "+ sys.version_info[0]) + return None + + +def __Posedisabled(ic, prefix): + ''' + Prints a warning that the client is disabled. This function should never be used. Use getPose3dClient instead of this + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + + @type ic: Ice Communicator + @type prefix: String + + @return None + + ''' + print(prefix + " Disabled") + return None + +def getPose3dClient (ic, prefix, node=None): + ''' + Returns a Pose3D Client. + + @param ic: Ice Communicator + @param prefix: prefix name of client in config file + @param node: ROS node + + @type ic: Ice Communicator + @type prefix: String + @type node: ROS node + + @return None if pose3d is disabled + + ''' + prop = prop = ic.getProperties() + server = prop.getPropertyAsIntWithDefault(prefix+".Server",0) + + cons = [__Posedisabled, __getPoseIceClient, __getListenerPose] + + return cons[server](ic, prefix) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/__init__.py b/src/libs/jderobotcomm_py/jderobotComm/ros/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py new file mode 100644 index 000000000..a16ecf7e9 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerCamera.py @@ -0,0 +1,98 @@ +import rospy +from sensor_msgs.msg import Image as ImageROS +import threading +from math import pi as PI +from jderobotTypes import Image +import cv2 +from cv_bridge import CvBridge, CvBridgeError + + + + +def imageMsg2Image(img, bridge): + ''' + Translates from ROS Image to JderobotTypes Image. + + @param img: ROS Image to translate + @param bridge: bridge to do translation + + @type img: sensor_msgs.msg.Image + @type brige: CvBridge + + @return a JderobotTypes.Image translated from img + + ''' + image = Image() + + image.width = img.width + image.height = img.height + image.format = "RGB8" + image.timeStamp = img.header.stamp.secs + (img.header.stamp.nsecs *1e-9) + cv_image = bridge.imgmsg_to_cv2(img, "rgb8") + image.data = cv_image + return image + +class ListenerCamera: + ''' + ROS Camera (Image) Subscriber. Camera Client to Receive Images from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerCamera Constructor. + + @param topic: ROS topic to subscribe + + @type topic: String + + ''' + self.topic = topic + self.data = Image() + self.sub = None + self.lock = threading.Lock() + + self.bridge = CvBridge() + self.start() + + def __callback (self, img): + ''' + Callback function to receive and save Images. + + @param img: ROS Image received + + @type img: sensor_msgs.msg.Image + + ''' + image = imageMsg2Image(img, self.bridge) + + self.lock.acquire() + self.data = image + 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, ImageROS, self.__callback) + + def getImage(self): + ''' + Returns last Image. + + @return last JdeRobotTypes Image saved + + ''' + self.lock.acquire() + image = self.data + self.lock.release() + + return image + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerLaser.py b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerLaser.py new file mode 100644 index 000000000..c687a227c --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerLaser.py @@ -0,0 +1,99 @@ +import rospy +from sensor_msgs.msg import LaserScan +import threading +from math import pi as PI +from jderobotTypes import LaserData + + + +def laserScan2LaserData(scan): + ''' + Translates from ROS LaserScan to JderobotTypes LaserData. + + @param scan: ROS LaserScan to translate + + @type scan: LaserScan + + @return a LaserData translated from scan + + ''' + laser = LaserData() + laser.values = scan.ranges + ''' + ROS Angle Map JdeRobot Angle Map + 0 PI/2 + | | + | | + PI/2 --------- -PI/2 PI --------- 0 + | | + | | + ''' + laser.minAngle = scan.angle_min - PI/2 + laser.maxAngle = scan.angle_max - PI/2 + laser.maxRange = scan.range_max + laser.minRange = scan.range_min + laser.timeStamp = scan.header.stamp.secs + (scan.header.stamp.nsecs *1e-9) + return laser + +class ListenerLaser: + ''' + ROS Laser Subscriber. Laser Client to Receive Laser Scans from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerLaser Constructor. + + @param topic: ROS topic to subscribe + + @type topic: String + + ''' + self.topic = topic + self.data = LaserData() + self.sub = None + self.lock = threading.Lock() + self.start() + + def __callback (self, scan): + ''' + Callback function to receive and save Laser Scans. + + @param scan: ROS LaserScan received + + @type scan: LaserScan + + ''' + laser = laserScan2LaserData(scan) + + self.lock.acquire() + self.data = laser + 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, LaserScan, self.__callback) + + def getLaserData(self): + ''' + Returns last LaserData. + + @return last JdeRobotTypes LaserData saved + + ''' + self.lock.acquire() + laser = self.data + self.lock.release() + + return laser + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/listenerPose3d.py b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerPose3d.py new file mode 100644 index 000000000..89fa69f3f --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ros/listenerPose3d.py @@ -0,0 +1,156 @@ +import rospy +from nav_msgs.msg import Odometry +import threading +from math import asin, atan2, pi +from jderobotTypes import Pose3d + +def quat2Yaw(qw, qx, qy, qz): + ''' + Translates from Quaternion to Yaw. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Yaw value translated from Quaternion + + ''' + rotateZa0=2.0*(qx*qy + qw*qz) + rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz + rotateZ=0.0 + if(rotateZa0 != 0.0 and rotateZa1 != 0.0): + rotateZ=atan2(rotateZa0,rotateZa1) + return rotateZ + +def quat2Pitch(qw, qx, qy, qz): + ''' + Translates from Quaternion to Pitch. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Pitch value translated from Quaternion + + ''' + + rotateYa0=-2.0*(qx*qz - qw*qy) + rotateY=0.0 + if(rotateYa0 >= 1.0): + rotateY = pi/2.0 + elif(rotateYa0 <= -1.0): + rotateY = -pi/2.0 + else: + rotateY = asin(rotateYa0) + + return rotateY + +def quat2Roll (qw, qx, qy, qz): + ''' + Translates from Quaternion to Roll. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Roll value translated from Quaternion + + ''' + rotateXa0=2.0*(qy*qz + qw*qx) + rotateXa1=qw*qw - qx*qx - qy*qy + qz*qz + rotateX=0.0 + + if(rotateXa0 != 0.0 and rotateXa1 != 0.0): + rotateX=atan2(rotateXa0, rotateXa1) + return rotateX + + +def odometry2Pose3D(odom): + ''' + Translates from ROS Odometry to JderobotTypes Pose3d. + + @param odom: ROS Odometry to translate + + @type odom: Odometry + + @return a Pose3d translated from odom + + ''' + pose = Pose3d() + ori = odom.pose.pose.orientation + + pose.x = odom.pose.pose.position.x + pose.y = odom.pose.pose.position.y + pose.z = odom.pose.pose.position.z + #pose.h = odom.pose.pose.position.h + pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) + pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) + pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) + pose.q = [ori.w, ori.x, ori.y, ori.z] + pose.timeStamp = odom.header.stamp.secs + (odom.header.stamp.nsecs *1e-9) + + return pose + + +class ListenerPose3d: + ''' + ROS Pose3D Subscriber. Pose3D Client to Receive pose3d from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerPose3d Constructor. + + @param topic: ROS topic to subscribe + + @type topic: String + + ''' + self.topic = topic + self.data = Pose3d() + self.sub = None + self.lock = threading.Lock() + self.start() + + def __callback (self, odom): + ''' + Callback function to receive and save Pose3d. + + @param odom: ROS Odometry received + + @type odom: Odometry + + ''' + pose = odometry2Pose3D(odom) + + self.lock.acquire() + self.data = pose + 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, Odometry, self.__callback) + + def getPose3d(self): + ''' + Returns last Pose3d. + + @return last JdeRobotTypes Pose3d saved + + ''' + self.lock.acquire() + pose = self.data + self.lock.release() + + return pose + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/publisherMotors.py b/src/libs/jderobotcomm_py/jderobotComm/ros/publisherMotors.py new file mode 100644 index 000000000..345e42f5e --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ros/publisherMotors.py @@ -0,0 +1,177 @@ +import rospy +from geometry_msgs.msg import Twist +import threading +from math import pi as PI +from jderobotTypes import CMDVel +from .threadPublisher import ThreadPublisher + + + +def cmdvel2Twist(vel): + ''' + Translates from JderobotTypes CMDVel to ROS Twist. + + @param vel: JderobotTypes CMDVel to translate + + @type img: JdeRobotTypes.CMDVel + + @return a Twist translated from vel + + ''' + tw = Twist() + tw.linear.x = vel.vx + tw.linear.y = vel.vy + tw.linear.z = vel.vz + tw.angular.x = vel.ax + tw.angular.y = vel.ay + tw.angular.z = vel.az + + return tw + +class PublisherMotors: + ''' + ROS Motors Publisher. Motors Client to Send CMDVel to ROS nodes. + ''' + def __init__(self, topic, maxV, maxW): + ''' + ListenerMotors Constructor. + + @param topic: ROS topic to publish + + @type topic: String + + ''' + self.maxW = maxW + self.maxV = maxV + + self.topic = topic + self.data = CMDVel() + self.pub = self.pub = rospy.Publisher(self.topic, Twist, queue_size=1) + self.lock = threading.Lock() + + self.kill_event = threading.Event() + self.thread = ThreadPublisher(self, self.kill_event) + + self.thread.daemon = True + self.start() + + def publish (self): + ''' + Function to publish cmdvel. + ''' + self.lock.acquire() + tw = cmdvel2Twist(self.data) + self.lock.release() + self.pub.publish(tw) + + def stop(self): + ''' + Stops (Unregisters) the client. If client is stopped you can not start again, Threading.Thread raised error + + ''' + self.kill_event.set() + self.pub.unregister() + + def start (self): + ''' + Starts (Subscribes) the client. If client is stopped you can not start again, Threading.Thread raised error + + ''' + self.kill_event.clear() + self.thread.start() + + + + def getMaxW(self): + return self.maxW + + def getMaxV(self): + return self.maxV + + + def sendVelocities(self, vel): + ''' + Sends CMDVel. + + @param vel: CMDVel to publish + + @type vel: CMDVel + + ''' + self.lock.acquire() + self.data = vel + self.lock.release() + + def sendV(self, v): + ''' + Sends V velocity. uses self.sendVX + + @param v: V velocity + + @type v: float + + ''' + self.sendVX(v) + + def sendL(self, l): + ''' + Sends L velocity. uses self.sendVY + + @param l: L velocity + + @type l: float + + ''' + self.sendVY(l) + + def sendW(self, w): + ''' + Sends W velocity. uses self.sendAZ + + @param w: W velocity + + @type w: float + + ''' + self.sendAZ(w) + + def sendVX(self, vx): + ''' + Sends VX velocity. + + @param vx: VX velocity + + @type vx: float + + ''' + self.lock.acquire() + self.data.vx = vx + self.lock.release() + + def sendVY(self, vy): + ''' + Sends VY velocity. + + @param vy: VY velocity + + @type vy: float + + ''' + self.lock.acquire() + self.data.vy = vy + self.lock.release() + + def sendAZ(self, az): + ''' + Sends AZ velocity. + + @param az: AZ velocity + + @type az: float + + ''' + self.lock.acquire() + self.data.vz = vz + self.lock.release() + + diff --git a/src/libs/jderobotcomm_py/jderobotComm/ros/threadPublisher.py b/src/libs/jderobotcomm_py/jderobotComm/ros/threadPublisher.py new file mode 100644 index 000000000..69aa0ad48 --- /dev/null +++ b/src/libs/jderobotcomm_py/jderobotComm/ros/threadPublisher.py @@ -0,0 +1,46 @@ +# +# 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 : +# Alberto Martin Florido +# Aitor Martinez Fernandez +# +import threading +import time +from datetime import datetime + +time_cycle = 80 + + +class ThreadPublisher(threading.Thread): + + def __init__(self, pub, kill_event): + self.pub = pub + self.kill_event = kill_event + threading.Thread.__init__(self, args=kill_event) + + def run(self): + while (not self.kill_event.is_set()): + start_time = datetime.now() + + self.pub.publish() + + finish_Time = datetime.now() + + dt = finish_Time - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + #print (ms) + if (ms < time_cycle): + time.sleep((time_cycle - ms) / 1000.0) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/test.cfg b/src/libs/jderobotcomm_py/test.cfg new file mode 100644 index 000000000..b14bdd63f --- /dev/null +++ b/src/libs/jderobotcomm_py/test.cfg @@ -0,0 +1,36 @@ + +# 0 -> Deactivate, 1 -> Ice , 2 -> ROS +kobukiViewer.Motors.Server=2 +kobukiViewer.Motors.Proxy=Motors:tcp -h localhost -p 9001 +kobukiViewer.Motors.Topic=/cmd_vel_mux/input/teleop +kobukiViewer.Motors.Name=kobukiViewerMotors + +# 0 -> Deactivate, 1 -> Ice , 2 -> ROS +kobukiViewer.Pose3D.Server=2 +kobukiViewer.Pose3D.Proxy=Pose3D:tcp -h localhost -p 9001 +kobukiViewer.Pose3D.Topic=/odom +kobukiViewer.Pose3D.Name=kobukiViewerPose3d + +# 0 -> Deactivate, 1 -> Ice , 2 -> ROS +kobukiViewer.Camera1.Server=2 +kobukiViewer.Camera1.Proxy=CameraL:tcp -h localhost -p 9001 +kobukiViewer.Camera1.Format=RGB8 +kobukiViewer.Camera1.Topic=/camera/rgb/image_raw +kobukiViewer.Camera1.Name=kobukiViewerCamera1 + +# 0 -> Deactivate, 1 -> Ice , 2 -> ROS +kobukiViewer.Camera2.Server=1 +kobukiViewer.Camera2.Proxy=CameraR:tcp -h localhost -p 9001 +kobukiViewer.Camera2.Format=RGB8 +kobukiViewer.Camera2.Topic=/scan +kobukiViewer.Camera2.Name=kobukiViewerCamera2 + + +# 0 -> Deactivate, 1 -> Ice , 2 -> ROS +kobukiViewer.Laser.Server=2 +kobukiViewer.Laser.Proxy=Laser:tcp -h localhost -p 9001 +kobukiViewer.Laser.Topic=/scan +kobukiViewer.Laser.Name=kobukiViewerLaser + +kobukiViewer.Vmax=3 +kobukiViewer.Wmax=0.7 \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/testCamera.py b/src/libs/jderobotcomm_py/testCamera.py new file mode 100755 index 000000000..75dc99e00 --- /dev/null +++ b/src/libs/jderobotcomm_py/testCamera.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import easyiceconfig as EasyIce +import jderobotComm as comm +import sys +import time +import signal + +from jderobotTypes import LaserData + + + + +if __name__ == '__main__': + + ic = EasyIce.initialize(sys.argv) + ic, node = comm.init(ic) + + client = comm.getCameraClient(ic, "kobukiViewer.Camera1") + + for i in range (10): + #print("client1", end=":") + image = client.getImage() + print(image) + time.sleep(1) + + client.stop() + + + comm.destroy(ic, node) + + + + + + + + + diff --git a/src/libs/jderobotcomm_py/testLaser.py b/src/libs/jderobotcomm_py/testLaser.py new file mode 100755 index 000000000..39d3135e2 --- /dev/null +++ b/src/libs/jderobotcomm_py/testLaser.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +import easyiceconfig as EasyIce +import jderobotComm as comm +import sys +import time +import signal + +from jderobotTypes import LaserData + + + + +if __name__ == '__main__': + + ic = EasyIce.initialize(sys.argv) + ic, node = comm.init(ic) + + client = comm.getLaserClient(ic, "kobukiViewer.Laser") + client2 = comm.getLaserClient(ic, "kobukiViewer.Laser") + + for i in range (10): + #print("client1", end=":") + laser = client.getLaserData() + #print(laser) + time.sleep(1) + + + for i in range (10): + laser = client2.getLaserData() + print(laser) + time.sleep(1) + + comm.destroy(ic, node) + + + + + + + + + diff --git a/src/libs/jderobotcomm_py/testMotors.py b/src/libs/jderobotcomm_py/testMotors.py new file mode 100644 index 000000000..ae5107009 --- /dev/null +++ b/src/libs/jderobotcomm_py/testMotors.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +import easyiceconfig as EasyIce +import jderobotComm as comm +import sys +import time +import signal + +from jderobotTypes import CMDVel + + + + +if __name__ == '__main__': + + ic = EasyIce.initialize(sys.argv) + ic, node = comm.init(ic) + + vel = CMDVel() + vel.vx = 1 + vel.az = 0.1 + + client = comm.getMotorsClient(ic, "kobukiViewer.Motors") + #client2 = comm.getLaserClient(ic, "kobukiViewer.Laser") + for i in range (10): + client.sendVelocities(vel) + time.sleep(1) + + comm.destroy(ic, node) \ No newline at end of file diff --git a/src/libs/jderobotcomm_py/testPose.py b/src/libs/jderobotcomm_py/testPose.py new file mode 100644 index 000000000..eaf4b026c --- /dev/null +++ b/src/libs/jderobotcomm_py/testPose.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import easyiceconfig as EasyIce +import jderobotComm as comm +import sys +import time +import signal + +from jderobotTypes import Pose3d + + + + +if __name__ == '__main__': + + ic = EasyIce.initialize(sys.argv) + ic, node = comm.init(ic) + + client = comm.getPose3dClient(ic, "kobukiViewer.Pose3D") + + for i in range (10): + #print("client1", end=":") + laser = client.getPose3d() + print(laser) + time.sleep(1) + + comm.destroy(ic, node) \ No newline at end of file diff --git a/src/types/CMakeLists.txt b/src/types/CMakeLists.txt index e8d29fbc7..c1a4dfe11 100644 --- a/src/types/CMakeLists.txt +++ b/src/types/CMakeLists.txt @@ -1 +1,2 @@ -add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/cpp) \ No newline at end of file +add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/cpp) +add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/python) \ No newline at end of file diff --git a/src/types/python/CMakeLists.txt b/src/types/python/CMakeLists.txt new file mode 100644 index 000000000..e92912ad5 --- /dev/null +++ b/src/types/python/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8) + +configure_module_python(jderobotTypes) + +add_custom_target(jderobotTypes_py ALL) +copy_to_binary_python(jderobotTypes_py jderobotTypes) + +install_python(jderobotTypes core) diff --git a/src/types/python/jderobotTypes/__init__.py b/src/types/python/jderobotTypes/__init__.py new file mode 100644 index 000000000..e43b256bd --- /dev/null +++ b/src/types/python/jderobotTypes/__init__.py @@ -0,0 +1,4 @@ +from .laserData import LaserData +from .image import Image +from .pose3d import Pose3d +from .cmdvel import CMDVel \ No newline at end of file diff --git a/src/types/python/jderobotTypes/cmdvel.py b/src/types/python/jderobotTypes/cmdvel.py new file mode 100644 index 000000000..8a3c054f2 --- /dev/null +++ b/src/types/python/jderobotTypes/cmdvel.py @@ -0,0 +1,38 @@ +# +# 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 CMDVel (): + + def __init__(self): + + self.vx = 0 # vel in x[m/s] (use this for V in wheeled robots) + self.vy = 0 # vel in y[m/s] + self.vz = 0 # vel in z[m/s] + self.ax = 0 # angular vel in X axis [rad/s] + self.ay = 0 # angular vel in X axis [rad/s] + self.az = 0 # angular vel in Z axis [rad/s] (use this for W in wheeled robots) + self.timeStamp = 0 # Time stamp [s] + + + def __str__(self): + s = "CMDVel: {\n vx: " + str(self.vx) + "\n vy: " + str(self.vy) + s = s + "\n vz: " + str(self.vz) + "\n ax: " + str(self.ax) + s = s + "\n ay: " + str(self.ay) + "\n az: " + str(self.az) + s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" + return s \ No newline at end of file diff --git a/src/types/python/jderobotTypes/image.py b/src/types/python/jderobotTypes/image.py new file mode 100644 index 000000000..28cfebaf7 --- /dev/null +++ b/src/types/python/jderobotTypes/image.py @@ -0,0 +1,40 @@ +# +# 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 numpy as np + + +class Image: + + def __init__(self): + + self.height = 3 # Image height [pixels] + self.width = 3 # Image width [pixels] + self.timeStamp = 0 # Time stamp [s] */ + self.format = "" # Image format string (RGB8, BGR,...) + self.data = np.zeros((self.height, self.width, 3), np.uint8) # The image data itself + self.data.shape = self.height, self.width, 3 + + + def __str__(self): + s = "Image: {\n height: " + str(self.height) + "\n width: " + str(self.width) + s = s + "\n format: " + self.format + "\n timeStamp: " + str(self.timeStamp) + s = s + "\n data: " + str(self.data) + "\n}" + return s + diff --git a/src/types/python/jderobotTypes/laserData.py b/src/types/python/jderobotTypes/laserData.py new file mode 100644 index 000000000..ea173f513 --- /dev/null +++ b/src/types/python/jderobotTypes/laserData.py @@ -0,0 +1,38 @@ +# +# 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 LaserData (): + + def __init__(self): + + self.values = [] # meters + self.minAngle = 0 # Angle of first value (rads) + self.maxAngle = 0 # Angle of last value (rads) + self.minRange = 0 # Max Range posible (meters) + self.maxRange = 0 #Min Range posible (meters) + self.timeStamp = 0 # seconds + + + def __str__(self): + s = "LaserData: {\n minAngle: " + str(self.minAngle) + "\n maxAngle: " + str(self.maxAngle) + s = s + "\n minRange: " + str(self.minRange) + "\n maxRange: " + str(self.maxRange) + s = s + "\n timeStamp: " + str(self.timeStamp) + "\n values: " + str(self.values) + "\n}" + return s + diff --git a/src/types/python/jderobotTypes/pose3d.py b/src/types/python/jderobotTypes/pose3d.py new file mode 100644 index 000000000..75b3b0723 --- /dev/null +++ b/src/types/python/jderobotTypes/pose3d.py @@ -0,0 +1,40 @@ +# +# 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 Pose3d (): + + def __init__(self): + + self.x = 0 # X coord [meters] + self.y = 0 # Y coord [meters] + self.z = 0 # Z coord [meters] + self.h = 1 # H param + self.yaw = 0 #Yaw angle[rads] + self.pitch = 0 # Pitch angle[rads] + self.roll = 0 # Roll angle[rads] + self.q = [0,0,0,0] # Quaternion + self.timeStamp = 0 # Time stamp [s] + + + def __str__(self): + s = "Pose3D: {\n x: " + str(self.x) + "\n Y: " + str(self.y) + s = s + "\n Z: " + str(self.z) + "\n H: " + str(self.h) + s = s + "\n Yaw: " + str(self.yaw) + "\n Pitch: " + str(self.pitch) + "\n Roll: " + str(self.roll) + s = s + "\n quaternion: " + str(self.q) + "\n timeStamp: " + str(self.timeStamp) + "\n}" + return s \ No newline at end of file